golang A* 导航算法

简介

使用 A* 导航算法的例子，用于在二维地图中寻找从起点到终点的最优路径，直接上代码：

package main

import (
	"container/heap"
	"fmt"
	"math"
)

// 定义一个 Node 结构体，表示一个节点
type Node struct {
	x, y      int     // 节点的坐标
	g, h, f   float64 // 节点的 g、h、f 值
	openIndex int     // 节点在开放列表中的索引
	closed    bool    // 节点是否在关闭列表中
	parent    *Node   // 节点的父节点
}

// 定义一个 ByF 类型，用于实现 Node 的优先队列
type ByF []*Node

func (a ByF) Len() int           { return len(a) }
func (a ByF) Swap(i, j int)      { a[i], a[j] = a[j], a[i] }
func (a ByF) Less(i, j int) bool { return a[i].f < a[j].f }
func (a *ByF) Push(x interface{}) {
	*a = append(*a, x.(*Node))
	x.(*Node).openIndex = len(*a) - 1
}
func (a *ByF) Pop() interface{} {
	old := *a
	n := len(old)
	x := old[n-1]
	x.openIndex = -1
	*a = old[0 : n-1]
	return x
}

// A* 导航算法的实现
func aStar(start *Node, end *Node, mapData [][]int) {
	rows := len(mapData)
	cols := len(mapData[0])

	openList := make(ByF, 0)
	heap.Init(&openList)

	// 将起点加入开放列表
	heap.Push(&openList, start)

	for openList.Len() > 0 {
		// 从开放列表中取出 f 值最小的节点
		currentNode := heap.Pop(&openList).(*Node)

		// 如果当前节点是终点，则找到路径，退出循环
		if currentNode.x == end.x && currentNode.y == end.y {
			fmt.Println("找到路径")
			path := []Node{}
			for currentNode != nil {
				path = append(path, *currentNode)
				currentNode = currentNode.parent
			}
			for i := len(path) - 1; i >= 0; i-- {
				fmt.Printf("(%d, %d) ", path[i].x, path[i].y)
			}
			fmt.Println()
			return
		}

		// 将当前节点加入关闭列表
		currentNode.closed = true

		// 获取当前节点的相邻节点
		neighbors := getNeighbors(currentNode, rows, cols, mapData)

		// 处理每个相邻节点
		for _, neighbor := range neighbors {
			// 如果相邻节点已经在关闭列表中，则跳过
			if neighbor.closed {
				continue
			}

			// 计算当前节点到相邻节点的移
			tentativeG := currentNode.g + getDistance(currentNode, neighbor)

			// 如果相邻节点不在开放列表中，则加入开放列表
			if neighbor.openIndex == -1 {
				neighbor.g = tentativeG
				neighbor.h = getDistance(neighbor, end)
				neighbor.f = neighbor.g + neighbor.h
				neighbor.parent = currentNode
				heap.Push(&openList, neighbor)
			} else if tentativeG < neighbor.g { // 如果当前路径到该节点更优，则更新节点信息
				neighbor.g = tentativeG
				neighbor.f = neighbor.g + neighbor.h
				neighbor.parent = currentNode
				heap.Fix(&openList, neighbor.openIndex)
			}
		}
	}

	// 如果找不到路径，则输出提示信息
	fmt.Println("找不到路径")
}

// 获取某个节点的相邻节点
func getNeighbors(node *Node, rows int, cols int, mapData [][]int) []*Node {
	result := make([]*Node, 0)
	for i := -1; i <= 1; i++ {
		for j := -1; j <= 1; j++ {
			// 如果 i 和 j 都为 0，则是当前节点，跳过
			if i == 0 && j == 0 {
				continue
			}

			x := node.x + i
			y := node.y + j

			// 如果超出地图边界，则跳过
			if x < 0 || x >= rows || y < 0 || y >= cols {
				continue
			}

			// 如果是障碍物，则跳过
			if mapData[x][y] == 1 {
				continue
			}

			// 添加相邻节点
			result = append(result, &Node{x, y, 0, 0, 0, -1, false, nil})
		}
	}

	return result
}

// 计算两个节点之间的欧几里得距离
func getDistance(a *Node, b *Node) float64 {
	dx := a.x - b.x
	dy := a.y - b.y
	return math.Sqrt(float64(dx*dx + dy*dy))
}

func main() {
	// 定义地图数据
	mapData := [][]int{
		{0, 0, 0, 0, 0, 0, 0},
		{0, 0, 0, 0, 1, 1, 0},
		{0, 0, 1, 1, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0},
		{0, 1, 1, 0, 0, 0, 0},
		{0, 0, 0, 0, 1, 1, 0},
		{0, 0, 0, 0, 0, 0, 0},
	}
	// 定义起点和终点
	start := &Node{0, 0, 0, 0, 0, -1, false, nil}
	end := &Node{6, 6, 0, 0, 0, -1, false, nil}

	// 使用 A* 导航算法寻找最短路径
	aStar(start, end, mapData)
}