1 /* Copyright (C) 2017 Wildfire Games.
2 * This file is part of 0 A.D.
3 *
4 * 0 A.D. is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 2 of the License, or
7 * (at your option) any later version.
8 *
9 * 0 A.D. is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with 0 A.D. If not, see <http://www.gnu.org/licenses/>.
16 */
17
18 #include "precompiled.h"
19
20 #include "LongPathfinder.h"
21
22 #include "lib/bits.h"
23 #include "ps/Profile.h"
24
25 #include "Geometry.h"
26
27 /**
28 * Jump point cache.
29 *
30 * The JPS algorithm wants to efficiently either find the first jump point
31 * in some direction from some cell (not counting the cell itself),
32 * if it is reachable without crossing any impassable cells;
33 * or know that there is no such reachable jump point.
34 * The jump point is always on a passable cell.
35 * We cache that data to allow fast lookups, which helps performance
36 * significantly (especially on sparse maps).
37 * Recalculation might be expensive but the underlying passability data
38 * changes relatively rarely.
39 *
40 * To allow the algorithm to detect goal cells, we want to treat them as
41 * jump points too. (That means the algorithm will push those cells onto
42 * its open queue, and will eventually pop a goal cell and realise it's done.)
43 * (Goals might be circles/squares/etc, not just a single cell.)
44 * But the goal generally changes for every path request, so we can't cache
45 * it like the normal jump points.
46 * Instead, if there's no jump point from some cell then we'll cache the
47 * first impassable cell as an 'obstruction jump point'
48 * (with a flag to distinguish from a real jump point), and then the caller
49 * can test whether the goal includes a cell that's closer than the first
50 * (obstruction or real) jump point,
51 * and treat the goal cell as a jump point in that case.
52 *
53 * We only ever need to find the jump point relative to a passable cell;
54 * the cache is allowed to return bogus values for impassable cells.
55 */
56 class JumpPointCache
57 {
58 /**
59 * Simple space-inefficient row storage.
60 */
61 struct RowRaw
62 {
63 std::vector<u16> data;
64
GetMemoryUsageJumpPointCache::RowRaw65 size_t GetMemoryUsage() const
66 {
67 return data.capacity() * sizeof(u16);
68 }
69
RowRawJumpPointCache::RowRaw70 RowRaw(int length)
71 {
72 data.resize(length);
73 }
74
75 /**
76 * Set cells x0 <= x < x1 to have jump point x1.
77 */
SetRangeJumpPointCache::RowRaw78 void SetRange(int x0, int x1, bool obstruction)
79 {
80 ENSURE(0 <= x0 && x0 <= x1 && x1 < (int)data.size());
81 for (int x = x0; x < x1; ++x)
82 data[x] = (x1 << 1) | (obstruction ? 1 : 0);
83 }
84
85 /**
86 * Returns the coordinate of the next jump point xp (where x < xp),
87 * and whether it's an obstruction point or jump point.
88 */
GetJumpPointCache::RowRaw89 void Get(int x, int& xp, bool& obstruction)
90 {
91 ENSURE(0 <= x && x < (int)data.size());
92 xp = data[x] >> 1;
93 obstruction = data[x] & 1;
94 }
95
FinishJumpPointCache::RowRaw96 void Finish() { }
97 };
98
99 struct RowTree
100 {
101 /**
102 * Represents an interval [u15 x0, u16 x1)
103 * with a boolean obstruction flag,
104 * packed into a single u32.
105 */
106 struct Interval
107 {
IntervalJumpPointCache::RowTree::Interval108 Interval() : data(0) { }
109
IntervalJumpPointCache::RowTree::Interval110 Interval(int x0, int x1, bool obstruction)
111 {
112 ENSURE(0 <= x0 && x0 < 0x8000);
113 ENSURE(0 <= x1 && x1 < 0x10000);
114 data = ((u32)x0 << 17) | (u32)(obstruction ? 0x10000 : 0) | (u32)x1;
115 }
116
x0JumpPointCache::RowTree::Interval117 int x0() { return data >> 17; }
x1JumpPointCache::RowTree::Interval118 int x1() { return data & 0xFFFF; }
obstructionJumpPointCache::RowTree::Interval119 bool obstruction() { return (data & 0x10000) != 0; }
120
121 u32 data;
122 };
123
124 std::vector<Interval> data;
125
GetMemoryUsageJumpPointCache::RowTree126 size_t GetMemoryUsage() const
127 {
128 return data.capacity() * sizeof(Interval);
129 }
130
RowTreeJumpPointCache::RowTree131 RowTree(int UNUSED(length))
132 {
133 }
134
SetRangeJumpPointCache::RowTree135 void SetRange(int x0, int x1, bool obstruction)
136 {
137 ENSURE(0 <= x0 && x0 <= x1);
138 data.emplace_back(x0, x1, obstruction);
139 }
140
141 /**
142 * Recursive helper function for Finish().
143 * Given two ranges [x0, pivot) and [pivot, x1) in the sorted array 'data',
144 * the pivot element is added onto the binary tree (stored flattened in an
145 * array), and then each range is split into two sub-ranges with a pivot in
146 * the middle (to ensure the tree remains balanced) and ConstructTree recurses.
147 */
ConstructTreeJumpPointCache::RowTree148 void ConstructTree(std::vector<Interval>& tree, size_t x0, size_t pivot, size_t x1, size_t idx_tree)
149 {
150 ENSURE(x0 < data.size());
151 ENSURE(x1 <= data.size());
152 ENSURE(x0 <= pivot);
153 ENSURE(pivot < x1);
154 ENSURE(idx_tree < tree.size());
155
156 tree[idx_tree] = data[pivot];
157
158 if (x0 < pivot)
159 ConstructTree(tree, x0, (x0 + pivot) / 2, pivot, (idx_tree << 1) + 1);
160 if (pivot + 1 < x1)
161 ConstructTree(tree, pivot + 1, (pivot + x1) / 2, x1, (idx_tree << 1) + 2);
162 }
163
FinishJumpPointCache::RowTree164 void Finish()
165 {
166 // Convert the sorted interval list into a balanced binary tree
167
168 std::vector<Interval> tree;
169
170 if (!data.empty())
171 {
172 size_t depth = ceil_log2(data.size() + 1);
173 tree.resize((1 << depth) - 1);
174 ConstructTree(tree, 0, data.size() / 2, data.size(), 0);
175 }
176
177 data.swap(tree);
178 }
179
GetJumpPointCache::RowTree180 void Get(int x, int& xp, bool& obstruction)
181 {
182 // Search the binary tree for an interval which contains x
183 int i = 0;
184 while (true)
185 {
186 ENSURE(i < (int)data.size());
187 Interval interval = data[i];
188 if (x < interval.x0())
189 i = (i << 1) + 1;
190 else if (x >= interval.x1())
191 i = (i << 1) + 2;
192 else
193 {
194 ENSURE(interval.x0() <= x && x < interval.x1());
195 xp = interval.x1();
196 obstruction = interval.obstruction();
197 return;
198 }
199 }
200 }
201 };
202
203 // Pick one of the row implementations
204 typedef RowRaw Row;
205
206 public:
207 int m_Width;
208 int m_Height;
209 std::vector<Row> m_JumpPointsRight;
210 std::vector<Row> m_JumpPointsLeft;
211 std::vector<Row> m_JumpPointsUp;
212 std::vector<Row> m_JumpPointsDown;
213
214 /**
215 * Compute the cached obstruction/jump points for each cell,
216 * in a single direction. By default the code assumes the rightwards
217 * (+i) direction; set 'transpose' to switch to upwards (+j),
218 * and/or set 'mirror' to reverse the direction.
219 */
ComputeRows(std::vector<Row> & rows,const Grid<NavcellData> & terrain,pass_class_t passClass,bool transpose,bool mirror)220 void ComputeRows(std::vector<Row>& rows,
221 const Grid<NavcellData>& terrain, pass_class_t passClass,
222 bool transpose, bool mirror)
223 {
224 int w = terrain.m_W;
225 int h = terrain.m_H;
226
227 if (transpose)
228 std::swap(w, h);
229
230 // Check the terrain passability, adjusted for transpose/mirror
231 #define TERRAIN_IS_PASSABLE(i, j) \
232 IS_PASSABLE( \
233 mirror \
234 ? (transpose ? terrain.get((j), w-1-(i)) : terrain.get(w-1-(i), (j))) \
235 : (transpose ? terrain.get((j), (i)) : terrain.get((i), (j))) \
236 , passClass)
237
238 rows.reserve(h);
239 for (int j = 0; j < h; ++j)
240 rows.emplace_back(w);
241
242 for (int j = 1; j < h - 1; ++j)
243 {
244 // Find the first passable cell.
245 // Then, find the next jump/obstruction point after that cell,
246 // and store that point for the passable range up to that cell,
247 // then repeat.
248
249 int i = 0;
250 while (i < w)
251 {
252 // Restart the 'while' loop until we reach a passable cell
253 if (!TERRAIN_IS_PASSABLE(i, j))
254 {
255 ++i;
256 continue;
257 }
258
259 // i is now a passable cell; find the next jump/obstruction point.
260 // (We assume the map is surrounded by impassable cells, so we don't
261 // need to explicitly check for world bounds here.)
262
263 int i0 = i;
264 while (true)
265 {
266 ++i;
267
268 // Check if we hit an obstructed tile
269 if (!TERRAIN_IS_PASSABLE(i, j))
270 {
271 rows[j].SetRange(i0, i, true);
272 break;
273 }
274
275 // Check if we reached a jump point
276 if ((!TERRAIN_IS_PASSABLE(i - 1, j - 1) && TERRAIN_IS_PASSABLE(i, j - 1)) ||
277 (!TERRAIN_IS_PASSABLE(i - 1, j + 1) && TERRAIN_IS_PASSABLE(i, j + 1)))
278 {
279 rows[j].SetRange(i0, i, false);
280 break;
281 }
282 }
283 }
284
285 rows[j].Finish();
286 }
287 #undef TERRAIN_IS_PASSABLE
288 }
289
reset(const Grid<NavcellData> * terrain,pass_class_t passClass)290 void reset(const Grid<NavcellData>* terrain, pass_class_t passClass)
291 {
292 PROFILE3("JumpPointCache reset");
293 TIMER(L"JumpPointCache reset");
294
295 m_Width = terrain->m_W;
296 m_Height = terrain->m_H;
297
298 ComputeRows(m_JumpPointsRight, *terrain, passClass, false, false);
299 ComputeRows(m_JumpPointsLeft, *terrain, passClass, false, true);
300 ComputeRows(m_JumpPointsUp, *terrain, passClass, true, false);
301 ComputeRows(m_JumpPointsDown, *terrain, passClass, true, true);
302 }
303
GetMemoryUsage() const304 size_t GetMemoryUsage() const
305 {
306 size_t bytes = 0;
307 for (int i = 0; i < m_Width; ++i)
308 {
309 bytes += m_JumpPointsUp[i].GetMemoryUsage();
310 bytes += m_JumpPointsDown[i].GetMemoryUsage();
311 }
312 for (int j = 0; j < m_Height; ++j)
313 {
314 bytes += m_JumpPointsRight[j].GetMemoryUsage();
315 bytes += m_JumpPointsLeft[j].GetMemoryUsage();
316 }
317 return bytes;
318 }
319
320 /**
321 * Returns the next jump point (or goal point) to explore,
322 * at (ip, j) where i < ip.
323 * Returns i if there is no such point.
324 */
GetJumpPointRight(int i,int j,const PathGoal & goal)325 int GetJumpPointRight(int i, int j, const PathGoal& goal)
326 {
327 int ip;
328 bool obstruction;
329 m_JumpPointsRight[j].Get(i, ip, obstruction);
330 // Adjust ip to be a goal cell, if there is one closer than the jump point;
331 // and then return the new ip if there is a goal,
332 // or the old ip if there is a (non-obstruction) jump point
333 if (goal.NavcellRectContainsGoal(i + 1, j, ip - 1, j, &ip, NULL) || !obstruction)
334 return ip;
335 return i;
336 }
337
GetJumpPointLeft(int i,int j,const PathGoal & goal)338 int GetJumpPointLeft(int i, int j, const PathGoal& goal)
339 {
340 int mip; // mirrored value, because m_JumpPointsLeft is generated from a mirrored map
341 bool obstruction;
342 m_JumpPointsLeft[j].Get(m_Width - 1 - i, mip, obstruction);
343 int ip = m_Width - 1 - mip;
344 if (goal.NavcellRectContainsGoal(i - 1, j, ip + 1, j, &ip, NULL) || !obstruction)
345 return ip;
346 return i;
347 }
348
GetJumpPointUp(int i,int j,const PathGoal & goal)349 int GetJumpPointUp(int i, int j, const PathGoal& goal)
350 {
351 int jp;
352 bool obstruction;
353 m_JumpPointsUp[i].Get(j, jp, obstruction);
354 if (goal.NavcellRectContainsGoal(i, j + 1, i, jp - 1, NULL, &jp) || !obstruction)
355 return jp;
356 return j;
357 }
358
GetJumpPointDown(int i,int j,const PathGoal & goal)359 int GetJumpPointDown(int i, int j, const PathGoal& goal)
360 {
361 int mjp; // mirrored value
362 bool obstruction;
363 m_JumpPointsDown[i].Get(m_Height - 1 - j, mjp, obstruction);
364 int jp = m_Height - 1 - mjp;
365 if (goal.NavcellRectContainsGoal(i, j - 1, i, jp + 1, NULL, &jp) || !obstruction)
366 return jp;
367 return j;
368 }
369 };
370
371 //////////////////////////////////////////////////////////
372
LongPathfinder()373 LongPathfinder::LongPathfinder() :
374 m_UseJPSCache(false),
375 m_Grid(NULL), m_GridSize(0),
376 m_DebugOverlay(NULL), m_DebugGrid(NULL), m_DebugPath(NULL)
377 {
378 }
379
~LongPathfinder()380 LongPathfinder::~LongPathfinder()
381 {
382 SAFE_DELETE(m_DebugOverlay);
383 SAFE_DELETE(m_DebugGrid);
384 SAFE_DELETE(m_DebugPath);
385 }
386
387 #define PASSABLE(i, j) IS_PASSABLE(state.terrain->get(i, j), state.passClass)
388
389 // Calculate heuristic cost from tile i,j to goal
390 // (This ought to be an underestimate for correctness)
CalculateHeuristic(int i,int j,int iGoal,int jGoal)391 PathCost LongPathfinder::CalculateHeuristic(int i, int j, int iGoal, int jGoal)
392 {
393 int di = abs(i - iGoal);
394 int dj = abs(j - jGoal);
395 int diag = std::min(di, dj);
396 return PathCost(di - diag + dj - diag, diag);
397 }
398
399 // Do the A* processing for a neighbour tile i,j.
ProcessNeighbour(int pi,int pj,int i,int j,PathCost pg,PathfinderState & state)400 void LongPathfinder::ProcessNeighbour(int pi, int pj, int i, int j, PathCost pg, PathfinderState& state)
401 {
402 // Reject impassable tiles
403 if (!PASSABLE(i, j))
404 return;
405
406 PathfindTile& n = state.tiles->get(i, j);
407
408 if (n.IsClosed())
409 return;
410
411 PathCost dg;
412 if (pi == i)
413 dg = PathCost::horizvert(abs(pj - j));
414 else if (pj == j)
415 dg = PathCost::horizvert(abs(pi - i));
416 else
417 {
418 ASSERT(abs((int)pi - (int)i) == abs((int)pj - (int)j)); // must be 45 degrees
419 dg = PathCost::diag(abs((int)pi - (int)i));
420 }
421
422 PathCost g = pg + dg; // cost to this tile = cost to predecessor + delta from predecessor
423
424 PathCost h = CalculateHeuristic(i, j, state.iGoal, state.jGoal);
425
426 // If this is a new tile, compute the heuristic distance
427 if (n.IsUnexplored())
428 {
429 // Remember the best tile we've seen so far, in case we never actually reach the target
430 if (h < state.hBest)
431 {
432 state.hBest = h;
433 state.iBest = i;
434 state.jBest = j;
435 }
436 }
437 else
438 {
439 // If we've already seen this tile, and the new path to this tile does not have a
440 // better cost, then stop now
441 if (g >= n.GetCost())
442 return;
443
444 // Otherwise, we have a better path.
445
446 // If we've already added this tile to the open list:
447 if (n.IsOpen())
448 {
449 // This is a better path, so replace the old one with the new cost/parent
450 PathCost gprev = n.GetCost();
451 n.SetCost(g);
452 n.SetPred(pi, pj, i, j);
453 state.open.promote(TileID(i, j), gprev + h, g + h, h);
454 return;
455 }
456 }
457
458 // Add it to the open list:
459 n.SetStatusOpen();
460 n.SetCost(g);
461 n.SetPred(pi, pj, i, j);
462 PriorityQueue::Item t = { TileID(i, j), g + h, h };
463 state.open.push(t);
464 }
465
466 /*
467 * In the JPS algorithm, after a tile is taken off the open queue,
468 * we don't process every adjacent neighbour (as in standard A*).
469 * Instead we only move in a subset of directions (depending on the
470 * direction from the predecessor); and instead of moving by a single
471 * cell, we move up to the next jump point in that direction.
472 * The AddJumped... functions do this by calling ProcessNeighbour
473 * on the jump point (if any) in a certain direction.
474 * The HasJumped... functions return whether there is any jump point
475 * in that direction.
476 */
477
478 // JPS functions scan navcells towards one direction
479 // OnTheWay tests whether we are scanning towards the right direction, to avoid useless scans
OnTheWay(int i,int j,int di,int dj,int iGoal,int jGoal)480 inline bool OnTheWay(int i, int j, int di, int dj, int iGoal, int jGoal)
481 {
482 if (dj != 0)
483 {
484 // We're not moving towards the goal
485 if ((jGoal - j) * dj < 0)
486 return false;
487 }
488 else if (j != jGoal)
489 return false;
490
491 if (di != 0)
492 {
493 // We're not moving towards the goal
494 if ((iGoal - i) * di < 0)
495 return false;
496 }
497 else if (i != iGoal)
498 return false;
499
500 return true;
501 }
502
503
AddJumpedHoriz(int i,int j,int di,PathCost g,PathfinderState & state,bool detectGoal)504 void LongPathfinder::AddJumpedHoriz(int i, int j, int di, PathCost g, PathfinderState& state, bool detectGoal)
505 {
506 if (m_UseJPSCache)
507 {
508 int jump;
509 if (di > 0)
510 jump = state.jpc->GetJumpPointRight(i, j, state.goal);
511 else
512 jump = state.jpc->GetJumpPointLeft(i, j, state.goal);
513
514 if (jump != i)
515 ProcessNeighbour(i, j, jump, j, g, state);
516 }
517 else
518 {
519 ASSERT(di == 1 || di == -1);
520 int ni = i + di;
521 while (true)
522 {
523 if (!PASSABLE(ni, j))
524 break;
525
526 if (detectGoal && state.goal.NavcellContainsGoal(ni, j))
527 {
528 state.open.clear();
529 ProcessNeighbour(i, j, ni, j, g, state);
530 break;
531 }
532
533 if ((!PASSABLE(ni - di, j - 1) && PASSABLE(ni, j - 1)) ||
534 (!PASSABLE(ni - di, j + 1) && PASSABLE(ni, j + 1)))
535 {
536 ProcessNeighbour(i, j, ni, j, g, state);
537 break;
538 }
539
540 ni += di;
541 }
542 }
543 }
544
545 // Returns the i-coordinate of the jump point if it exists, else returns i
HasJumpedHoriz(int i,int j,int di,PathfinderState & state,bool detectGoal)546 int LongPathfinder::HasJumpedHoriz(int i, int j, int di, PathfinderState& state, bool detectGoal)
547 {
548 if (m_UseJPSCache)
549 {
550 int jump;
551 if (di > 0)
552 jump = state.jpc->GetJumpPointRight(i, j, state.goal);
553 else
554 jump = state.jpc->GetJumpPointLeft(i, j, state.goal);
555
556 return jump;
557 }
558 else
559 {
560 ASSERT(di == 1 || di == -1);
561 int ni = i + di;
562 while (true)
563 {
564 if (!PASSABLE(ni, j))
565 return i;
566
567 if (detectGoal && state.goal.NavcellContainsGoal(ni, j))
568 {
569 state.open.clear();
570 return ni;
571 }
572
573 if ((!PASSABLE(ni - di, j - 1) && PASSABLE(ni, j - 1)) ||
574 (!PASSABLE(ni - di, j + 1) && PASSABLE(ni, j + 1)))
575 return ni;
576
577 ni += di;
578 }
579 }
580 }
581
AddJumpedVert(int i,int j,int dj,PathCost g,PathfinderState & state,bool detectGoal)582 void LongPathfinder::AddJumpedVert(int i, int j, int dj, PathCost g, PathfinderState& state, bool detectGoal)
583 {
584 if (m_UseJPSCache)
585 {
586 int jump;
587 if (dj > 0)
588 jump = state.jpc->GetJumpPointUp(i, j, state.goal);
589 else
590 jump = state.jpc->GetJumpPointDown(i, j, state.goal);
591
592 if (jump != j)
593 ProcessNeighbour(i, j, i, jump, g, state);
594 }
595 else
596 {
597 ASSERT(dj == 1 || dj == -1);
598 int nj = j + dj;
599 while (true)
600 {
601 if (!PASSABLE(i, nj))
602 break;
603
604 if (detectGoal && state.goal.NavcellContainsGoal(i, nj))
605 {
606 state.open.clear();
607 ProcessNeighbour(i, j, i, nj, g, state);
608 break;
609 }
610
611 if ((!PASSABLE(i - 1, nj - dj) && PASSABLE(i - 1, nj)) ||
612 (!PASSABLE(i + 1, nj - dj) && PASSABLE(i + 1, nj)))
613 {
614 ProcessNeighbour(i, j, i, nj, g, state);
615 break;
616 }
617
618 nj += dj;
619 }
620 }
621 }
622
623 // Returns the j-coordinate of the jump point if it exists, else returns j
HasJumpedVert(int i,int j,int dj,PathfinderState & state,bool detectGoal)624 int LongPathfinder::HasJumpedVert(int i, int j, int dj, PathfinderState& state, bool detectGoal)
625 {
626 if (m_UseJPSCache)
627 {
628 int jump;
629 if (dj > 0)
630 jump = state.jpc->GetJumpPointUp(i, j, state.goal);
631 else
632 jump = state.jpc->GetJumpPointDown(i, j, state.goal);
633
634 return jump;
635 }
636 else
637 {
638 ASSERT(dj == 1 || dj == -1);
639 int nj = j + dj;
640 while (true)
641 {
642 if (!PASSABLE(i, nj))
643 return j;
644
645 if (detectGoal && state.goal.NavcellContainsGoal(i, nj))
646 {
647 state.open.clear();
648 return nj;
649 }
650
651 if ((!PASSABLE(i - 1, nj - dj) && PASSABLE(i - 1, nj)) ||
652 (!PASSABLE(i + 1, nj - dj) && PASSABLE(i + 1, nj)))
653 return nj;
654
655 nj += dj;
656 }
657 }
658 }
659
660 /*
661 * We never cache diagonal jump points - they're usually so frequent that
662 * a linear search is about as cheap and avoids the setup cost and memory cost.
663 */
AddJumpedDiag(int i,int j,int di,int dj,PathCost g,PathfinderState & state)664 void LongPathfinder::AddJumpedDiag(int i, int j, int di, int dj, PathCost g, PathfinderState& state)
665 {
666 // ProcessNeighbour(i, j, i + di, j + dj, g, state);
667 // return;
668
669 ASSERT(di == 1 || di == -1);
670 ASSERT(dj == 1 || dj == -1);
671
672 int ni = i + di;
673 int nj = j + dj;
674 bool detectGoal = OnTheWay(i, j, di, dj, state.iGoal, state.jGoal);
675 while (true)
676 {
677 // Stop if we hit an obstructed cell
678 if (!PASSABLE(ni, nj))
679 return;
680
681 // Stop if moving onto this cell caused us to
682 // touch the corner of an obstructed cell
683 if (!PASSABLE(ni - di, nj) || !PASSABLE(ni, nj - dj))
684 return;
685
686 // Process this cell if it's at the goal
687 if (detectGoal && state.goal.NavcellContainsGoal(ni, nj))
688 {
689 state.open.clear();
690 ProcessNeighbour(i, j, ni, nj, g, state);
691 return;
692 }
693
694 int fi = HasJumpedHoriz(ni, nj, di, state, detectGoal && OnTheWay(ni, nj, di, 0, state.iGoal, state.jGoal));
695 int fj = HasJumpedVert(ni, nj, dj, state, detectGoal && OnTheWay(ni, nj, 0, dj, state.iGoal, state.jGoal));
696 if (fi != ni || fj != nj)
697 {
698 ProcessNeighbour(i, j, ni, nj, g, state);
699 g += PathCost::diag(abs(ni - i));
700
701 if (fi != ni)
702 ProcessNeighbour(ni, nj, fi, nj, g, state);
703
704 if (fj != nj)
705 ProcessNeighbour(ni, nj, ni, fj, g, state);
706
707 return;
708 }
709
710 ni += di;
711 nj += dj;
712 }
713 }
714
ComputeJPSPath(entity_pos_t x0,entity_pos_t z0,const PathGoal & origGoal,pass_class_t passClass,WaypointPath & path)715 void LongPathfinder::ComputeJPSPath(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, WaypointPath& path)
716 {
717 PROFILE("ComputePathJPS");
718 PROFILE2_IFSPIKE("ComputePathJPS", 0.0002);
719 PathfinderState state = { 0 };
720
721 state.jpc = m_JumpPointCache[passClass].get();
722 if (m_UseJPSCache && !state.jpc)
723 {
724 state.jpc = new JumpPointCache;
725 state.jpc->reset(m_Grid, passClass);
726 debug_printf("PATHFINDER: JPC memory: %d kB\n", (int)state.jpc->GetMemoryUsage() / 1024);
727 m_JumpPointCache[passClass] = shared_ptr<JumpPointCache>(state.jpc);
728 }
729
730 // Convert the start coordinates to tile indexes
731 u16 i0, j0;
732 Pathfinding::NearestNavcell(x0, z0, i0, j0, m_GridSize, m_GridSize);
733
734 if (!IS_PASSABLE(m_Grid->get(i0, j0), passClass))
735 {
736 // The JPS pathfinder requires units to be on passable tiles
737 // (otherwise it might crash), so handle the supposedly-invalid
738 // state specially
739 m_PathfinderHier.FindNearestPassableNavcell(i0, j0, passClass);
740 }
741
742 state.goal = origGoal;
743
744 // Make the goal reachable. This includes shortening the path if the goal is in a non-passable
745 // region, transforming non-point goals to reachable point goals, etc.
746 m_PathfinderHier.MakeGoalReachable(i0, j0, state.goal, passClass);
747
748 ENSURE(state.goal.type == PathGoal::POINT);
749
750 // If we're already at the goal tile, then move directly to the exact goal coordinates
751 if (state.goal.NavcellContainsGoal(i0, j0))
752 {
753 path.m_Waypoints.emplace_back(Waypoint{ state.goal.x, state.goal.z });
754 return;
755 }
756
757 Pathfinding::NearestNavcell(state.goal.x, state.goal.z, state.iGoal, state.jGoal, m_GridSize, m_GridSize);
758
759 ENSURE((state.goal.x / Pathfinding::NAVCELL_SIZE).ToInt_RoundToNegInfinity() == state.iGoal);
760 ENSURE((state.goal.z / Pathfinding::NAVCELL_SIZE).ToInt_RoundToNegInfinity() == state.jGoal);
761
762 state.passClass = passClass;
763
764 state.steps = 0;
765
766 state.tiles = new PathfindTileGrid(m_Grid->m_W, m_Grid->m_H);
767 state.terrain = m_Grid;
768
769 state.iBest = i0;
770 state.jBest = j0;
771 state.hBest = CalculateHeuristic(i0, j0, state.iGoal, state.jGoal);
772
773 PriorityQueue::Item start = { TileID(i0, j0), PathCost() };
774 state.open.push(start);
775 state.tiles->get(i0, j0).SetStatusOpen();
776 state.tiles->get(i0, j0).SetPred(i0, j0, i0, j0);
777 state.tiles->get(i0, j0).SetCost(PathCost());
778
779 while (true)
780 {
781 ++state.steps;
782
783 // If we ran out of tiles to examine, give up
784 if (state.open.empty())
785 break;
786
787 // Move best tile from open to closed
788 PriorityQueue::Item curr = state.open.pop();
789 u16 i = curr.id.i();
790 u16 j = curr.id.j();
791 state.tiles->get(i, j).SetStatusClosed();
792
793 // If we've reached the destination, stop
794 if (state.goal.NavcellContainsGoal(i, j))
795 {
796 state.iBest = i;
797 state.jBest = j;
798 state.hBest = PathCost();
799 break;
800 }
801
802 PathfindTile tile = state.tiles->get(i, j);
803 PathCost g = tile.GetCost();
804
805 // Get the direction of the predecessor tile from this tile
806 int dpi = tile.GetPredDI();
807 int dpj = tile.GetPredDJ();
808 dpi = (dpi < 0 ? -1 : dpi > 0 ? 1 : 0);
809 dpj = (dpj < 0 ? -1 : dpj > 0 ? 1 : 0);
810
811 if (dpi != 0 && dpj == 0)
812 {
813 // Moving horizontally from predecessor
814 if (!PASSABLE(i + dpi, j - 1))
815 {
816 AddJumpedDiag(i, j, -dpi, -1, g, state);
817 AddJumpedVert(i, j, -1, g, state, OnTheWay(i, j, 0, -1, state.iGoal, state.jGoal));
818 }
819 if (!PASSABLE(i + dpi, j + 1))
820 {
821 AddJumpedDiag(i, j, -dpi, +1, g, state);
822 AddJumpedVert(i, j, +1, g, state, OnTheWay(i, j, 0, +1, state.iGoal, state.jGoal));
823 }
824 AddJumpedHoriz(i, j, -dpi, g, state, OnTheWay(i, j, -dpi, 0, state.iGoal, state.jGoal));
825 }
826 else if (dpi == 0 && dpj != 0)
827 {
828 // Moving vertically from predecessor
829 if (!PASSABLE(i - 1, j + dpj))
830 {
831 AddJumpedDiag(i, j, -1, -dpj, g, state);
832 AddJumpedHoriz(i, j, -1, g, state,OnTheWay(i, j, -1, 0, state.iGoal, state.jGoal));
833 }
834 if (!PASSABLE(i + 1, j + dpj))
835 {
836 AddJumpedDiag(i, j, +1, -dpj, g, state);
837 AddJumpedHoriz(i, j, +1, g, state,OnTheWay(i, j, +1, 0, state.iGoal, state.jGoal));
838 }
839 AddJumpedVert(i, j, -dpj, g, state, OnTheWay(i, j, 0, -dpj, state.iGoal, state.jGoal));
840 }
841 else if (dpi != 0 && dpj != 0)
842 {
843 // Moving diagonally from predecessor
844 AddJumpedHoriz(i, j, -dpi, g, state, OnTheWay(i, j, -dpi, 0, state.iGoal, state.jGoal));
845 AddJumpedVert(i, j, -dpj, g, state, OnTheWay(i, j, 0, -dpj, state.iGoal, state.jGoal));
846 AddJumpedDiag(i, j, -dpi, -dpj, g, state);
847 }
848 else
849 {
850 // No predecessor, i.e. the start tile
851 // Start searching in every direction
852
853 // XXX - check passability?
854
855 bool passl = PASSABLE(i - 1, j);
856 bool passr = PASSABLE(i + 1, j);
857 bool passd = PASSABLE(i, j - 1);
858 bool passu = PASSABLE(i, j + 1);
859
860 if (passl && passd)
861 ProcessNeighbour(i, j, i-1, j-1, g, state);
862 if (passr && passd)
863 ProcessNeighbour(i, j, i+1, j-1, g, state);
864 if (passl && passu)
865 ProcessNeighbour(i, j, i-1, j+1, g, state);
866 if (passr && passu)
867 ProcessNeighbour(i, j, i+1, j+1, g, state);
868 if (passl)
869 ProcessNeighbour(i, j, i-1, j, g, state);
870 if (passr)
871 ProcessNeighbour(i, j, i+1, j, g, state);
872 if (passd)
873 ProcessNeighbour(i, j, i, j-1, g, state);
874 if (passu)
875 ProcessNeighbour(i, j, i, j+1, g, state);
876 }
877 }
878
879 // Reconstruct the path (in reverse)
880 u16 ip = state.iBest, jp = state.jBest;
881 while (ip != i0 || jp != j0)
882 {
883 PathfindTile& n = state.tiles->get(ip, jp);
884 entity_pos_t x, z;
885 Pathfinding::NavcellCenter(ip, jp, x, z);
886 path.m_Waypoints.emplace_back(Waypoint{ x, z });
887
888 // Follow the predecessor link
889 ip = n.GetPredI(ip);
890 jp = n.GetPredJ(jp);
891 }
892 // The last waypoint is slightly incorrect (it's not the goal but the center
893 // of the navcell of the goal), so replace it
894 if (!path.m_Waypoints.empty())
895 path.m_Waypoints.front() = { state.goal.x, state.goal.z };
896
897 ImprovePathWaypoints(path, passClass, origGoal.maxdist, x0, z0);
898
899 // Save this grid for debug display
900 delete m_DebugGrid;
901 m_DebugGrid = state.tiles;
902 m_DebugSteps = state.steps;
903 m_DebugGoal = state.goal;
904 }
905
906 #undef PASSABLE
907
ImprovePathWaypoints(WaypointPath & path,pass_class_t passClass,entity_pos_t maxDist,entity_pos_t x0,entity_pos_t z0)908 void LongPathfinder::ImprovePathWaypoints(WaypointPath& path, pass_class_t passClass, entity_pos_t maxDist, entity_pos_t x0, entity_pos_t z0)
909 {
910 if (path.m_Waypoints.empty())
911 return;
912
913 if (maxDist > fixed::Zero())
914 {
915 CFixedVector2D start(x0, z0);
916 CFixedVector2D first(path.m_Waypoints.back().x, path.m_Waypoints.back().z);
917 CFixedVector2D offset = first - start;
918 if (offset.CompareLength(maxDist) > 0)
919 {
920 offset.Normalize(maxDist);
921 path.m_Waypoints.emplace_back(Waypoint{ (start + offset).X, (start + offset).Y });
922 }
923 }
924
925 if (path.m_Waypoints.size() < 2)
926 return;
927
928 std::vector<Waypoint>& waypoints = path.m_Waypoints;
929 std::vector<Waypoint> newWaypoints;
930
931 CFixedVector2D prev(waypoints.front().x, waypoints.front().z);
932 newWaypoints.push_back(waypoints.front());
933 for (size_t k = 1; k < waypoints.size() - 1; ++k)
934 {
935 CFixedVector2D ahead(waypoints[k + 1].x, waypoints[k + 1].z);
936 CFixedVector2D curr(waypoints[k].x, waypoints[k].z);
937
938 if (maxDist > fixed::Zero() && (curr - prev).CompareLength(maxDist) > 0)
939 {
940 // We are too far away from the previous waypoint, so create one in
941 // between and continue with the improvement of the path
942 prev = prev + (curr - prev) / 2;
943 newWaypoints.emplace_back(Waypoint{ prev.X, prev.Y });
944 }
945
946 // If we're mostly straight, don't even bother.
947 if ((ahead - curr).Perpendicular().Dot(curr - prev).Absolute() <= fixed::Epsilon() * 100)
948 continue;
949
950 if (!Pathfinding::CheckLineMovement(prev.X, prev.Y, ahead.X, ahead.Y, passClass, *m_Grid))
951 {
952 prev = CFixedVector2D(waypoints[k].x, waypoints[k].z);
953 newWaypoints.push_back(waypoints[k]);
954 }
955 }
956 newWaypoints.push_back(waypoints.back());
957 path.m_Waypoints.swap(newWaypoints);
958 }
959
GetDebugDataJPS(u32 & steps,double & time,Grid<u8> & grid) const960 void LongPathfinder::GetDebugDataJPS(u32& steps, double& time, Grid<u8>& grid) const
961 {
962 steps = m_DebugSteps;
963 time = m_DebugTime;
964
965 if (!m_DebugGrid)
966 return;
967
968 u16 iGoal, jGoal;
969 Pathfinding::NearestNavcell(m_DebugGoal.x, m_DebugGoal.z, iGoal, jGoal, m_GridSize, m_GridSize);
970
971 grid = Grid<u8>(m_DebugGrid->m_W, m_DebugGrid->m_H);
972 for (u16 j = 0; j < grid.m_H; ++j)
973 {
974 for (u16 i = 0; i < grid.m_W; ++i)
975 {
976 if (i == iGoal && j == jGoal)
977 continue;
978 PathfindTile t = m_DebugGrid->get(i, j);
979 grid.set(i, j, (t.IsOpen() ? 1 : 0) | (t.IsClosed() ? 2 : 0));
980 }
981 }
982 }
983
SetDebugOverlay(bool enabled)984 void LongPathfinder::SetDebugOverlay(bool enabled)
985 {
986 if (enabled && !m_DebugOverlay)
987 m_DebugOverlay = new LongOverlay(*this);
988 else if (!enabled && m_DebugOverlay)
989 SAFE_DELETE(m_DebugOverlay);
990 }
991
ComputePath(entity_pos_t x0,entity_pos_t z0,const PathGoal & origGoal,pass_class_t passClass,std::vector<CircularRegion> excludedRegions,WaypointPath & path)992 void LongPathfinder::ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal,
993 pass_class_t passClass, std::vector<CircularRegion> excludedRegions, WaypointPath& path)
994 {
995 GenerateSpecialMap(passClass, excludedRegions);
996 ComputePath(x0, z0, origGoal, SPECIAL_PASS_CLASS, path);
997 }
998
InRegion(u16 i,u16 j,CircularRegion region)999 inline bool InRegion(u16 i, u16 j, CircularRegion region)
1000 {
1001 fixed cellX = Pathfinding::NAVCELL_SIZE * i;
1002 fixed cellZ = Pathfinding::NAVCELL_SIZE * j;
1003
1004 return CFixedVector2D(cellX - region.x, cellZ - region.z).CompareLength(region.r) <= 0;
1005 }
1006
GenerateSpecialMap(pass_class_t passClass,std::vector<CircularRegion> excludedRegions)1007 void LongPathfinder::GenerateSpecialMap(pass_class_t passClass, std::vector<CircularRegion> excludedRegions)
1008 {
1009 for (u16 j = 0; j < m_Grid->m_H; ++j)
1010 {
1011 for (u16 i = 0; i < m_Grid->m_W; ++i)
1012 {
1013 NavcellData n = m_Grid->get(i, j);
1014 if (!IS_PASSABLE(n, passClass))
1015 {
1016 n |= SPECIAL_PASS_CLASS;
1017 m_Grid->set(i, j, n);
1018 continue;
1019 }
1020
1021 for (CircularRegion& region : excludedRegions)
1022 {
1023 if (!InRegion(i, j, region))
1024 continue;
1025
1026 n |= SPECIAL_PASS_CLASS;
1027 break;
1028 }
1029 m_Grid->set(i, j, n);
1030 }
1031 }
1032 }
1033