-
Notifications
You must be signed in to change notification settings - Fork 1.3k
/
BiAStarFinder.js
181 lines (157 loc) · 6.52 KB
/
BiAStarFinder.js
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
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
var Heap = require('heap');
var Util = require('../core/Util');
var Heuristic = require('../core/Heuristic');
var DiagonalMovement = require('../core/DiagonalMovement');
/**
* A* path-finder.
* based upon https://github.com/bgrins/javascript-astar
* @constructor
* @param {Object} opt
* @param {boolean} opt.allowDiagonal Whether diagonal movement is allowed.
* Deprecated, use diagonalMovement instead.
* @param {boolean} opt.dontCrossCorners Disallow diagonal movement touching
* block corners. Deprecated, use diagonalMovement instead.
* @param {DiagonalMovement} opt.diagonalMovement Allowed diagonal movement.
* @param {function} opt.heuristic Heuristic function to estimate the distance
* (defaults to manhattan).
* @param {number} opt.weight Weight to apply to the heuristic to allow for
* suboptimal paths, in order to speed up the search.
*/
function BiAStarFinder(opt) {
opt = opt || {};
this.allowDiagonal = opt.allowDiagonal;
this.dontCrossCorners = opt.dontCrossCorners;
this.diagonalMovement = opt.diagonalMovement;
this.heuristic = opt.heuristic || Heuristic.manhattan;
this.weight = opt.weight || 1;
if (!this.diagonalMovement) {
if (!this.allowDiagonal) {
this.diagonalMovement = DiagonalMovement.Never;
} else {
if (this.dontCrossCorners) {
this.diagonalMovement = DiagonalMovement.OnlyWhenNoObstacles;
} else {
this.diagonalMovement = DiagonalMovement.IfAtMostOneObstacle;
}
}
}
//When diagonal movement is allowed the manhattan heuristic is not admissible
//It should be octile instead
if (this.diagonalMovement === DiagonalMovement.Never) {
this.heuristic = opt.heuristic || Heuristic.manhattan;
} else {
this.heuristic = opt.heuristic || Heuristic.octile;
}
}
/**
* Find and return the the path.
* @return {Array<Array<number>>} The path, including both start and
* end positions.
*/
BiAStarFinder.prototype.findPath = function(startX, startY, endX, endY, grid) {
var cmp = function(nodeA, nodeB) {
return nodeA.f - nodeB.f;
},
startOpenList = new Heap(cmp),
endOpenList = new Heap(cmp),
startNode = grid.getNodeAt(startX, startY),
endNode = grid.getNodeAt(endX, endY),
heuristic = this.heuristic,
diagonalMovement = this.diagonalMovement,
weight = this.weight,
abs = Math.abs, SQRT2 = Math.SQRT2,
node, neighbors, neighbor, i, l, x, y, ng,
BY_START = 1, BY_END = 2;
// set the `g` and `f` value of the start node to be 0
// and push it into the start open list
startNode.g = 0;
startNode.f = 0;
startOpenList.push(startNode);
startNode.opened = BY_START;
// set the `g` and `f` value of the end node to be 0
// and push it into the open open list
endNode.g = 0;
endNode.f = 0;
endOpenList.push(endNode);
endNode.opened = BY_END;
// while both the open lists are not empty
while (!startOpenList.empty() && !endOpenList.empty()) {
// pop the position of start node which has the minimum `f` value.
node = startOpenList.pop();
node.closed = true;
// get neigbours of the current node
neighbors = grid.getNeighbors(node, diagonalMovement);
for (i = 0, l = neighbors.length; i < l; ++i) {
neighbor = neighbors[i];
if (neighbor.closed) {
continue;
}
if (neighbor.opened === BY_END) {
return Util.biBacktrace(node, neighbor);
}
x = neighbor.x;
y = neighbor.y;
// get the distance between current node and the neighbor
// and calculate the next g score
ng = node.g + ((x - node.x === 0 || y - node.y === 0) ? 1 : SQRT2);
// check if the neighbor has not been inspected yet, or
// can be reached with smaller cost from the current node
if (!neighbor.opened || ng < neighbor.g) {
neighbor.g = ng;
neighbor.h = neighbor.h ||
weight * heuristic(abs(x - endX), abs(y - endY));
neighbor.f = neighbor.g + neighbor.h;
neighbor.parent = node;
if (!neighbor.opened) {
startOpenList.push(neighbor);
neighbor.opened = BY_START;
} else {
// the neighbor can be reached with smaller cost.
// Since its f value has been updated, we have to
// update its position in the open list
startOpenList.updateItem(neighbor);
}
}
} // end for each neighbor
// pop the position of end node which has the minimum `f` value.
node = endOpenList.pop();
node.closed = true;
// get neigbours of the current node
neighbors = grid.getNeighbors(node, diagonalMovement);
for (i = 0, l = neighbors.length; i < l; ++i) {
neighbor = neighbors[i];
if (neighbor.closed) {
continue;
}
if (neighbor.opened === BY_START) {
return Util.biBacktrace(neighbor, node);
}
x = neighbor.x;
y = neighbor.y;
// get the distance between current node and the neighbor
// and calculate the next g score
ng = node.g + ((x - node.x === 0 || y - node.y === 0) ? 1 : SQRT2);
// check if the neighbor has not been inspected yet, or
// can be reached with smaller cost from the current node
if (!neighbor.opened || ng < neighbor.g) {
neighbor.g = ng;
neighbor.h = neighbor.h ||
weight * heuristic(abs(x - startX), abs(y - startY));
neighbor.f = neighbor.g + neighbor.h;
neighbor.parent = node;
if (!neighbor.opened) {
endOpenList.push(neighbor);
neighbor.opened = BY_END;
} else {
// the neighbor can be reached with smaller cost.
// Since its f value has been updated, we have to
// update its position in the open list
endOpenList.updateItem(neighbor);
}
}
} // end for each neighbor
} // end while not open list empty
// fail to find the path
return [];
};
module.exports = BiAStarFinder;