Line data Source code
1 : /* Copyright (C) 2021 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 : #include "HierarchicalPathfinder.h"
27 :
28 : #include <mutex>
29 :
30 : namespace
31 : {
32 : static std::mutex g_DebugMutex;
33 : }
34 :
35 : /**
36 : * Jump point cache.
37 : *
38 : * The JPS algorithm wants to efficiently either find the first jump point
39 : * in some direction from some cell (not counting the cell itself),
40 : * if it is reachable without crossing any impassable cells;
41 : * or know that there is no such reachable jump point.
42 : * The jump point is always on a passable cell.
43 : * We cache that data to allow fast lookups, which helps performance
44 : * significantly (especially on sparse maps).
45 : * Recalculation might be expensive but the underlying passability data
46 : * changes relatively rarely.
47 : *
48 : * To allow the algorithm to detect goal cells, we want to treat them as
49 : * jump points too. (That means the algorithm will push those cells onto
50 : * its open queue, and will eventually pop a goal cell and realise it's done.)
51 : * (Goals might be circles/squares/etc, not just a single cell.)
52 : * But the goal generally changes for every path request, so we can't cache
53 : * it like the normal jump points.
54 : * Instead, if there's no jump point from some cell then we'll cache the
55 : * first impassable cell as an 'obstruction jump point'
56 : * (with a flag to distinguish from a real jump point), and then the caller
57 : * can test whether the goal includes a cell that's closer than the first
58 : * (obstruction or real) jump point,
59 : * and treat the goal cell as a jump point in that case.
60 : *
61 : * We only ever need to find the jump point relative to a passable cell;
62 : * the cache is allowed to return bogus values for impassable cells.
63 : */
64 0 : class JumpPointCache
65 : {
66 : /**
67 : * Simple space-inefficient row storage.
68 : */
69 0 : struct RowRaw
70 : {
71 : std::vector<u16> data;
72 :
73 0 : size_t GetMemoryUsage() const
74 : {
75 0 : return data.capacity() * sizeof(u16);
76 : }
77 :
78 0 : RowRaw(int length)
79 0 : {
80 0 : data.resize(length);
81 0 : }
82 :
83 : /**
84 : * Set cells x0 <= x < x1 to have jump point x1.
85 : */
86 0 : void SetRange(int x0, int x1, bool obstruction)
87 : {
88 0 : ENSURE(0 <= x0 && x0 <= x1 && x1 < (int)data.size());
89 0 : for (int x = x0; x < x1; ++x)
90 0 : data[x] = (x1 << 1) | (obstruction ? 1 : 0);
91 0 : }
92 :
93 : /**
94 : * Returns the coordinate of the next jump point xp (where x < xp),
95 : * and whether it's an obstruction point or jump point.
96 : */
97 0 : void Get(int x, int& xp, bool& obstruction) const
98 : {
99 0 : ENSURE(0 <= x && x < (int)data.size());
100 0 : xp = data[x] >> 1;
101 0 : obstruction = data[x] & 1;
102 0 : }
103 :
104 0 : void Finish() { }
105 : };
106 :
107 : struct RowTree
108 : {
109 : /**
110 : * Represents an interval [u15 x0, u16 x1)
111 : * with a boolean obstruction flag,
112 : * packed into a single u32.
113 : */
114 : struct Interval
115 : {
116 : Interval() : data(0) { }
117 :
118 : Interval(int x0, int x1, bool obstruction)
119 : {
120 : ENSURE(0 <= x0 && x0 < 0x8000);
121 : ENSURE(0 <= x1 && x1 < 0x10000);
122 : data = ((u32)x0 << 17) | (u32)(obstruction ? 0x10000 : 0) | (u32)x1;
123 : }
124 :
125 : int x0() { return data >> 17; }
126 : int x1() { return data & 0xFFFF; }
127 : bool obstruction() { return (data & 0x10000) != 0; }
128 :
129 : u32 data;
130 : };
131 :
132 : std::vector<Interval> data;
133 :
134 : size_t GetMemoryUsage() const
135 : {
136 : return data.capacity() * sizeof(Interval);
137 : }
138 :
139 : RowTree(int UNUSED(length))
140 : {
141 : }
142 :
143 : void SetRange(int x0, int x1, bool obstruction)
144 : {
145 : ENSURE(0 <= x0 && x0 <= x1);
146 : data.emplace_back(x0, x1, obstruction);
147 : }
148 :
149 : /**
150 : * Recursive helper function for Finish().
151 : * Given two ranges [x0, pivot) and [pivot, x1) in the sorted array 'data',
152 : * the pivot element is added onto the binary tree (stored flattened in an
153 : * array), and then each range is split into two sub-ranges with a pivot in
154 : * the middle (to ensure the tree remains balanced) and ConstructTree recurses.
155 : */
156 : void ConstructTree(std::vector<Interval>& tree, size_t x0, size_t pivot, size_t x1, size_t idx_tree)
157 : {
158 : ENSURE(x0 < data.size());
159 : ENSURE(x1 <= data.size());
160 : ENSURE(x0 <= pivot);
161 : ENSURE(pivot < x1);
162 : ENSURE(idx_tree < tree.size());
163 :
164 : tree[idx_tree] = data[pivot];
165 :
166 : if (x0 < pivot)
167 : ConstructTree(tree, x0, (x0 + pivot) / 2, pivot, (idx_tree << 1) + 1);
168 : if (pivot + 1 < x1)
169 : ConstructTree(tree, pivot + 1, (pivot + x1) / 2, x1, (idx_tree << 1) + 2);
170 : }
171 :
172 : void Finish()
173 : {
174 : // Convert the sorted interval list into a balanced binary tree
175 :
176 : std::vector<Interval> tree;
177 :
178 : if (!data.empty())
179 : {
180 : size_t depth = ceil_log2(data.size() + 1);
181 : tree.resize((1 << depth) - 1);
182 : ConstructTree(tree, 0, data.size() / 2, data.size(), 0);
183 : }
184 :
185 : data.swap(tree);
186 : }
187 :
188 : void Get(int x, int& xp, bool& obstruction) const
189 : {
190 : // Search the binary tree for an interval which contains x
191 : int i = 0;
192 : while (true)
193 : {
194 : ENSURE(i < (int)data.size());
195 : Interval interval = data[i];
196 : if (x < interval.x0())
197 : i = (i << 1) + 1;
198 : else if (x >= interval.x1())
199 : i = (i << 1) + 2;
200 : else
201 : {
202 : ENSURE(interval.x0() <= x && x < interval.x1());
203 : xp = interval.x1();
204 : obstruction = interval.obstruction();
205 : return;
206 : }
207 : }
208 : }
209 : };
210 :
211 : // Pick one of the row implementations
212 : typedef RowRaw Row;
213 :
214 : public:
215 : int m_Width;
216 : int m_Height;
217 : std::vector<Row> m_JumpPointsRight;
218 : std::vector<Row> m_JumpPointsLeft;
219 : std::vector<Row> m_JumpPointsUp;
220 : std::vector<Row> m_JumpPointsDown;
221 :
222 : /**
223 : * Compute the cached obstruction/jump points for each cell,
224 : * in a single direction. By default the code assumes the rightwards
225 : * (+i) direction; set 'transpose' to switch to upwards (+j),
226 : * and/or set 'mirror' to reverse the direction.
227 : */
228 0 : void ComputeRows(std::vector<Row>& rows,
229 : const Grid<NavcellData>& terrain, pass_class_t passClass,
230 : bool transpose, bool mirror)
231 : {
232 0 : int w = terrain.m_W;
233 0 : int h = terrain.m_H;
234 :
235 0 : if (transpose)
236 0 : std::swap(w, h);
237 :
238 : // Check the terrain passability, adjusted for transpose/mirror
239 : #define TERRAIN_IS_PASSABLE(i, j) \
240 : IS_PASSABLE( \
241 : mirror \
242 : ? (transpose ? terrain.get((j), w-1-(i)) : terrain.get(w-1-(i), (j))) \
243 : : (transpose ? terrain.get((j), (i)) : terrain.get((i), (j))) \
244 : , passClass)
245 :
246 0 : rows.reserve(h);
247 0 : for (int j = 0; j < h; ++j)
248 0 : rows.emplace_back(w);
249 :
250 0 : for (int j = 1; j < h - 1; ++j)
251 : {
252 : // Find the first passable cell.
253 : // Then, find the next jump/obstruction point after that cell,
254 : // and store that point for the passable range up to that cell,
255 : // then repeat.
256 :
257 0 : int i = 0;
258 0 : while (i < w)
259 : {
260 : // Restart the 'while' loop until we reach a passable cell
261 0 : if (!TERRAIN_IS_PASSABLE(i, j))
262 : {
263 0 : ++i;
264 0 : continue;
265 : }
266 :
267 : // i is now a passable cell; find the next jump/obstruction point.
268 : // (We assume the map is surrounded by impassable cells, so we don't
269 : // need to explicitly check for world bounds here.)
270 :
271 0 : int i0 = i;
272 : while (true)
273 : {
274 0 : ++i;
275 :
276 : // Check if we hit an obstructed tile
277 0 : if (!TERRAIN_IS_PASSABLE(i, j))
278 : {
279 0 : rows[j].SetRange(i0, i, true);
280 0 : break;
281 : }
282 :
283 : // Check if we reached a jump point
284 0 : if ((!TERRAIN_IS_PASSABLE(i - 1, j - 1) && TERRAIN_IS_PASSABLE(i, j - 1)) ||
285 0 : (!TERRAIN_IS_PASSABLE(i - 1, j + 1) && TERRAIN_IS_PASSABLE(i, j + 1)))
286 : {
287 0 : rows[j].SetRange(i0, i, false);
288 0 : break;
289 : }
290 : }
291 : }
292 :
293 0 : rows[j].Finish();
294 : }
295 : #undef TERRAIN_IS_PASSABLE
296 0 : }
297 :
298 0 : void reset(const Grid<NavcellData>* terrain, pass_class_t passClass)
299 : {
300 0 : PROFILE2("JumpPointCache reset");
301 0 : TIMER(L"JumpPointCache reset");
302 :
303 0 : m_Width = terrain->m_W;
304 0 : m_Height = terrain->m_H;
305 :
306 0 : ComputeRows(m_JumpPointsRight, *terrain, passClass, false, false);
307 0 : ComputeRows(m_JumpPointsLeft, *terrain, passClass, false, true);
308 0 : ComputeRows(m_JumpPointsUp, *terrain, passClass, true, false);
309 0 : ComputeRows(m_JumpPointsDown, *terrain, passClass, true, true);
310 0 : }
311 :
312 0 : size_t GetMemoryUsage() const
313 : {
314 0 : size_t bytes = 0;
315 0 : for (int i = 0; i < m_Width; ++i)
316 : {
317 0 : bytes += m_JumpPointsUp[i].GetMemoryUsage();
318 0 : bytes += m_JumpPointsDown[i].GetMemoryUsage();
319 : }
320 0 : for (int j = 0; j < m_Height; ++j)
321 : {
322 0 : bytes += m_JumpPointsRight[j].GetMemoryUsage();
323 0 : bytes += m_JumpPointsLeft[j].GetMemoryUsage();
324 : }
325 0 : return bytes;
326 : }
327 :
328 : /**
329 : * Returns the next jump point (or goal point) to explore,
330 : * at (ip, j) where i < ip.
331 : * Returns i if there is no such point.
332 : */
333 0 : int GetJumpPointRight(int i, int j, const PathGoal& goal) const
334 : {
335 : int ip;
336 : bool obstruction;
337 0 : m_JumpPointsRight[j].Get(i, ip, obstruction);
338 : // Adjust ip to be a goal cell, if there is one closer than the jump point;
339 : // and then return the new ip if there is a goal,
340 : // or the old ip if there is a (non-obstruction) jump point
341 0 : if (goal.NavcellRectContainsGoal(i + 1, j, ip - 1, j, &ip, NULL) || !obstruction)
342 0 : return ip;
343 0 : return i;
344 : }
345 :
346 0 : int GetJumpPointLeft(int i, int j, const PathGoal& goal) const
347 : {
348 : int mip; // mirrored value, because m_JumpPointsLeft is generated from a mirrored map
349 : bool obstruction;
350 0 : m_JumpPointsLeft[j].Get(m_Width - 1 - i, mip, obstruction);
351 0 : int ip = m_Width - 1 - mip;
352 0 : if (goal.NavcellRectContainsGoal(i - 1, j, ip + 1, j, &ip, NULL) || !obstruction)
353 0 : return ip;
354 0 : return i;
355 : }
356 :
357 0 : int GetJumpPointUp(int i, int j, const PathGoal& goal) const
358 : {
359 : int jp;
360 : bool obstruction;
361 0 : m_JumpPointsUp[i].Get(j, jp, obstruction);
362 0 : if (goal.NavcellRectContainsGoal(i, j + 1, i, jp - 1, NULL, &jp) || !obstruction)
363 0 : return jp;
364 0 : return j;
365 : }
366 :
367 0 : int GetJumpPointDown(int i, int j, const PathGoal& goal) const
368 : {
369 : int mjp; // mirrored value
370 : bool obstruction;
371 0 : m_JumpPointsDown[i].Get(m_Height - 1 - j, mjp, obstruction);
372 0 : int jp = m_Height - 1 - mjp;
373 0 : if (goal.NavcellRectContainsGoal(i, j - 1, i, jp + 1, NULL, &jp) || !obstruction)
374 0 : return jp;
375 0 : return j;
376 : }
377 : };
378 :
379 : //////////////////////////////////////////////////////////
380 :
381 3 : LongPathfinder::LongPathfinder() :
382 : m_UseJPSCache(false),
383 3 : m_Grid(NULL), m_GridSize(0)
384 : {
385 3 : }
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)
391 0 : PathCost LongPathfinder::CalculateHeuristic(int i, int j, int iGoal, int jGoal) const
392 : {
393 0 : int di = abs(i - iGoal);
394 0 : int dj = abs(j - jGoal);
395 0 : int diag = std::min(di, dj);
396 0 : return PathCost(di - diag + dj - diag, diag);
397 : }
398 :
399 : // Do the A* processing for a neighbour tile i,j.
400 0 : void LongPathfinder::ProcessNeighbour(int pi, int pj, int i, int j, PathCost pg, PathfinderState& state) const
401 : {
402 : // Reject impassable tiles
403 0 : if (!PASSABLE(i, j))
404 0 : return;
405 :
406 0 : PathfindTile& n = state.tiles->get(i, j);
407 :
408 0 : if (n.IsClosed())
409 0 : return;
410 :
411 0 : PathCost dg;
412 0 : if (pi == i)
413 0 : dg = PathCost::horizvert(abs(pj - j));
414 0 : else if (pj == j)
415 0 : dg = PathCost::horizvert(abs(pi - i));
416 : else
417 : {
418 0 : ASSERT(abs((int)pi - (int)i) == abs((int)pj - (int)j)); // must be 45 degrees
419 0 : dg = PathCost::diag(abs((int)pi - (int)i));
420 : }
421 :
422 0 : PathCost g = pg + dg; // cost to this tile = cost to predecessor + delta from predecessor
423 :
424 0 : PathCost h = CalculateHeuristic(i, j, state.iGoal, state.jGoal);
425 :
426 : // If this is a new tile, compute the heuristic distance
427 0 : if (n.IsUnexplored())
428 : {
429 : // Remember the best tile we've seen so far, in case we never actually reach the target
430 0 : if (h < state.hBest)
431 : {
432 0 : state.hBest = h;
433 0 : state.iBest = i;
434 0 : 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 0 : if (g >= n.GetCost())
442 0 : return;
443 :
444 : // Otherwise, we have a better path.
445 :
446 : // If we've already added this tile to the open list:
447 0 : if (n.IsOpen())
448 : {
449 : // This is a better path, so replace the old one with the new cost/parent
450 0 : PathCost gprev = n.GetCost();
451 0 : n.SetCost(g);
452 0 : n.SetPred(pi, pj, i, j);
453 0 : state.open.promote(TileID(i, j), gprev + h, g + h, h);
454 0 : return;
455 : }
456 : }
457 :
458 : // Add it to the open list:
459 0 : n.SetStatusOpen();
460 0 : n.SetCost(g);
461 0 : n.SetPred(pi, pj, i, j);
462 0 : PriorityQueue::Item t = { TileID(i, j), g + h, h };
463 0 : 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
480 0 : inline bool OnTheWay(int i, int j, int di, int dj, int iGoal, int jGoal)
481 : {
482 0 : if (dj != 0)
483 : {
484 : // We're not moving towards the goal
485 0 : if ((jGoal - j) * dj < 0)
486 0 : return false;
487 : }
488 0 : else if (j != jGoal)
489 0 : return false;
490 :
491 0 : if (di != 0)
492 : {
493 : // We're not moving towards the goal
494 0 : if ((iGoal - i) * di < 0)
495 0 : return false;
496 : }
497 0 : else if (i != iGoal)
498 0 : return false;
499 :
500 0 : return true;
501 : }
502 :
503 :
504 0 : void LongPathfinder::AddJumpedHoriz(int i, int j, int di, PathCost g, PathfinderState& state, bool detectGoal) const
505 : {
506 0 : if (m_UseJPSCache)
507 : {
508 : int jump;
509 0 : if (di > 0)
510 0 : jump = state.jpc->GetJumpPointRight(i, j, state.goal);
511 : else
512 0 : jump = state.jpc->GetJumpPointLeft(i, j, state.goal);
513 :
514 0 : if (jump != i)
515 0 : ProcessNeighbour(i, j, jump, j, g, state);
516 : }
517 : else
518 : {
519 0 : ASSERT(di == 1 || di == -1);
520 0 : int ni = i + di;
521 : while (true)
522 : {
523 0 : if (!PASSABLE(ni, j))
524 0 : break;
525 :
526 0 : if (detectGoal && state.goal.NavcellContainsGoal(ni, j))
527 : {
528 0 : state.open.clear();
529 0 : ProcessNeighbour(i, j, ni, j, g, state);
530 0 : break;
531 : }
532 :
533 0 : if ((!PASSABLE(ni - di, j - 1) && PASSABLE(ni, j - 1)) ||
534 0 : (!PASSABLE(ni - di, j + 1) && PASSABLE(ni, j + 1)))
535 : {
536 0 : ProcessNeighbour(i, j, ni, j, g, state);
537 0 : break;
538 : }
539 :
540 0 : ni += di;
541 : }
542 : }
543 0 : }
544 :
545 : // Returns the i-coordinate of the jump point if it exists, else returns i
546 0 : int LongPathfinder::HasJumpedHoriz(int i, int j, int di, PathfinderState& state, bool detectGoal) const
547 : {
548 0 : if (m_UseJPSCache)
549 : {
550 : int jump;
551 0 : if (di > 0)
552 0 : jump = state.jpc->GetJumpPointRight(i, j, state.goal);
553 : else
554 0 : jump = state.jpc->GetJumpPointLeft(i, j, state.goal);
555 :
556 0 : return jump;
557 : }
558 : else
559 : {
560 0 : ASSERT(di == 1 || di == -1);
561 0 : int ni = i + di;
562 : while (true)
563 : {
564 0 : if (!PASSABLE(ni, j))
565 0 : return i;
566 :
567 0 : if (detectGoal && state.goal.NavcellContainsGoal(ni, j))
568 : {
569 0 : state.open.clear();
570 0 : return ni;
571 : }
572 :
573 0 : if ((!PASSABLE(ni - di, j - 1) && PASSABLE(ni, j - 1)) ||
574 0 : (!PASSABLE(ni - di, j + 1) && PASSABLE(ni, j + 1)))
575 0 : return ni;
576 :
577 0 : ni += di;
578 : }
579 : }
580 : }
581 :
582 0 : void LongPathfinder::AddJumpedVert(int i, int j, int dj, PathCost g, PathfinderState& state, bool detectGoal) const
583 : {
584 0 : if (m_UseJPSCache)
585 : {
586 : int jump;
587 0 : if (dj > 0)
588 0 : jump = state.jpc->GetJumpPointUp(i, j, state.goal);
589 : else
590 0 : jump = state.jpc->GetJumpPointDown(i, j, state.goal);
591 :
592 0 : if (jump != j)
593 0 : ProcessNeighbour(i, j, i, jump, g, state);
594 : }
595 : else
596 : {
597 0 : ASSERT(dj == 1 || dj == -1);
598 0 : int nj = j + dj;
599 : while (true)
600 : {
601 0 : if (!PASSABLE(i, nj))
602 0 : break;
603 :
604 0 : if (detectGoal && state.goal.NavcellContainsGoal(i, nj))
605 : {
606 0 : state.open.clear();
607 0 : ProcessNeighbour(i, j, i, nj, g, state);
608 0 : break;
609 : }
610 :
611 0 : if ((!PASSABLE(i - 1, nj - dj) && PASSABLE(i - 1, nj)) ||
612 0 : (!PASSABLE(i + 1, nj - dj) && PASSABLE(i + 1, nj)))
613 : {
614 0 : ProcessNeighbour(i, j, i, nj, g, state);
615 0 : break;
616 : }
617 :
618 0 : nj += dj;
619 : }
620 : }
621 0 : }
622 :
623 : // Returns the j-coordinate of the jump point if it exists, else returns j
624 0 : int LongPathfinder::HasJumpedVert(int i, int j, int dj, PathfinderState& state, bool detectGoal) const
625 : {
626 0 : if (m_UseJPSCache)
627 : {
628 : int jump;
629 0 : if (dj > 0)
630 0 : jump = state.jpc->GetJumpPointUp(i, j, state.goal);
631 : else
632 0 : jump = state.jpc->GetJumpPointDown(i, j, state.goal);
633 :
634 0 : return jump;
635 : }
636 : else
637 : {
638 0 : ASSERT(dj == 1 || dj == -1);
639 0 : int nj = j + dj;
640 : while (true)
641 : {
642 0 : if (!PASSABLE(i, nj))
643 0 : return j;
644 :
645 0 : if (detectGoal && state.goal.NavcellContainsGoal(i, nj))
646 : {
647 0 : state.open.clear();
648 0 : return nj;
649 : }
650 :
651 0 : if ((!PASSABLE(i - 1, nj - dj) && PASSABLE(i - 1, nj)) ||
652 0 : (!PASSABLE(i + 1, nj - dj) && PASSABLE(i + 1, nj)))
653 0 : return nj;
654 :
655 0 : 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 : */
664 0 : void LongPathfinder::AddJumpedDiag(int i, int j, int di, int dj, PathCost g, PathfinderState& state) const
665 : {
666 : // ProcessNeighbour(i, j, i + di, j + dj, g, state);
667 : // return;
668 :
669 0 : ASSERT(di == 1 || di == -1);
670 0 : ASSERT(dj == 1 || dj == -1);
671 :
672 0 : int ni = i + di;
673 0 : int nj = j + dj;
674 0 : bool detectGoal = OnTheWay(i, j, di, dj, state.iGoal, state.jGoal);
675 : while (true)
676 : {
677 : // Stop if we hit an obstructed cell
678 0 : if (!PASSABLE(ni, nj))
679 0 : return;
680 :
681 : // Stop if moving onto this cell caused us to
682 : // touch the corner of an obstructed cell
683 0 : if (!PASSABLE(ni - di, nj) || !PASSABLE(ni, nj - dj))
684 0 : return;
685 :
686 : // Process this cell if it's at the goal
687 0 : if (detectGoal && state.goal.NavcellContainsGoal(ni, nj))
688 : {
689 0 : state.open.clear();
690 0 : ProcessNeighbour(i, j, ni, nj, g, state);
691 0 : return;
692 : }
693 :
694 0 : int fi = HasJumpedHoriz(ni, nj, di, state, detectGoal && OnTheWay(ni, nj, di, 0, state.iGoal, state.jGoal));
695 0 : int fj = HasJumpedVert(ni, nj, dj, state, detectGoal && OnTheWay(ni, nj, 0, dj, state.iGoal, state.jGoal));
696 0 : if (fi != ni || fj != nj)
697 : {
698 0 : ProcessNeighbour(i, j, ni, nj, g, state);
699 0 : g += PathCost::diag(abs(ni - i));
700 :
701 0 : if (fi != ni)
702 0 : ProcessNeighbour(ni, nj, fi, nj, g, state);
703 :
704 0 : if (fj != nj)
705 0 : ProcessNeighbour(ni, nj, ni, fj, g, state);
706 :
707 0 : return;
708 : }
709 :
710 0 : ni += di;
711 0 : nj += dj;
712 0 : }
713 : }
714 :
715 0 : void LongPathfinder::ComputeJPSPath(const HierarchicalPathfinder& hierPath, entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, WaypointPath& path) const
716 : {
717 0 : PROFILE2("ComputePathJPS");
718 0 : PathfinderState state = { 0 };
719 :
720 0 : if (m_UseJPSCache)
721 : {
722 : // Needs to lock for construction, or several threads might try doing that at the same time.
723 : static std::mutex JPCMutex;
724 0 : std::unique_lock<std::mutex> lock(JPCMutex);
725 0 : std::map<pass_class_t, std::shared_ptr<JumpPointCache>>::const_iterator it = m_JumpPointCache.find(passClass);
726 0 : if (it != m_JumpPointCache.end())
727 0 : state.jpc = it->second.get();
728 :
729 0 : if (!state.jpc)
730 : {
731 0 : m_JumpPointCache[passClass] = std::make_shared<JumpPointCache>();
732 0 : m_JumpPointCache[passClass]->reset(m_Grid, passClass);
733 0 : debug_printf("PATHFINDER: JPC memory: %d kB\n", (int)state.jpc->GetMemoryUsage() / 1024);
734 0 : state.jpc = m_JumpPointCache[passClass].get();
735 : }
736 : }
737 :
738 : // Convert the start coordinates to tile indexes
739 : u16 i0, j0;
740 0 : Pathfinding::NearestNavcell(x0, z0, i0, j0, m_GridSize, m_GridSize);
741 :
742 0 : if (!IS_PASSABLE(m_Grid->get(i0, j0), passClass))
743 : {
744 : // The JPS pathfinder requires units to be on passable tiles
745 : // (otherwise it might crash), so handle the supposedly-invalid
746 : // state specially
747 0 : hierPath.FindNearestPassableNavcell(i0, j0, passClass);
748 : }
749 :
750 0 : state.goal = origGoal;
751 :
752 : // Make the goal reachable. This includes shortening the path if the goal is in a non-passable
753 : // region, transforming non-point goals to reachable point goals, etc.
754 0 : hierPath.MakeGoalReachable(i0, j0, state.goal, passClass);
755 :
756 0 : ENSURE(state.goal.type == PathGoal::POINT);
757 :
758 : // If we're already at the goal tile, then move directly to the exact goal coordinates
759 0 : if (state.goal.NavcellContainsGoal(i0, j0))
760 : {
761 0 : path.m_Waypoints.emplace_back(Waypoint{ state.goal.x, state.goal.z });
762 0 : return;
763 : }
764 :
765 0 : Pathfinding::NearestNavcell(state.goal.x, state.goal.z, state.iGoal, state.jGoal, m_GridSize, m_GridSize);
766 :
767 0 : ENSURE((state.goal.x / Pathfinding::NAVCELL_SIZE).ToInt_RoundToNegInfinity() == state.iGoal);
768 0 : ENSURE((state.goal.z / Pathfinding::NAVCELL_SIZE).ToInt_RoundToNegInfinity() == state.jGoal);
769 :
770 0 : state.passClass = passClass;
771 :
772 0 : state.steps = 0;
773 :
774 0 : state.tiles = new PathfindTileGrid(m_Grid->m_W, m_Grid->m_H);
775 0 : state.terrain = m_Grid;
776 :
777 0 : state.iBest = i0;
778 0 : state.jBest = j0;
779 0 : state.hBest = CalculateHeuristic(i0, j0, state.iGoal, state.jGoal);
780 :
781 0 : PriorityQueue::Item start = { TileID(i0, j0), PathCost() };
782 0 : state.open.push(start);
783 0 : state.tiles->get(i0, j0).SetStatusOpen();
784 0 : state.tiles->get(i0, j0).SetPred(i0, j0, i0, j0);
785 0 : state.tiles->get(i0, j0).SetCost(PathCost());
786 :
787 : while (true)
788 : {
789 0 : ++state.steps;
790 :
791 : // If we ran out of tiles to examine, give up
792 0 : if (state.open.empty())
793 0 : break;
794 :
795 : // Move best tile from open to closed
796 0 : PriorityQueue::Item curr = state.open.pop();
797 0 : u16 i = curr.id.i();
798 0 : u16 j = curr.id.j();
799 0 : state.tiles->get(i, j).SetStatusClosed();
800 :
801 : // If we've reached the destination, stop
802 0 : if (state.goal.NavcellContainsGoal(i, j))
803 : {
804 0 : state.iBest = i;
805 0 : state.jBest = j;
806 0 : state.hBest = PathCost();
807 0 : break;
808 : }
809 :
810 0 : PathfindTile tile = state.tiles->get(i, j);
811 0 : PathCost g = tile.GetCost();
812 :
813 : // Get the direction of the predecessor tile from this tile
814 0 : int dpi = tile.GetPredDI();
815 0 : int dpj = tile.GetPredDJ();
816 0 : dpi = (dpi < 0 ? -1 : dpi > 0 ? 1 : 0);
817 0 : dpj = (dpj < 0 ? -1 : dpj > 0 ? 1 : 0);
818 :
819 0 : if (dpi != 0 && dpj == 0)
820 : {
821 : // Moving horizontally from predecessor
822 0 : if (!PASSABLE(i + dpi, j - 1))
823 : {
824 0 : AddJumpedDiag(i, j, -dpi, -1, g, state);
825 0 : AddJumpedVert(i, j, -1, g, state, OnTheWay(i, j, 0, -1, state.iGoal, state.jGoal));
826 : }
827 0 : if (!PASSABLE(i + dpi, j + 1))
828 : {
829 0 : AddJumpedDiag(i, j, -dpi, +1, g, state);
830 0 : AddJumpedVert(i, j, +1, g, state, OnTheWay(i, j, 0, +1, state.iGoal, state.jGoal));
831 : }
832 0 : AddJumpedHoriz(i, j, -dpi, g, state, OnTheWay(i, j, -dpi, 0, state.iGoal, state.jGoal));
833 : }
834 0 : else if (dpi == 0 && dpj != 0)
835 : {
836 : // Moving vertically from predecessor
837 0 : if (!PASSABLE(i - 1, j + dpj))
838 : {
839 0 : AddJumpedDiag(i, j, -1, -dpj, g, state);
840 0 : AddJumpedHoriz(i, j, -1, g, state,OnTheWay(i, j, -1, 0, state.iGoal, state.jGoal));
841 : }
842 0 : if (!PASSABLE(i + 1, j + dpj))
843 : {
844 0 : AddJumpedDiag(i, j, +1, -dpj, g, state);
845 0 : AddJumpedHoriz(i, j, +1, g, state,OnTheWay(i, j, +1, 0, state.iGoal, state.jGoal));
846 : }
847 0 : AddJumpedVert(i, j, -dpj, g, state, OnTheWay(i, j, 0, -dpj, state.iGoal, state.jGoal));
848 : }
849 0 : else if (dpi != 0 && dpj != 0)
850 : {
851 : // Moving diagonally from predecessor
852 0 : AddJumpedHoriz(i, j, -dpi, g, state, OnTheWay(i, j, -dpi, 0, state.iGoal, state.jGoal));
853 0 : AddJumpedVert(i, j, -dpj, g, state, OnTheWay(i, j, 0, -dpj, state.iGoal, state.jGoal));
854 0 : AddJumpedDiag(i, j, -dpi, -dpj, g, state);
855 : }
856 : else
857 : {
858 : // No predecessor, i.e. the start tile
859 : // Start searching in every direction
860 :
861 : // XXX - check passability?
862 :
863 0 : bool passl = PASSABLE(i - 1, j);
864 0 : bool passr = PASSABLE(i + 1, j);
865 0 : bool passd = PASSABLE(i, j - 1);
866 0 : bool passu = PASSABLE(i, j + 1);
867 :
868 0 : if (passl && passd)
869 0 : ProcessNeighbour(i, j, i-1, j-1, g, state);
870 0 : if (passr && passd)
871 0 : ProcessNeighbour(i, j, i+1, j-1, g, state);
872 0 : if (passl && passu)
873 0 : ProcessNeighbour(i, j, i-1, j+1, g, state);
874 0 : if (passr && passu)
875 0 : ProcessNeighbour(i, j, i+1, j+1, g, state);
876 0 : if (passl)
877 0 : ProcessNeighbour(i, j, i-1, j, g, state);
878 0 : if (passr)
879 0 : ProcessNeighbour(i, j, i+1, j, g, state);
880 0 : if (passd)
881 0 : ProcessNeighbour(i, j, i, j-1, g, state);
882 0 : if (passu)
883 0 : ProcessNeighbour(i, j, i, j+1, g, state);
884 : }
885 0 : }
886 :
887 : // Reconstruct the path (in reverse)
888 0 : u16 ip = state.iBest, jp = state.jBest;
889 0 : while (ip != i0 || jp != j0)
890 : {
891 0 : PathfindTile& n = state.tiles->get(ip, jp);
892 0 : entity_pos_t x, z;
893 0 : Pathfinding::NavcellCenter(ip, jp, x, z);
894 0 : path.m_Waypoints.emplace_back(Waypoint{ x, z });
895 :
896 : // Follow the predecessor link
897 0 : ip = n.GetPredI(ip);
898 0 : jp = n.GetPredJ(jp);
899 : }
900 : // The last waypoint is slightly incorrect (it's not the goal but the center
901 : // of the navcell of the goal), so replace it
902 0 : if (!path.m_Waypoints.empty())
903 0 : path.m_Waypoints.front() = { state.goal.x, state.goal.z };
904 :
905 0 : ImprovePathWaypoints(path, passClass, origGoal.maxdist, x0, z0);
906 :
907 : // Save this grid for debug display
908 0 : if (m_Debug.Overlay)
909 : {
910 0 : std::lock_guard<std::mutex> lock(g_DebugMutex);
911 0 : delete m_Debug.Grid;
912 0 : m_Debug.Grid = state.tiles;
913 0 : m_Debug.Steps = state.steps;
914 0 : m_Debug.Goal = state.goal;
915 : }
916 : else
917 0 : SAFE_DELETE(state.tiles);
918 : }
919 :
920 : #undef PASSABLE
921 :
922 0 : void LongPathfinder::ImprovePathWaypoints(WaypointPath& path, pass_class_t passClass, entity_pos_t maxDist, entity_pos_t x0, entity_pos_t z0) const
923 : {
924 0 : if (path.m_Waypoints.empty())
925 0 : return;
926 :
927 0 : if (maxDist > fixed::Zero())
928 : {
929 0 : CFixedVector2D start(x0, z0);
930 0 : CFixedVector2D first(path.m_Waypoints.back().x, path.m_Waypoints.back().z);
931 0 : CFixedVector2D offset = first - start;
932 0 : if (offset.CompareLength(maxDist) > 0)
933 : {
934 0 : offset.Normalize(maxDist);
935 0 : path.m_Waypoints.emplace_back(Waypoint{ (start + offset).X, (start + offset).Y });
936 : }
937 : }
938 :
939 0 : if (path.m_Waypoints.size() < 2)
940 0 : return;
941 :
942 0 : std::vector<Waypoint>& waypoints = path.m_Waypoints;
943 0 : std::vector<Waypoint> newWaypoints;
944 :
945 0 : CFixedVector2D prev(waypoints.front().x, waypoints.front().z);
946 0 : newWaypoints.push_back(waypoints.front());
947 0 : for (size_t k = 1; k < waypoints.size() - 1; ++k)
948 : {
949 0 : CFixedVector2D ahead(waypoints[k + 1].x, waypoints[k + 1].z);
950 0 : CFixedVector2D curr(waypoints[k].x, waypoints[k].z);
951 :
952 0 : if (maxDist > fixed::Zero() && (curr - prev).CompareLength(maxDist) > 0)
953 : {
954 : // We are too far away from the previous waypoint, so create one in
955 : // between and continue with the improvement of the path
956 0 : prev = prev + (curr - prev) / 2;
957 0 : newWaypoints.emplace_back(Waypoint{ prev.X, prev.Y });
958 : }
959 :
960 : // If we're mostly straight, don't even bother.
961 0 : if ((ahead - curr).Perpendicular().Dot(curr - prev).Absolute() <= fixed::Epsilon() * 100)
962 0 : continue;
963 :
964 0 : if (!Pathfinding::CheckLineMovement(prev.X, prev.Y, ahead.X, ahead.Y, passClass, *m_Grid))
965 : {
966 0 : prev = CFixedVector2D(waypoints[k].x, waypoints[k].z);
967 0 : newWaypoints.push_back(waypoints[k]);
968 : }
969 : }
970 0 : newWaypoints.push_back(waypoints.back());
971 0 : path.m_Waypoints.swap(newWaypoints);
972 : }
973 :
974 0 : void LongPathfinder::GetDebugDataJPS(u32& steps, double& time, Grid<u8>& grid) const
975 : {
976 0 : steps = m_Debug.Steps;
977 0 : time = m_Debug.Time;
978 :
979 0 : if (!m_Debug.Grid)
980 0 : return;
981 :
982 0 : std::lock_guard<std::mutex> lock(g_DebugMutex);
983 :
984 : u16 iGoal, jGoal;
985 0 : Pathfinding::NearestNavcell(m_Debug.Goal.x, m_Debug.Goal.z, iGoal, jGoal, m_GridSize, m_GridSize);
986 :
987 0 : grid = Grid<u8>(m_Debug.Grid->m_W, m_Debug.Grid->m_H);
988 0 : for (u16 j = 0; j < grid.m_H; ++j)
989 : {
990 0 : for (u16 i = 0; i < grid.m_W; ++i)
991 : {
992 0 : if (i == iGoal && j == jGoal)
993 0 : continue;
994 0 : PathfindTile t = m_Debug.Grid->get(i, j);
995 0 : grid.set(i, j, (t.IsOpen() ? 1 : 0) | (t.IsClosed() ? 2 : 0));
996 : }
997 : }
998 : }
999 :
1000 0 : void LongPathfinder::ComputePath(const HierarchicalPathfinder& hierPath, entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal,
1001 : pass_class_t passClass, WaypointPath& path) const
1002 : {
1003 0 : if (!m_Grid)
1004 : {
1005 0 : LOGERROR("The pathfinder grid hasn't been setup yet, aborting ComputeJPSPath");
1006 0 : return;
1007 : }
1008 :
1009 0 : ComputeJPSPath(hierPath, x0, z0, origGoal, passClass, path);
1010 : }
1011 0 : void LongPathfinder::ComputePath(const HierarchicalPathfinder& hierPath, entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal,
1012 : pass_class_t passClass, std::vector<CircularRegion> excludedRegions, WaypointPath& path)
1013 : {
1014 0 : GenerateSpecialMap(passClass, excludedRegions);
1015 0 : ComputeJPSPath(hierPath, x0, z0, origGoal, SPECIAL_PASS_CLASS, path);
1016 0 : }
1017 :
1018 0 : inline bool InRegion(u16 i, u16 j, CircularRegion region)
1019 : {
1020 0 : fixed cellX = Pathfinding::NAVCELL_SIZE * i;
1021 0 : fixed cellZ = Pathfinding::NAVCELL_SIZE * j;
1022 :
1023 0 : return CFixedVector2D(cellX - region.x, cellZ - region.z).CompareLength(region.r) <= 0;
1024 : }
1025 :
1026 0 : void LongPathfinder::GenerateSpecialMap(pass_class_t passClass, std::vector<CircularRegion> excludedRegions)
1027 : {
1028 0 : for (u16 j = 0; j < m_Grid->m_H; ++j)
1029 : {
1030 0 : for (u16 i = 0; i < m_Grid->m_W; ++i)
1031 : {
1032 0 : NavcellData n = m_Grid->get(i, j);
1033 0 : if (!IS_PASSABLE(n, passClass))
1034 : {
1035 0 : n |= SPECIAL_PASS_CLASS;
1036 0 : m_Grid->set(i, j, n);
1037 0 : continue;
1038 : }
1039 :
1040 0 : for (CircularRegion& region : excludedRegions)
1041 : {
1042 0 : if (!InRegion(i, j, region))
1043 0 : continue;
1044 :
1045 0 : n |= SPECIAL_PASS_CLASS;
1046 0 : break;
1047 : }
1048 0 : m_Grid->set(i, j, n);
1049 : }
1050 : }
1051 0 : }
1052 :
1053 : /**
1054 : * Terrain overlay for pathfinder debugging.
1055 : * Renders a representation of the most recent pathfinding operation.
1056 : */
1057 0 : class LongOverlay : public TerrainTextureOverlay
1058 : {
1059 : public:
1060 : LongPathfinder& m_Pathfinder;
1061 :
1062 0 : LongOverlay(LongPathfinder& pathfinder) :
1063 0 : TerrainTextureOverlay(Pathfinding::NAVCELLS_PER_TERRAIN_TILE), m_Pathfinder(pathfinder)
1064 : {
1065 0 : }
1066 :
1067 0 : virtual void BuildTextureRGBA(u8* data, size_t w, size_t h)
1068 : {
1069 : // Grab the debug data for the most recently generated path
1070 : u32 steps;
1071 : double time;
1072 0 : Grid<u8> debugGrid;
1073 0 : m_Pathfinder.GetDebugData(steps, time, debugGrid);
1074 :
1075 : // Render navcell passability
1076 0 : u8* p = data;
1077 0 : for (size_t j = 0; j < h; ++j)
1078 : {
1079 0 : for (size_t i = 0; i < w; ++i)
1080 : {
1081 0 : SColor4ub color(0, 0, 0, 0);
1082 0 : if (!IS_PASSABLE(m_Pathfinder.m_Grid->get((int)i, (int)j), m_Pathfinder.m_Debug.PassClass))
1083 0 : color = SColor4ub(255, 0, 0, 127);
1084 :
1085 0 : if (debugGrid.m_W && debugGrid.m_H)
1086 : {
1087 0 : u8 n = debugGrid.get((int)i, (int)j);
1088 :
1089 0 : if (n == 1)
1090 0 : color = SColor4ub(255, 255, 0, 127);
1091 0 : else if (n == 2)
1092 0 : color = SColor4ub(0, 255, 0, 127);
1093 :
1094 0 : if (m_Pathfinder.m_Debug.Goal.NavcellContainsGoal(i, j))
1095 0 : color = SColor4ub(0, 0, 255, 127);
1096 : }
1097 :
1098 0 : *p++ = color.R;
1099 0 : *p++ = color.G;
1100 0 : *p++ = color.B;
1101 0 : *p++ = color.A;
1102 : }
1103 : }
1104 :
1105 : // Render the most recently generated path
1106 0 : if (m_Pathfinder.m_Debug.Path && !m_Pathfinder.m_Debug.Path->m_Waypoints.empty())
1107 : {
1108 0 : std::vector<Waypoint>& waypoints = m_Pathfinder.m_Debug.Path->m_Waypoints;
1109 0 : u16 ip = 0, jp = 0;
1110 0 : for (size_t k = 0; k < waypoints.size(); ++k)
1111 : {
1112 : u16 i, j;
1113 0 : Pathfinding::NearestNavcell(waypoints[k].x, waypoints[k].z, i, j, m_Pathfinder.m_GridSize, m_Pathfinder.m_GridSize);
1114 0 : if (k == 0)
1115 : {
1116 0 : ip = i;
1117 0 : jp = j;
1118 : }
1119 : else
1120 : {
1121 0 : bool firstCell = true;
1122 0 : do
1123 : {
1124 0 : if (data[(jp*w + ip)*4+3] == 0)
1125 : {
1126 0 : data[(jp*w + ip)*4+0] = 0xFF;
1127 0 : data[(jp*w + ip)*4+1] = 0xFF;
1128 0 : data[(jp*w + ip)*4+2] = 0xFF;
1129 0 : data[(jp*w + ip)*4+3] = firstCell ? 0xA0 : 0x60;
1130 : }
1131 0 : ip = ip < i ? ip+1 : ip > i ? ip-1 : ip;
1132 0 : jp = jp < j ? jp+1 : jp > j ? jp-1 : jp;
1133 0 : firstCell = false;
1134 : }
1135 0 : while (ip != i || jp != j);
1136 : }
1137 : }
1138 : }
1139 0 : }
1140 : };
1141 :
1142 : // These two functions must come below LongOverlay's definition.
1143 3 : void LongPathfinder::SetDebugOverlay(bool enabled)
1144 : {
1145 3 : if (enabled && !m_Debug.Overlay)
1146 0 : m_Debug.Overlay = new LongOverlay(*this);
1147 3 : else if (!enabled && m_Debug.Overlay)
1148 0 : SAFE_DELETE(m_Debug.Overlay);
1149 3 : }
1150 :
1151 6 : LongPathfinder::~LongPathfinder()
1152 : {
1153 3 : SAFE_DELETE(m_Debug.Overlay);
1154 3 : SAFE_DELETE(m_Debug.Grid);
1155 3 : SAFE_DELETE(m_Debug.Path);
1156 3 : }
|
