缘起

最近阅读<<我的第一本算法书>>(【日】石田保辉；宫崎修一)
本系列笔记拟采用golang练习之

A*(A-Star)算法

  1. A*（A-Star）算法也是一种在图中求解最短路径问题的算法，
  2. 由狄克斯特拉算法发展而来。
  4. A*算法不仅会考虑从起点到候补顶点的距离，
  5. 还会考虑从当前所在顶点到终点的估算距离。
  7. 距离估算值越接近当前顶点到终点的实际值，
  8. A*算法的搜索效率也就越高.
  9. 当距离估算值小于实际距离时，
  10. 是一定可以得到正确答案的.
  12. A*算法在游戏编程中经常被用于计算敌人追赶玩家时的行动路线等.
  14. 摘自 <<我的第一本算法书>> 【日】石田保辉；宫崎修一

场景

如下图, 某游戏中, 地图是网格状的, 我方在S点, 敌人在G点, 空白区域是湖泊/树林等不可到达区域:
image.png
现在需要追击敌人, 因此需要计算S点到G点的最短行进路线.
A*算法是以狄克斯特拉算法为基础, 区别是在计算候选节点的权重时, 需要同时考虑测量权重和估算权重.
此场景中, 使用S点到G点坐标的直线距离作为估算权重.
估算权重的作用就像牵引风筝的绳子, 使得每次选取的候选节点, 尽量是靠往终点方向.
image.png

流程

  1. 给定若干顶点, 以及顶点间的若干条边, 寻找从指定起点srcNode到指定终点dstNode的最小权重路径
  2. 设定srcNode的权重为0, 其他顶点的权重为无穷大
  3. 计算所有节点到dstNode节点的估算距离, 以x,y坐标的直线距离作为估算值
  4. 节点.总权重 = 节点.测量权重 + 节点.估算权重
  5. 将srcNode节点送入候选堆
  6. for 候选堆不为空:
    1. 从候选堆pop顶点node, node是总权重最小的候选节点
    2. 如果node.id == dstNode.id, 循环结束
    3. 遍历从node出发的所有边, 将边的终点to的测量权重, 更新为min(to.测量权重, node.测量权重+边.权重)
    4. 如果to.测量权重 > node.测量权重+边.权重, 说明更新有效
    5. 如果更新有效, 判断to是否在堆中, 如果是, 则上浮以维护堆秩序, 否则, 将to节点push入候选堆
  7. 判断dstNode的测量权重是否被更新(!=无穷大), 如果是则说明存在最短路径
  8. 反向查找最短路径:
    1. 设定当前节点current = 终点
    2. push节点current进路径队列
    3. 遍历终点为current的边, 查找符合条件的node:边的起点.测量权重 = current.测量权重-边.权重
    4. push节点node进路径队列
    5. 循环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

  1. package graph
  3. import (
  4.     "fmt"
  5.     "strings"
  6.     "testing"
  7. )
  8. import astar "learning/gooop/graph/a_star"
  10. func Test_AStarFinder(t *testing.T) {
  11.     fnAssertTrue := func(b bool, msg string) {
  12.         if !b {
  13.             t.Fatal(msg)
  14.         }
  15.     }
  17.     // 设定顶点
  18.     nodes := []astar.INode {
  19.         astar.NewNode("11", 1, 1),
  20.         astar.NewNode("21", 2, 1),
  21.         astar.NewNode("31", 3, 1),
  23.         astar.NewNode("12", 1, 2),
  24.         astar.NewNode("32", 3, 2),
  26.         astar.NewNode("13", 1, 3),
  27.         astar.NewNode("33", 3, 3),
  28.         astar.NewNode("43", 4, 3),
  29.         astar.NewNode("53", 5, 3),
  30.         astar.NewNode("63", 6, 3),
  31.         astar.NewNode("73", 7, 3),
  33.         astar.NewNode("14", 1, 4),
  34.         astar.NewNode("34", 3, 4),
  35.         astar.NewNode("74", 7, 4),
  37.         astar.NewNode("15", 1, 5),
  38.         astar.NewNode("35", 3, 5),
  39.         astar.NewNode("55", 5, 5),
  40.         astar.NewNode("65", 6, 5),
  41.         astar.NewNode("75", 7, 5),
  43.         astar.NewNode("16", 1, 6),
  44.         astar.NewNode("36", 3, 6),
  45.         astar.NewNode("56", 5, 6),
  47.         astar.NewNode("17", 1, 7),
  48.         astar.NewNode("27", 2, 7),
  49.         astar.NewNode("37", 3, 7),
  50.         astar.NewNode("47", 4, 7),
  51.         astar.NewNode("57", 5, 7),
  52.         astar.NewNode("67", 6, 7),
  53.         astar.NewNode("77", 7, 7),
  54.     }
  56.     // 为相邻点创建边
  57.     var lines []astar.ILine
  58.     mapNodes := make(map[string]astar.INode, len(nodes))
  59.     for _,it := range nodes {
  60.         k := fmt.Sprintf("%v,%v", it.GetX(), it.GetY())
  61.         mapNodes[k] = it
  62.     }
  64.     for _,it := range nodes {
  65.         if up,ok := mapNodes[fmt.Sprintf("%v,%v", it.GetX(), it.GetY() - 1)];ok {
  66.             lines = append(lines, astar.NewLine(it.ID(), up.ID(), 1), astar.NewLine(up.ID(), it.ID(), 1))
  67.         }
  69.         if down,ok := mapNodes[fmt.Sprintf("%v,%v", it.GetX(), it.GetY() + 1)];ok {
  70.             lines = append(lines, astar.NewLine(it.ID(), down.ID(), 1), astar.NewLine(down.ID(), it.ID(), 1))
  71.         }
  73.         if left,ok := mapNodes[fmt.Sprintf("%v,%v", it.GetX()-1, it.GetY())];ok {
  74.             lines = append(lines, astar.NewLine(it.ID(), left.ID(), 1), astar.NewLine(left.ID(), it.ID(), 1))
  75.         }
  77.         if right,ok := mapNodes[fmt.Sprintf("%v,%v", it.GetX()+1, it.GetY())];ok {
  78.             lines = append(lines, astar.NewLine(it.ID(), right.ID(), 1), astar.NewLine(right.ID(), it.ID(), 1))
  79.         }
  80.     }
  82.     // a*算法 查找最短路径
  83.     ok,path := astar.AStarPathFinder.FindPath(nodes, lines, "33", "77")
  84.     if !ok {
  85.         t.Fatal("failed to find min path")
  86.     }
  88.     fnPathToString := func(nodes []astar.INode) string {
  89.         items := make([]string, len(nodes))
  90.         for i,it := range nodes {
  91.             items[i] = fmt.Sprintf("%s", it)
  92.         }
  93.         return strings.Join(items, " ")
  94.     }
  95.     pathString := fnPathToString(path)
  96.     t.Log(pathString)
  97.     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")
  98. }

