-
Notifications
You must be signed in to change notification settings - Fork 0
/
astar_path_computer.go
155 lines (146 loc) · 3.92 KB
/
astar_path_computer.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
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
package main
import (
"fmt"
)
// Used to associate data with a grid cell during computation
type Node struct {
Pos Position // position in grid
From *Node // pointer to next
WhichList int // == N means OPEN, N + 1 means CLOSED
G int // path cost
H int // heuristic
F int // path cost + heuristic
HeapIX int // index in heap array
}
// Prints in format (k)[x, y]
func (n *Node) String() string {
return fmt.Sprintf("(%d)%v", n.F, n.Pos)
}
type AStarPathComputer struct {
Grid *Grid
OH *NodeHeap
N int
Nodes [][]Node
startNode *Node
endNode *Node
}
func NewAStarPathComputer(grid *Grid) *AStarPathComputer {
// build nodes grid
// NOTE: in array-speak, the "rows" are columns. It's just nicer to put
// X as the first coordinate instead of Y
nodes := make([][]Node, grid.W)
for x := 0; x < grid.W; x++ {
nodes[x] = make([]Node, grid.H)
for y := 0; y < grid.H; y++ {
nodes[x][y] = Node{Pos: Position{x, y}}
}
}
// make node heap
c := &AStarPathComputer{
Grid: grid,
N: 0,
Nodes: nodes,
OH: NewNodeHeap(),
}
return c
}
func (c *AStarPathComputer) AStarPath(start Position, end Position) (path []Position) {
// clear the heap which contains leftover nodes from the last calculation
c.OH.Clear()
// increment N so WhichList works properly
c.N += 2
// set start and end nodes
c.startNode = &c.Nodes[start.X][start.Y]
c.endNode = &c.Nodes[end.X][end.Y]
// add first node to OPEN heap
*c.startNode = Node{
Pos: c.startNode.Pos,
From: nil,
WhichList: c.N,
G: 0,
H: ManhattanDistance(start, end),
}
c.OH.Add(c.startNode)
// while open heap has elements...
for c.OH.Len() > 0 {
// pop from open heap and set as closed (whichlist == c.N + 1)
cur, err := c.OH.Pop()
// if err, we have exhausted all squares on open heap and found no path
// return empty list
if err != nil {
return []Position{}
}
// set popped node to CLOSED
cur.WhichList = c.N + 1
// if the current cell is the end, we're here. build the return list
if cur == c.endNode {
path = make([]Position, 0)
for cur != nil {
path = append(path, cur.Pos)
cur = cur.From
}
// return the path to the user
return path
}
// else, we have yet to complete the path. So:
// for each neighbor
for _, delta := range deltas {
nbrPos, dist, err := c.Grid.NbrOf(cur.Pos, delta)
if err != nil {
continue
}
nbr := &c.Nodes[nbrPos.X][nbrPos.Y]
// compute g, h for the neighbor
g := cur.G + dist
h := ManhattanDistance(nbr.Pos, end)
// don't consider this neighbor if the neighbor is in the closed
// list *and* our g is greater or equal to its g score (we already
// have a better way to get to it)
isClosed := nbr.WhichList == c.N+1
if isClosed && g >= nbr.G {
continue
}
// if not on open heap, add it with "From" == cur
isOpen := nbr.WhichList == c.N
if !isOpen {
// set From, G, H
nbr.From = cur
nbr.G = g
nbr.H = h
// set whichlist == OPEN
nbr.WhichList = c.N
// push to open heap
c.OH.Add(nbr)
} else {
// if it *is* on the open heap already, check to see if
// this is a better path to that square
// on -> "open node"
gAlready := nbr.G
if g < gAlready {
// if the open node could be reached better by
// this path, set the g to the new lower g, set the
// "From" reference to cur and fix up the heap because
// we've changed the value of one of its elements
nbr.From = cur
// compute new F after setting new G and add to OPEN heap
nbr.G = g
nbr.F = nbr.G + nbr.H
c.OH.Modified(nbr)
}
}
}
}
return path
}
// Manhattan distance (times 10, since 10 = 1 orthogonal, 14 = 1 diagonal)
func ManhattanDistance(p1 Position, p2 Position) int {
dx := p1.X - p2.X
if dx < 0 {
dx *= -1
}
dy := p1.Y - p2.Y
if dy < 0 {
dy *= -1
}
return 10 * (dx + dy)
}