golang A* 导航算法

简介

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

package mainimport ("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 []*Nodefunc (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 := *an := 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 = tentativeGneighbor.h = getDistance(neighbor, end)neighbor.f = neighbor.g + neighbor.hneighbor.parent = currentNodeheap.Push(&openList, neighbor)} else if tentativeG < neighbor.g { // 如果当前路径到该节点更优，则更新节点信息neighbor.g = tentativeGneighbor.f = neighbor.g + neighbor.hneighbor.parent = currentNodeheap.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 + iy := 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.xdy := a.y - b.yreturn 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)
}

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！