测试输出

  1. $ go test -v a_star_finder_test.go 
  2. === RUN   Test_AStarFinder
  3.     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)
  4. --- PASS: Test_AStarFinder (0.00s)
  5. PASS
  6. ok      command-line-arguments  0.002s

INode.go

顶点接口, 支持xy坐标和估算权重

  1. package a_star
  3. type INode interface {
  4.     ID() string
  6.     GetX() int
  7.     GetY() int
  8.     SetX(int)
  9.     SetY(int)
  11.     SetEstimatedWeight(int)
  12.     SetMeasuredWeight(int)
  13.     GetMeasuredWeight() int
  14.     GetTotalWeight() int
  15. }
  17. const MaxWeight = int(0x7fffffff_00000000)

ILine.go

边接口

  1. package a_star
  3. type ILine interface {
  4.     From() string
  5.     To() string
  6.     Weight() int
  7. }

IPathFinder.go

最短路径查找算法接口

  1. package a_star
  3. type IPathFinder interface {
  4.     FindPath(nodes []INode, lines []ILine, from string, to string) (bool,[]INode)
  5. }

IComparator.go

顶点比较接口

  1. package a_star
  3. type IComparator interface {
  4.     Less(a interface{}, b interface{}) bool
  5. }

IHeap.go

顶点堆接口

  1. package a_star
  3. type IHeap interface {
  4.     Size() int
  5.     IsEmpty() bool
  6.     IsNotEmpty() bool
  8.     Push(node interface{})
  9.     Pop() (bool, interface{})
  11.     IndexOf(node interface{}) int
  12.     ShiftUp(i int)
  13. }

