-
Notifications
You must be signed in to change notification settings - Fork 4
/
pathfind.go
131 lines (104 loc) · 2.83 KB
/
pathfind.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
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
package pathfind
// a graph node
type Node interface{}
// a pathfinding graph
type Graph interface {
// send the list of neighboring nodes to a channel
Neighbors(chan Edge, Node)
// the heuristic function to approximate the cost between nodes
H(Node, Node) float32
}
// connections between nodes
type Edge struct {
Node Node
Cost float32
}
// 2D A* pathfinding node
type Path struct {
Parent *Path
Node Node
f, g float32
}
// traverse a node graph for the optimal path
func Search(g Graph, start, goal Node) ([]Node, bool) {
var closedSet []*Path
// all the path nodes being searched
openSet := []*Path{&Path{Node: start, g: 0, f: g.H(start, goal)}}
// search the open set nodes for the best path
for len(openSet) > 0 {
current, path := bestNode(openSet)
// have we reached the goal?
if path.Node == goal {
return nodeListOfPath(path), true
}
// remove current from the open set
openSet[current] = openSet[len(openSet) - 1]
openSet = openSet[:len(openSet) - 1]
// add it to the closed set
closedSet = append(closedSet, path)
// create a channel for all neighbors to be written to
neighbors := make(chan Edge)
// fetch the neighbor nodes
go g.Neighbors(neighbors, path.Node)
// get all the neighboring nodes to this one
for e := range neighbors {
if nodeInSet(e.Node, closedSet) != nil {
continue
}
// check to see if the neighbor is in the open set already
p := nodeInSet(e.Node, openSet)
// calculate the tentative g score for this node and its h score
score := path.g + e.Cost
h := g.H(e.Node, goal)
// add the node to the open set if it isn't in there
if p == nil {
p = &Path{
Node: e.Node,
Parent: path,
g: score,
f: score + h,
}
// add the new path node to the open set
openSet = append(openSet, p)
} else {
// did we get there faster than the old path?
if score < p.g {
p.Parent = path
// update with the new g and f scores
p.g = score
p.f = score + h
}
}
}
}
// no path found
return []Node{}, false
}
// return the list of nodes for given path (from -> goal)
func nodeListOfPath(path *Path) []Node {
if path.Parent == nil {
return []Node{path.Node}
}
// recursively build the list
return append(nodeListOfPath(path.Parent), path.Node)
}
// return a path node with the lowest f score from a set
func bestNode(set []*Path) (int, *Path) {
best := 0
// loop over the entire set of nodes
for i, path := range(set[1:]) {
if path.f < set[best].f {
best = i
}
}
return best, set[best]
}
// true if the node is in a path set
func nodeInSet(node Node, set []*Path) *Path {
for _, path := range(set) {
if path.Node == node {
return path
}
}
return nil
}