-
Notifications
You must be signed in to change notification settings - Fork 122
/
jps.lua
270 lines (237 loc) · 11.2 KB
/
jps.lua
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
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
-- Jump Point search algorithm
if (...) then
-- Dependancies
local _PATH = (...):match('(.+)%.search.jps$')
local Heuristics = require (_PATH .. '.core.heuristics')
local Heap = require (_PATH.. '.core.bheap')
-- Internalization
local max, abs = math.max, math.abs
-- Local helpers, these routines will stay private
-- As they are internally used by the public interface
-- Resets properties of nodes expanded during a search
-- This is a lot faster than resetting all nodes
-- between consecutive pathfinding requests
--[[
Looks for the neighbours of a given node.
Returns its natural neighbours plus forced neighbours when the given
node has no parent (generally occurs with the starting node).
Otherwise, based on the direction of move from the parent, returns
neighbours while pruning directions which will lead to symmetric paths.
In case diagonal moves are forbidden, when the given node has no
parent, we return straight neighbours (up, down, left and right).
Otherwise, we add left and right node (perpendicular to the direction
of move) in the neighbours list.
--]]
local function findNeighbours(finder, node, clearance)
if node._parent then
local neighbours = {}
local x,y = node._x, node._y
-- Node have a parent, we will prune some neighbours
-- Gets the direction of move
local dx = (x-node._parent._x)/max(abs(x-node._parent._x),1)
local dy = (y-node._parent._y)/max(abs(y-node._parent._y),1)
-- Diagonal move case
if dx~=0 and dy~=0 then
local walkY, walkX
-- Natural neighbours
if finder._grid:isWalkableAt(x,y+dy,finder._walkable, clearance) then
neighbours[#neighbours+1] = finder._grid:getNodeAt(x,y+dy)
walkY = true
end
if finder._grid:isWalkableAt(x+dx,y,finder._walkable, clearance) then
neighbours[#neighbours+1] = finder._grid:getNodeAt(x+dx,y)
walkX = true
end
if walkX or walkY then
neighbours[#neighbours+1] = finder._grid:getNodeAt(x+dx,y+dy)
end
-- Forced neighbours
if (not finder._grid:isWalkableAt(x-dx,y,finder._walkable, clearance)) and walkY then
neighbours[#neighbours+1] = finder._grid:getNodeAt(x-dx,y+dy)
end
if (not finder._grid:isWalkableAt(x,y-dy,finder._walkable, clearance)) and walkX then
neighbours[#neighbours+1] = finder._grid:getNodeAt(x+dx,y-dy)
end
else
-- Move along Y-axis case
if dx==0 then
local walkY
if finder._grid:isWalkableAt(x,y+dy,finder._walkable, clearance) then
neighbours[#neighbours+1] = finder._grid:getNodeAt(x,y+dy)
-- Forced neighbours are left and right ahead along Y
if (not finder._grid:isWalkableAt(x+1,y,finder._walkable, clearance)) then
neighbours[#neighbours+1] = finder._grid:getNodeAt(x+1,y+dy)
end
if (not finder._grid:isWalkableAt(x-1,y,finder._walkable, clearance)) then
neighbours[#neighbours+1] = finder._grid:getNodeAt(x-1,y+dy)
end
end
-- In case diagonal moves are forbidden : Needs to be optimized
if not finder._allowDiagonal then
if finder._grid:isWalkableAt(x+1,y,finder._walkable, clearance) then
neighbours[#neighbours+1] = finder._grid:getNodeAt(x+1,y)
end
if finder._grid:isWalkableAt(x-1,y,finder._walkable, clearance)
then neighbours[#neighbours+1] = finder._grid:getNodeAt(x-1,y)
end
end
else
-- Move along X-axis case
if finder._grid:isWalkableAt(x+dx,y,finder._walkable, clearance) then
neighbours[#neighbours+1] = finder._grid:getNodeAt(x+dx,y)
-- Forced neighbours are up and down ahead along X
if (not finder._grid:isWalkableAt(x,y+1,finder._walkable, clearance)) then
neighbours[#neighbours+1] = finder._grid:getNodeAt(x+dx,y+1)
end
if (not finder._grid:isWalkableAt(x,y-1,finder._walkable, clearance)) then
neighbours[#neighbours+1] = finder._grid:getNodeAt(x+dx,y-1)
end
end
-- : In case diagonal moves are forbidden
if not finder._allowDiagonal then
if finder._grid:isWalkableAt(x,y+1,finder._walkable, clearance) then
neighbours[#neighbours+1] = finder._grid:getNodeAt(x,y+1)
end
if finder._grid:isWalkableAt(x,y-1,finder._walkable, clearance) then
neighbours[#neighbours+1] = finder._grid:getNodeAt(x,y-1)
end
end
end
end
return neighbours
end
-- Node do not have parent, we return all neighbouring nodes
return finder._grid:getNeighbours(node, finder._walkable, finder._allowDiagonal, finder._tunnel, clearance)
end
--[[
Searches for a jump point (or a turning point) in a specific direction.
This is a generic translation of the algorithm 2 in the paper:
http://users.cecs.anu.edu.au/~dharabor/data/papers/harabor-grastien-aaai11.pdf
The current expanded node is a jump point if near a forced node
In case diagonal moves are forbidden, when lateral nodes (perpendicular to
the direction of moves are walkable, we force them to be turning points in other
to perform a straight move.
--]]
local function jump(finder, node, parent, endNode, clearance)
if not node then return end
local x,y = node._x, node._y
local dx, dy = x - parent._x,y - parent._y
-- If the node to be examined is unwalkable, return nil
if not finder._grid:isWalkableAt(x,y,finder._walkable, clearance) then return end
-- If the node to be examined is the endNode, return this node
if node == endNode then return node end
-- Diagonal search case
if dx~=0 and dy~=0 then
-- Current node is a jump point if one of his leftside/rightside neighbours ahead is forced
if (finder._grid:isWalkableAt(x-dx,y+dy,finder._walkable, clearance) and (not finder._grid:isWalkableAt(x-dx,y,finder._walkable, clearance))) or
(finder._grid:isWalkableAt(x+dx,y-dy,finder._walkable, clearance) and (not finder._grid:isWalkableAt(x,y-dy,finder._walkable, clearance))) then
return node
end
else
-- Search along X-axis case
if dx~=0 then
if finder._allowDiagonal then
-- Current node is a jump point if one of his upside/downside neighbours is forced
if (finder._grid:isWalkableAt(x+dx,y+1,finder._walkable, clearance) and (not finder._grid:isWalkableAt(x,y+1,finder._walkable, clearance))) or
(finder._grid:isWalkableAt(x+dx,y-1,finder._walkable, clearance) and (not finder._grid:isWalkableAt(x,y-1,finder._walkable, clearance))) then
return node
end
else
-- : in case diagonal moves are forbidden
if finder._grid:isWalkableAt(x+1,y,finder._walkable, clearance) or finder._grid:isWalkableAt(x-1,y,finder._walkable, clearance) then return node end
end
else
-- Search along Y-axis case
-- Current node is a jump point if one of his leftside/rightside neighbours is forced
if finder._allowDiagonal then
if (finder._grid:isWalkableAt(x+1,y+dy,finder._walkable, clearance) and (not finder._grid:isWalkableAt(x+1,y,finder._walkable, clearance))) or
(finder._grid:isWalkableAt(x-1,y+dy,finder._walkable, clearance) and (not finder._grid:isWalkableAt(x-1,y,finder._walkable, clearance))) then
return node
end
else
-- : in case diagonal moves are forbidden
if finder._grid:isWalkableAt(x,y+1,finder._walkable, clearance) or finder._grid:isWalkableAt(x,y-1,finder._walkable, clearance) then return node end
end
end
end
-- Recursive horizontal/vertical search
if dx~=0 and dy~=0 then
if jump(finder,finder._grid:getNodeAt(x+dx,y),node,endNode, clearance) then return node end
if jump(finder,finder._grid:getNodeAt(x,y+dy),node,endNode, clearance) then return node end
end
-- Recursive diagonal search
if finder._allowDiagonal then
if finder._grid:isWalkableAt(x+dx,y,finder._walkable, clearance) or finder._grid:isWalkableAt(x,y+dy,finder._walkable, clearance) then
return jump(finder,finder._grid:getNodeAt(x+dx,y+dy),node,endNode, clearance)
end
end
end
--[[
Searches for successors of a given node in the direction of each of its neighbours.
This is a generic translation of the algorithm 1 in the paper:
http://users.cecs.anu.edu.au/~dharabor/data/papers/harabor-grastien-aaai11.pdf
Also, we notice that processing neighbours in a reverse order producing a natural
looking path, as the pathfinder tends to keep heading in the same direction.
In case a jump point was found, and this node happened to be diagonal to the
node currently expanded in a straight mode search, we skip this jump point.
--]]
local function identifySuccessors(finder, openList, node, endNode, clearance, toClear)
-- Gets the valid neighbours of the given node
-- Looks for a jump point in the direction of each neighbour
local neighbours = findNeighbours(finder,node, clearance)
for i = #neighbours,1,-1 do
local skip = false
local neighbour = neighbours[i]
local jumpNode = jump(finder,neighbour,node,endNode, clearance)
-- : in case a diagonal jump point was found in straight mode, skip it.
if jumpNode and not finder._allowDiagonal then
if ((jumpNode._x ~= node._x) and (jumpNode._y ~= node._y)) then skip = true end
end
-- Performs regular A-star on a set of jump points
if jumpNode and not skip then
-- Update the jump node and move it in the closed list if it wasn't there
if not jumpNode._closed then
local extraG = Heuristics.EUCLIDIAN(jumpNode, node)
local newG = node._g + extraG
if not jumpNode._opened or newG < jumpNode._g then
toClear[jumpNode] = true -- Records this node to reset its properties later.
jumpNode._g = newG
jumpNode._h = jumpNode._h or
(finder._heuristic(jumpNode, endNode))
jumpNode._f = jumpNode._g+jumpNode._h
jumpNode._parent = node
if not jumpNode._opened then
openList:push(jumpNode)
jumpNode._opened = true
else
openList:heapify(jumpNode)
end
end
end
end
end
end
-- Calculates a path.
-- Returns the path from location `<startX, startY>` to location `<endX, endY>`.
return function(finder, startNode, endNode, clearance, toClear)
startNode._g, startNode._f, startNode._h = 0,0,0
local openList = Heap()
openList:push(startNode)
startNode._opened = true
toClear[startNode] = true
local node
while not openList:empty() do
-- Pops the lowest F-cost node, moves it in the closed list
node = openList:pop()
node._closed = true
-- If the popped node is the endNode, return it
if node == endNode then
return node
end
-- otherwise, identify successors of the popped node
identifySuccessors(finder, openList, node, endNode, clearance, toClear)
end
-- No path found, return nil
return nil
end
end