tNode.go

顶点, 实现INode

  1. package a_star
  3. import "fmt"
  5. type tNode struct {
  6.     id string
  7.     x int
  8.     y int
  9.     measuredWeight int
  10.     estimatedWeight int
  11. }
  13. func NewNode(id string, x int, y int) INode {
  14.     return &tNode{
  15.         id,x, y, MaxWeight,0,
  16.     }
  17. }
  19. func (me *tNode) ID() string {
  20.     return me.id
  21. }
  24. func (me *tNode) GetX() int {
  25.     return me.x
  26. }
  28. func (me *tNode) GetY() int {
  29.     return me.y
  30. }
  32. func (me *tNode) SetX(x int) {
  33.     me.x = x
  34. }
  36. func (me *tNode) SetY(y int) {
  37.     me.y = y
  38. }
  40. func (me *tNode) SetEstimatedWeight(w int) {
  41.     me.estimatedWeight = w
  42. }
  44. func (me *tNode) SetMeasuredWeight(w int) {
  45.     me.measuredWeight = w
  46. }
  48. func (me *tNode) GetMeasuredWeight() int {
  49.     return me.measuredWeight
  50. }
  52. func (me *tNode) GetTotalWeight() int {
  53.     return me.estimatedWeight + me.measuredWeight
  54. }
  56. func (me *tNode) String() string {
  57.     return fmt.Sprintf("%s(%v+%v)", me.id, me.measuredWeight, me.estimatedWeight)
  58. }

tLine.go

边, 实现ILine

  1. package a_star
  3. type tLine struct {
  4.     from string
  5.     to string
  6.     weight int
  7. }
  9. func NewLine(from string, to string, weight int) ILine {
  10.     return &tLine{
  11.         from,to,weight,
  12.     }
  13. }
  15. func (me *tLine) From() string {
  16.     return me.from
  17. }
  19. func (me *tLine) To() string {
  20.     return me.to
  21. }
  23. func (me *tLine) Weight() int {
  24.     return me.weight
  25. }

tNodeWeightComparator.go

基于权重的顶点比较器, 实现IComparator接口

  1. package a_star
  3. import "errors"
  6. type tNodeWeightComparator struct {
  7. }
  9. func newNodeWeightComparator() IComparator {
  10.     return &tNodeWeightComparator{
  11.     }
  12. }
  14. func (me *tNodeWeightComparator) Less(a interface{}, b interface{}) bool {
  15.     if a == nil || b == nil {
  16.         panic(gNullArgumentError)
  17.     }
  19.     n1 := a.(INode)
  20.     n2 := b.(INode)
  21.     return n1.GetTotalWeight() <= n2.GetTotalWeight()
  22. }
  24. var gNullArgumentError = errors.New("null argument error")

tArrayHeap.go

