/
astar.go
54 lines (46 loc) · 1.56 KB
/
astar.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
package pathfinder
import (
"container/heap"
"fmt"
)
// AStarFunc can be used to find a shortest path to a node that satisfies a goal function.
// It returns a Path from the source to the destination, the cost of that path,
// and an error if it was unable to find a path.
func (f Finder) AStarFunc(from Node, to GoalFunc) (Path, int, error) {
frontier := make(priorityQueue, 1)
frontier[0] = &item{node: from, priority: 0, index: 0}
heap.Init(&frontier)
cameFrom := map[Node]Node{}
costSoFar := map[Node]int{}
cameFrom[from] = from
costSoFar[from] = 0
for frontier.Len() > 0 {
i := heap.Pop(&frontier).(*item)
cur := i.node
if to(cur) {
curPath := Path{cur}
c := 0
for n := cameFrom[cur]; n != from; n = cameFrom[n] {
c++
curPath = append(Path{n}, curPath...)
}
curPath = append(Path{from}, curPath...)
return curPath, costSoFar[cur], nil
}
for _, next := range f.g.Neighbors(cur) {
nextCost := costSoFar[cur] + next.Cost
if cost, ok := costSoFar[next.Target]; !ok || nextCost < cost {
costSoFar[next.Target] = nextCost
heap.Push(&frontier, &item{node: next.Target, priority: nextCost + next.Heuristic})
cameFrom[next.Target] = cur
}
}
}
return Path{}, -1, fmt.Errorf("Unable to find path")
}
// AStar uses the A* algorithm to find a shortest path between two Nodes.
// It returns a Path from the source to the destination, the cost of that path,
// and an error if it was unable to find a path.
func (f Finder) AStar(from, to Node) (Path, int, error) {
return f.AStarFunc(from, func(n Node) bool { return n == to })
}