手撸golang 基本数据结构与算法 图的最短路径 A*(A-Star)算法

ioly 发布于 3 月 2 日

缘起

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

A*(A-Star)算法

A*（A-Star）算法也是一种在图中求解最短路径问题的算法，
由狄克斯特拉算法发展而来。

A*算法不仅会考虑从起点到候补顶点的距离，
还会考虑从当前所在顶点到终点的估算距离。

距离估算值越接近当前顶点到终点的实际值，
A*算法的搜索效率也就越高.
当距离估算值小于实际距离时，
是一定可以得到正确答案的.

A*算法在游戏编程中经常被用于计算敌人追赶玩家时的行动路线等.

摘自 <<我的第一本算法书>> 【日】石田保辉；宫崎修一

场景

如下图, 某游戏中, 地图是网格状的, 我方在S点, 敌人在G点, 空白区域是湖泊/树林等不可到达区域:

现在需要追击敌人, 因此需要计算S点到G点的最短行进路线.
A*算法是以狄克斯特拉算法为基础, 区别是在计算候选节点的权重时, 需要同时考虑测量权重和估算权重.
此场景中, 使用S点到G点坐标的直线距离作为估算权重.
估算权重的作用就像牵引风筝的绳子, 使得每次选取的候选节点, 尽量是靠往终点方向.

流程

for 候选堆不为空:

反向查找最短路径:

设计

单元测试

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)