堆的实现

  1. package a_star
  3. import (
  4.     "errors"
  5.     "fmt"
  6.     "strings"
  7. )
  9. type tArrayHeap struct {
  10.     comparator IComparator
  11.     items []interface{}
  12.     size int
  13.     version int64
  14. }
  16. func newArrayHeap(comparator IComparator) IHeap {
  17.     return &tArrayHeap{
  18.         comparator: comparator,
  19.         items: make([]interface{}, 0),
  20.         size: 0,
  21.         version: 0,
  22.     }
  23. }
  25. func (me *tArrayHeap) Size() int {
  26.     return me.size
  27. }
  29. func (me *tArrayHeap) IsEmpty() bool {
  30.     return me.size <= 0
  31. }
  33. func (me *tArrayHeap) IsNotEmpty() bool {
  34.     return !me.IsEmpty()
  35. }
  37. func (me *tArrayHeap) Push(value interface{}) {
  38.     me.version++
  40.     me.ensureSize(me.size + 1)
  41.     me.items[me.size] = value
  42.     me.size++
  44.     me.ShiftUp(me.size - 1)
  45.     me.version++
  46. }
  49. func (me *tArrayHeap) ensureSize(size int) {
  50.     for ;len(me.items) < size; {
  51.         me.items = append(me.items, nil)
  52.     }
  53. }
  55. func (me *tArrayHeap) parentOf(i int) int {
  56.     return (i - 1) / 2
  57. }
  59. func (me *tArrayHeap) leftChildOf(i int) int {
  60.     return i*2 + 1
  61. }
  63. func (me *tArrayHeap) rightChildOf(i int) int {
  64.     return me.leftChildOf(i) + 1
  65. }
  67. func (me *tArrayHeap) last() (i int, v interface{}) {
  68.     if me.IsEmpty() {
  69.         return -1, nil
  70.     }
  72.     i = me.size - 1
  73.     v = me.items[i]
  74.     return i,v
  75. }
  77. func (me *tArrayHeap) IndexOf(node interface{}) int {
  78.     n := -1
  79.     for i,it := range me.items {
  80.         if it == node {
  81.             n = i
  82.             break
  83.         }
  84.     }
  86.     return n
  87. }
  89. func (me *tArrayHeap) ShiftUp(i int) {
  90.     if i <= 0 {
  91.         return
  92.     }
  93.     v := me.items[i]
  95.     pi := me.parentOf(i)
  96.     pv := me.items[pi]
  98.     if me.comparator.Less(v, pv) {
  99.         me.items[pi], me.items[i] = v, pv
  100.         me.ShiftUp(pi)
  101.     }
  102. }
  104. func (me *tArrayHeap) Pop() (bool, interface{}) {
  105.     if me.IsEmpty() {
  106.         return false, nil
  107.     }
  109.     me.version++
  111.     top := me.items[0]
  112.     li, lv := me.last()
  113.     me.items[0] = nil
  114.     me.size--
  116.     if me.IsEmpty() {
  117.         return true, top
  118.     }
  120.     me.items[0] = lv
  121.     me.items[li] = nil
  123.     me.shiftDown(0)
  124.     me.version++
  126.     return true, top
  127. }
  129. func (me *tArrayHeap) shiftDown(i int) {
  130.     pv := me.items[i]
  131.     ok, ci, cv := me.minChildOf(i)
  132.     if ok && me.comparator.Less(cv, pv) {
  133.         me.items[i], me.items[ci] = cv, pv
  134.         me.shiftDown(ci)
  135.     }
  136. }
  138. func (me *tArrayHeap) minChildOf(p int) (ok bool, i int, v interface{}) {
  139.     li := me.leftChildOf(p)
  140.     if li >= me.size {
  141.         return false, 0, nil
  142.     }
  143.     lv := me.items[li]
  145.     ri := me.rightChildOf(p)
  146.     if ri >= me.size {
  147.         return true, li, lv
  148.     }
  149.     rv := me.items[ri]
  151.     if me.comparator.Less(lv, rv) {
  152.         return true, li, lv
  153.     } else {
  154.         return true, ri, rv
  155.     }
  156. }
  158. func (me *tArrayHeap) String() string {
  159.     level := 0
  160.     lines := make([]string, 0)
  161.     lines = append(lines, "")
  163.     for {
  164.         n := 1<<level
  165.         min := n - 1
  166.         max := n + min - 1
  167.         if min >= me.size {
  168.             break
  169.         }
  171.         line := make([]string, 0)
  172.         for i := min;i <= max;i++ {
  173.             if i >= me.size {
  174.                 break
  175.             }
  176.             line = append(line, fmt.Sprintf("%4d", me.items[i]))
  177.         }
  178.         lines = append(lines, strings.Join(line, ","))
  180.         level++
  181.     }
  183.     return strings.Join(lines, "\n")
  184. }
  186. var gNoMoreElementsError = errors.New("no more elements")

tAStarPathFinder.go

