手撸golang 基本数据结构与算法 图的最短路径 A*(A-Star)算法
缘起 最近阅读<<我的第一本算法书>>(【日】石田保辉；宫崎修一) 本系列笔记拟采用golang练习之 A*(A-Star)算法 A*（A-Star）算法也是一种在图中求解最短路径问题的算法， 由狄克斯特拉算法发展而来。 A*算法不仅会考虑从起点到候补顶点的距离， 还会考虑从当前所在顶点到终点的估算距离。 距离估算值越接近当前顶点到终点的实际值， A*算法的搜索效率也就越高. 当距离估算值小于实际距离时， 是一定可以得到正确答案的. A*算法在游戏编程中经常被用于计算敌人追赶玩家时的行动路线等. 摘自 <<我的第一本算法书>> 【日】石田保辉；宫崎修一 场景 如下图, 某游戏中, 地图是网格状的, 我方在S点, 敌人在G点, 空白区域是湖泊/树林等不可到达区域: 现在需要追击敌人, 因此需要计算S点到G点的最短行进路线. A*算法是以狄克斯特拉算法为基础, 区别是在计算候选节点的权重时, 需要同时考虑测量权重和估算权重. 此场景中, 使用S点到G点坐标的直线距离作为估算权重. 估算权重的作用就像牵引风筝的绳子, 使得每次选取的候选节点, 尽量是靠往终点方向. 流程 给定若干顶点, 以及顶点间的若干条边, 寻找从指定起点srcNode到指定终点dstNode的最小权重路径 设定srcNode的权重为0, 其他顶点的权重为无穷大 计算所有节点到dstNode节点的估算距离, 以x,y坐标的直线距离作为估算值 节点.总权重 = 节点.测量权重 + 节点.估算权重 将srcNode节点送入候选堆 for 候选堆不为空: 从候选堆pop顶点node, node是总权重最小的候选节点 如果node.id == dstNode.id, 循环结束 遍历从node出发的所有边, 将边的终点to的测量权重, 更新为min(to.测量权重, node.测量权重+边.权重) 如果to.测量权重 > node.测量权重+边.权重, 说明更新有效 如果更新有效, 判断to是否在堆中, 如果是, 则上浮以维护堆秩序, 否则, 将to节点push入候选堆 判断dstNode的测量权重是否被更新(!=无穷大), 如果是则说明存在最短路径 反向查找最短路径: 设定当前节点current = 终点 push节点current进路径队列 遍历终点为current的边, 查找符合条件的node:边的起点.测量权重 = current.测量权重-边.权重 push节点node进路径队列 循环1-4, 直到current == srcNode, 查找完成 设计 INode: 顶点接口, 支持xy坐标和估算权重 ILine: 边接口 IPathFinder: 最短路径查找算法接口 IComparator: 顶点比较接口 IHeap: 顶点堆接口 tNode: 顶点, 实现INode tLine: 边, 实现ILine tNodeWeightComparator: 基于权重的顶点比较器, 实现IComparator接口 tArrayHeap: 堆的实现 tAStarPathFinder: A*算法的实现, 使用xy坐标的直线距离作为估算权重 单元测试 a_star_finder_test.go package graph import ( "fmt" "strings" "testing" ) import astar "learning/gooop/graph/a_star" func Test_AStarFinder(t *testing.T) { fnAssertTrue := func(b bool, msg string) { if !b { t.Fatal(msg) } } // 设定顶点 nodes := []astar.INode { astar.NewNode("11", 1, 1), astar.NewNode("21", 2, 1), astar.NewNode("31", 3, 1), astar.NewNode("12", 1, 2), astar.NewNode("32", 3, 2), astar.NewNode("13", 1, 3), astar.NewNode("33", 3, 3), astar.NewNode("43", 4, 3), astar.NewNode("53", 5, 3), astar.NewNode("63", 6, 3), astar.NewNode("73", 7, 3), astar.NewNode("14", 1, 4), astar.NewNode("34", 3, 4), astar.NewNode("74", 7, 4), astar.NewNode("15", 1, 5), astar.NewNode("35", 3, 5), astar.NewNode("55", 5, 5), astar.NewNode("65", 6, 5), astar.NewNode("75", 7, 5), astar.NewNode("16", 1, 6), astar.NewNode("36", 3, 6), astar.NewNode("56", 5, 6), astar.NewNode("17", 1, 7), astar.NewNode("27", 2, 7), astar.NewNode("37", 3, 7), astar.NewNode("47", 4, 7), astar.NewNode("57", 5, 7), astar.NewNode("67", 6, 7), astar.NewNode("77", 7, 7), } // 为相邻点创建边 var lines []astar.ILine mapNodes := make(map[string]astar.INode, len(nodes)) for _,it := range nodes { k := fmt.Sprintf("%v,%v", it.GetX(), it.GetY()) mapNodes[k] = it } for _,it := range nodes { if up,ok := mapNodes[fmt.Sprintf("%v,%v", it.GetX(), it.GetY() - 1)];ok { lines = append(lines, astar.NewLine(it.ID(), up.ID(), 1), astar.NewLine(up.ID(), it.ID(), 1)) } if down,ok := mapNodes[fmt.Sprintf("%v,%v", it.GetX(), it.GetY() + 1)];ok { lines = append(lines, astar.NewLine(it.ID(), down.ID(), 1), astar.NewLine(down.ID(), it.ID(), 1)) } if left,ok := mapNodes[fmt.Sprintf("%v,%v", it.GetX()-1, it.GetY())];ok { lines = append(lines, astar.NewLine(it.ID(), left.ID(), 1), astar.NewLine(left.ID(), it.ID(), 1)) } if right,ok := mapNodes[fmt.Sprintf("%v,%v", it.GetX()+1, it.GetY())];ok { lines = append(lines, astar.NewLine(it.ID(), right.ID(), 1), astar.NewLine(right.ID(), it.ID(), 1)) } } // a*算法 查找最短路径 ok,path := astar.AStarPathFinder.FindPath(nodes, lines, "33", "77") if !ok { t.Fatal("failed to find min path") } fnPathToString := func(nodes []astar.INode) string { items := make([]string, len(nodes)) for i,it := range nodes { items[i] = fmt.Sprintf("%s", it) } return strings.Join(items, " ") } pathString := fnPathToString(path) t.Log(pathString) fnAssertTrue(pathString == "33(0+6) 34(1+5) 35(2+4) 36(3+4) 37(4+4) 47(5+3) 57(6+2) 67(7+1) 77(8+0)", "incorrect path") } 测试输出 $ go test -v a_star_finder_test.go === RUN Test_AStarFinder a_star_finder_test.go:96: 33(0+6) 34(1+5) 35(2+4) 36(3+4) 37(4+4) 47(5+3) 57(6+2) 67(7+1) 77(8+0) --- PASS: Test_AStarFinder (0.00s) PASS ok command-line-arguments 0.002s INode.go 顶点接口, 支持xy坐标和估算权重 package a_star type INode interface { ID() string GetX() int GetY() int SetX(int) SetY(int) SetEstimatedWeight(int) SetMeasuredWeight(int) GetMeasuredWeight() int GetTotalWeight() int } const MaxWeight = int(0x7fffffff_00000000) ILine.go 边接口 package a_star type ILine interface { From() string To() string Weight() int } IPathFinder.go 最短路径查找算法接口 package a_star type IPathFinder interface { FindPath(nodes []INode, lines []ILine, from string, to string) (bool,[]INode) } IComparator.go 顶点比较接口 package a_star type IComparator interface { Less(a interface{}, b interface{}) bool } IHeap.go 顶点堆接口 package a_star type IHeap interface { Size() int IsEmpty() bool IsNotEmpty() bool Push(node interface{}) Pop() (bool, interface{}) IndexOf(node interface{}) int ShiftUp(i int) } tNode.go 顶点, 实现INode package a_star import "fmt" type tNode struct { id string x int y int measuredWeight int estimatedWeight int } func NewNode(id string, x int, y int) INode { return &tNode{ id,x, y, MaxWeight,0, } } func (me *tNode) ID() string { return me.id } func (me *tNode) GetX() int { return me.x } func (me *tNode) GetY() int { return me.y } func (me *tNode) SetX(x int) { me.x = x } func (me *tNode) SetY(y int) { me.y = y } func (me *tNode) SetEstimatedWeight(w int) { me.estimatedWeight = w } func (me *tNode) SetMeasuredWeight(w int) { me.measuredWeight = w } func (me *tNode) GetMeasuredWeight() int { return me.measuredWeight } func (me *tNode) GetTotalWeight() int { return me.estimatedWeight + me.measuredWeight } func (me *tNode) String() string { return fmt.Sprintf("%s(%v+%v)", me.id, me.measuredWeight, me.estimatedWeight) } tLine.go 边, 实现ILine package a_star type tLine struct { from string to string weight int } func NewLine(from string, to string, weight int) ILine { return &tLine{ from,to,weight, } } func (me *tLine) From() string { return me.from } func (me *tLine) To() string { return me.to } func (me *tLine) Weight() int { return me.weight } tNodeWeightComparator.go 基于权重的顶点比较器, 实现IComparator接口 package a_star import "errors" type tNodeWeightComparator struct { } func newNodeWeightComparator() IComparator { return &tNodeWeightComparator{ } } func (me *tNodeWeightComparator) Less(a interface{}, b interface{}) bool { if a == nil || b == nil { panic(gNullArgumentError) } n1 := a.(INode) n2 := b.(INode) return n1.GetTotalWeight() <= n2.GetTotalWeight() } var gNullArgumentError = errors.New("null argument error") tArrayHeap.go 堆的实现 package a_star import ( "errors" "fmt" "strings" ) type tArrayHeap struct { comparator IComparator items []interface{} size int version int64 } func newArrayHeap(comparator IComparator) IHeap { return &tArrayHeap{ comparator: comparator, items: make([]interface{}, 0), size: 0, version: 0, } } func (me *tArrayHeap) Size() int { return me.size } func (me *tArrayHeap) IsEmpty() bool { return me.size <= 0 } func (me *tArrayHeap) IsNotEmpty() bool { return !me.IsEmpty() } func (me *tArrayHeap) Push(value interface{}) { me.version++ me.ensureSize(me.size + 1) me.items[me.size] = value me.size++ me.ShiftUp(me.size - 1) me.version++ } func (me *tArrayHeap) ensureSize(size int) { for ;len(me.items) < size; { me.items = append(me.items, nil) } } func (me *tArrayHeap) parentOf(i int) int { return (i - 1) / 2 } func (me *tArrayHeap) leftChildOf(i int) int { return i*2 + 1 } func (me *tArrayHeap) rightChildOf(i int) int { return me.leftChildOf(i) + 1 } func (me *tArrayHeap) last() (i int, v interface{}) { if me.IsEmpty() { return -1, nil } i = me.size - 1 v = me.items[i] return i,v } func (me *tArrayHeap) IndexOf(node interface{}) int { n := -1 for i,it := range me.items { if it == node { n = i break } } return n } func (me *tArrayHeap) ShiftUp(i int) { if i <= 0 { return } v := me.items[i] pi := me.parentOf(i) pv := me.items[pi] if me.comparator.Less(v, pv) { me.items[pi], me.items[i] = v, pv me.ShiftUp(pi) } } func (me *tArrayHeap) Pop() (bool, interface{}) { if me.IsEmpty() { return false, nil } me.version++ top := me.items[0] li, lv := me.last() me.items[0] = nil me.size-- if me.IsEmpty() { return true, top } me.items[0] = lv me.items[li] = nil me.shiftDown(0) me.version++ return true, top } func (me *tArrayHeap) shiftDown(i int) { pv := me.items[i] ok, ci, cv := me.minChildOf(i) if ok && me.comparator.Less(cv, pv) { me.items[i], me.items[ci] = cv, pv me.shiftDown(ci) } } func (me *tArrayHeap) minChildOf(p int) (ok bool, i int, v interface{}) { li := me.leftChildOf(p) if li >= me.size { return false, 0, nil } lv := me.items[li] ri := me.rightChildOf(p) if ri >= me.size { return true, li, lv } rv := me.items[ri] if me.comparator.Less(lv, rv) { return true, li, lv } else { return true, ri, rv } } func (me *tArrayHeap) String() string { level := 0 lines := make([]string, 0) lines = append(lines, "") for { n := 1<<level min := n - 1 max := n + min - 1 if min >= me.size { break } line := make([]string, 0) for i := min;i <= max;i++ { if i >= me.size { break } line = append(line, fmt.Sprintf("%4d", me.items[i])) } lines = append(lines, strings.Join(line, ",")) level++ } return strings.Join(lines, "\n") } var gNoMoreElementsError = errors.New("no more elements") tAStarPathFinder.go A*算法的实现, 使用xy坐标的直线距离作为估算权重 package a_star import "math" type tAStarPathFinder struct { } func newAStarPathFinder() IPathFinder { return &tAStarPathFinder{} } func (me *tAStarPathFinder) FindPath(nodes []INode, lines []ILine, srcID string, dstID string) (bool,[]INode) { // 节点索引 mapNodes := make(map[string]INode, 0) for _,it := range nodes { mapNodes[it.ID()] = it } srcNode, ok := mapNodes[srcID] if !ok { return false, nil } dstNode,ok := mapNodes[dstID] if !ok { return false, nil } // 边的索引 mapFromLines := make(map[string][]ILine, 0) mapToLines := make(map[string][]ILine, 0) for _, it := range lines { if v,ok := mapFromLines[it.From()];ok { mapFromLines[it.From()] = append(v, it) } else { mapFromLines[it.From()] = []ILine{ it } } if v,ok := mapToLines[it.To()];ok { mapToLines[it.To()] = append(v, it) } else { mapToLines[it.To()] = []ILine{ it } } } for _,it := range nodes { // 设置src节点的weight为0, 其他节点的weight为MaxWeight if it.ID() == srcID { it.SetMeasuredWeight(0) } else { it.SetMeasuredWeight(MaxWeight) } // 计算每个节点到dst节点的估算距离 if it.ID() == dstID { it.SetEstimatedWeight(0) } else { it.SetEstimatedWeight(me.distance(it.GetX(), it.GetY(), dstNode.GetY(), dstNode.GetY())) } } // 将起点push到堆 heap := newArrayHeap(newNodeWeightComparator()) heap.Push(srcNode) // 遍历候选节点 for heap.IsNotEmpty() { _, top := heap.Pop() from := top.(INode) if from.ID() == dstID { break } links, ok := mapFromLines[from.ID()] if ok { for _,line := range links { if to,ok := mapNodes[line.To()];ok { if me.updateMeasuredWeight(from, to, line) { n := heap.IndexOf(to) if n >= 0 { heap.ShiftUp(n) } else { heap.Push(to) } } } } } } // 逆向查找最短路径 if dstNode.GetMeasuredWeight() >= MaxWeight { return false, nil } path := []INode{ dstNode } current := dstNode maxRound := len(lines) for ;current != srcNode && maxRound > 0;maxRound-- { linkedLines, _ := mapToLines[current.ID()] for _,line := range linkedLines { from, _ := mapNodes[line.From()] if from.GetMeasuredWeight() == current.GetMeasuredWeight() - line.Weight() { current = from path = append(path, from) } } } if current != srcNode { return false, nil } me.reverse(path) return true, path } func (me *tAStarPathFinder) distance(x0, y0, x1, y1 int) int { dx := x0 - x1 dy := y0 - y1 return int(math.Round(math.Sqrt(float64(dx * dx + dy * dy)))) } func (me *tAStarPathFinder) reverse(nodes []INode) { for i,j := 0, len(nodes)-1;i < j;i,j=i+1,j-1 { nodes[i], nodes[j] = nodes[j], nodes[i] } } func (me *tAStarPathFinder) updateMeasuredWeight(from INode, to INode, line ILine) bool { w := me.min(from.GetMeasuredWeight() + line.Weight(), to.GetMeasuredWeight()) if to.GetMeasuredWeight() > w { to.SetMeasuredWeight(w) return true } return false } func (me *tAStarPathFinder) min(a, b int) int { if a <= b { return a } return b } var AStarPathFinder = newAStarPathFinder() (end)