A*算法的实现, 使用xy坐标的直线距离作为估算权重

  1. package a_star
  3. import "math"
  5. type tAStarPathFinder struct {
  6. }
  8. func newAStarPathFinder() IPathFinder {
  9.     return &tAStarPathFinder{}
  10. }
  12. func (me *tAStarPathFinder) FindPath(nodes []INode, lines []ILine, srcID string, dstID string) (bool,[]INode) {
  13.     // 节点索引
  14.     mapNodes := make(map[string]INode, 0)
  15.     for _,it := range nodes {
  16.         mapNodes[it.ID()] = it
  17.     }
  19.     srcNode, ok := mapNodes[srcID]
  20.     if !ok {
  21.         return false, nil
  22.     }
  24.     dstNode,ok := mapNodes[dstID]
  25.     if !ok {
  26.         return false, nil
  27.     }
  29.     // 边的索引
  30.     mapFromLines := make(map[string][]ILine, 0)
  31.     mapToLines := make(map[string][]ILine, 0)
  32.     for _, it := range lines {
  33.         if v,ok := mapFromLines[it.From()];ok {
  34.             mapFromLines[it.From()] = append(v, it)
  35.         } else {
  36.             mapFromLines[it.From()] = []ILine{ it }
  37.         }
  39.         if v,ok := mapToLines[it.To()];ok {
  40.             mapToLines[it.To()] = append(v, it)
  41.         } else {
  42.             mapToLines[it.To()] = []ILine{ it }
  43.         }
  44.     }
  47.     for _,it := range nodes {
  48.         // 设置src节点的weight为0, 其他节点的weight为MaxWeight
  49.         if it.ID() == srcID {
  50.             it.SetMeasuredWeight(0)
  51.         } else {
  52.             it.SetMeasuredWeight(MaxWeight)
  53.         }
  55.         // 计算每个节点到dst节点的估算距离
  56.         if it.ID() == dstID {
  57.             it.SetEstimatedWeight(0)
  58.         } else {
  59.             it.SetEstimatedWeight(me.distance(it.GetX(), it.GetY(), dstNode.GetY(), dstNode.GetY()))
  60.         }
  61.     }
  64.     // 将起点push到堆
  65.     heap := newArrayHeap(newNodeWeightComparator())
  66.     heap.Push(srcNode)
  68.     // 遍历候选节点
  69.     for heap.IsNotEmpty() {
  70.         _, top := heap.Pop()
  71.         from := top.(INode)
  72.         if from.ID() == dstID {
  73.             break
  74.         }
  76.         links, ok := mapFromLines[from.ID()]
  77.         if ok {
  78.             for _,line := range links {
  79.                 if to,ok := mapNodes[line.To()];ok {
  80.                     if me.updateMeasuredWeight(from, to, line) {
  81.                         n := heap.IndexOf(to)
  82.                         if n >= 0 {
  83.                             heap.ShiftUp(n)
  84.                         } else {
  85.                             heap.Push(to)
  86.                         }
  87.                     }
  88.                 }
  89.             }
  90.         }
  91.     }
  93.     // 逆向查找最短路径
  94.     if dstNode.GetMeasuredWeight() >= MaxWeight {
  95.         return false, nil
  96.     }
  98.     path := []INode{ dstNode }
  99.     current := dstNode
  100.     maxRound := len(lines)
  101.     for ;current != srcNode && maxRound > 0;maxRound-- {
  102.         linkedLines, _ := mapToLines[current.ID()]
  103.         for _,line := range linkedLines {
  104.             from, _ := mapNodes[line.From()]
  105.             if from.GetMeasuredWeight() == current.GetMeasuredWeight() - line.Weight() {
  106.                 current = from
  107.                 path = append(path, from)
  108.             }
  109.         }
  110.     }
  112.     if current != srcNode {
  113.         return false, nil
  114.     }
  116.     me.reverse(path)
  117.     return true, path
  118. }
  120. func (me *tAStarPathFinder) distance(x0, y0, x1, y1 int) int {
  121.     dx := x0 - x1
  122.     dy := y0 - y1
  123.     return int(math.Round(math.Sqrt(float64(dx * dx + dy * dy))))
  124. }
  127. func (me *tAStarPathFinder) reverse(nodes []INode) {
  128.     for i,j := 0, len(nodes)-1;i < j;i,j=i+1,j-1 {
  129.         nodes[i], nodes[j] = nodes[j], nodes[i]
  130.     }
  131. }
  133. func (me *tAStarPathFinder) updateMeasuredWeight(from INode, to INode, line ILine) bool {
  134.     w := me.min(from.GetMeasuredWeight() + line.Weight(), to.GetMeasuredWeight())
  135.     if to.GetMeasuredWeight() > w {
  136.         to.SetMeasuredWeight(w)
  137.         return true
  138.     }
  140.     return false
  141. }
  143. func (me *tAStarPathFinder) min(a, b int) int {
  144.     if a <= b {
  145.         return a
  146.     }
  147.     return b
  148. }
  150. var AStarPathFinder = newAStarPathFinder()

(end)