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 /**
19 * @file
20 * Vertex-based algorithm for CCmpPathfinder.
21 * Computes paths around the corners of rectangular obstructions.
22 *
23 * Useful search term for this algorithm: "points of visibility".
24 *
25 * Since we sometimes want to use this for avoiding moving units, there is no
26 * pre-computation - the whole visibility graph is effectively regenerated for
27 * each path, and it does A* over that graph.
28 *
29 * This scales very poorly in the number of obstructions, so it should be used
30 * with a limited range and not exceedingly frequently.
31 */
32
33 #include "precompiled.h"
34
35 #include "CCmpPathfinder_Common.h"
36
37 #include "lib/timer.h"
38 #include "ps/Profile.h"
39 #include "simulation2/components/ICmpObstructionManager.h"
40 #include "simulation2/helpers/PriorityQueue.h"
41 #include "simulation2/helpers/Render.h"
42
43 /* Quadrant optimisation:
44 * (loosely based on GPG2 "Optimizing Points-of-Visibility Pathfinding")
45 *
46 * Consider the vertex ("@") at a corner of an axis-aligned rectangle ("#"):
47 *
48 * TL : TR
49 * :
50 * ####@ - - -
51 * #####
52 * #####
53 * BL ## BR
54 *
55 * The area around the vertex is split into TopLeft, BottomRight etc quadrants.
56 *
57 * If the shortest path reaches this vertex, it cannot continue to a vertex in
58 * the BL quadrant (it would be blocked by the shape).
59 * Since the shortest path is wrapped tightly around the edges of obstacles,
60 * if the path approached this vertex from the TL quadrant,
61 * it cannot continue to the TL or TR quadrants (the path could be shorter if it
62 * skipped this vertex).
63 * Therefore it must continue to a vertex in the BR quadrant (so this vertex is in
64 * *that* vertex's TL quadrant).
65 *
66 * That lets us significantly reduce the search space by quickly discarding vertexes
67 * from the wrong quadrants.
68 *
69 * (This causes badness if the path starts from inside the shape, so we add some hacks
70 * for that case.)
71 *
72 * (For non-axis-aligned rectangles it's harder to do this computation, so we'll
73 * not bother doing any discarding for those.)
74 */
75 static const u8 QUADRANT_NONE = 0;
76 static const u8 QUADRANT_BL = 1;
77 static const u8 QUADRANT_TR = 2;
78 static const u8 QUADRANT_TL = 4;
79 static const u8 QUADRANT_BR = 8;
80 static const u8 QUADRANT_BLTR = QUADRANT_BL|QUADRANT_TR;
81 static const u8 QUADRANT_TLBR = QUADRANT_TL|QUADRANT_BR;
82 static const u8 QUADRANT_ALL = QUADRANT_BLTR|QUADRANT_TLBR;
83
84 // When computing vertexes to insert into the search graph,
85 // add a small delta so that the vertexes of an edge don't get interpreted
86 // as crossing the edge (given minor numerical inaccuracies)
87 static const entity_pos_t EDGE_EXPAND_DELTA = entity_pos_t::FromInt(1)/16;
88
89 /**
90 * Check whether a ray from 'a' to 'b' crosses any of the edges.
91 * (Edges are one-sided so it's only considered a cross if going from front to back.)
92 */
CheckVisibility(const CFixedVector2D & a,const CFixedVector2D & b,const std::vector<Edge> & edges)93 inline static bool CheckVisibility(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector<Edge>& edges)
94 {
95 CFixedVector2D abn = (b - a).Perpendicular();
96
97 // Edges of general non-axis-aligned shapes
98 for (size_t i = 0; i < edges.size(); ++i)
99 {
100 CFixedVector2D p0 = edges[i].p0;
101 CFixedVector2D p1 = edges[i].p1;
102
103 CFixedVector2D d = (p1 - p0).Perpendicular();
104
105 // If 'a' is behind the edge, we can't cross
106 fixed q = (a - p0).Dot(d);
107 if (q < fixed::Zero())
108 continue;
109
110 // If 'b' is in front of the edge, we can't cross
111 fixed r = (b - p0).Dot(d);
112 if (r > fixed::Zero())
113 continue;
114
115 // The ray is crossing the infinitely-extended edge from in front to behind.
116 // Check the finite edge is crossing the infinitely-extended ray too.
117 // (Given the previous tests, it can only be crossing in one direction.)
118 fixed s = (p0 - a).Dot(abn);
119 if (s > fixed::Zero())
120 continue;
121
122 fixed t = (p1 - a).Dot(abn);
123 if (t < fixed::Zero())
124 continue;
125
126 return false;
127 }
128
129 return true;
130 }
131
132 // Handle the axis-aligned shape edges separately (for performance):
133 // (These are specialised versions of the general unaligned edge code.
134 // They assume the caller has already excluded edges for which 'a' is
135 // on the wrong side.)
136
CheckVisibilityLeft(const CFixedVector2D & a,const CFixedVector2D & b,const std::vector<EdgeAA> & edges)137 inline static bool CheckVisibilityLeft(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector<EdgeAA>& edges)
138 {
139 if (a.X >= b.X)
140 return true;
141
142 CFixedVector2D abn = (b - a).Perpendicular();
143
144 for (size_t i = 0; i < edges.size(); ++i)
145 {
146 if (b.X < edges[i].p0.X)
147 continue;
148
149 CFixedVector2D p0 (edges[i].p0.X, edges[i].c1);
150 fixed s = (p0 - a).Dot(abn);
151 if (s > fixed::Zero())
152 continue;
153
154 CFixedVector2D p1 (edges[i].p0.X, edges[i].p0.Y);
155 fixed t = (p1 - a).Dot(abn);
156 if (t < fixed::Zero())
157 continue;
158
159 return false;
160 }
161
162 return true;
163 }
164
CheckVisibilityRight(const CFixedVector2D & a,const CFixedVector2D & b,const std::vector<EdgeAA> & edges)165 inline static bool CheckVisibilityRight(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector<EdgeAA>& edges)
166 {
167 if (a.X <= b.X)
168 return true;
169
170 CFixedVector2D abn = (b - a).Perpendicular();
171
172 for (size_t i = 0; i < edges.size(); ++i)
173 {
174 if (b.X > edges[i].p0.X)
175 continue;
176
177 CFixedVector2D p0 (edges[i].p0.X, edges[i].c1);
178 fixed s = (p0 - a).Dot(abn);
179 if (s > fixed::Zero())
180 continue;
181
182 CFixedVector2D p1 (edges[i].p0.X, edges[i].p0.Y);
183 fixed t = (p1 - a).Dot(abn);
184 if (t < fixed::Zero())
185 continue;
186
187 return false;
188 }
189
190 return true;
191 }
192
CheckVisibilityBottom(const CFixedVector2D & a,const CFixedVector2D & b,const std::vector<EdgeAA> & edges)193 inline static bool CheckVisibilityBottom(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector<EdgeAA>& edges)
194 {
195 if (a.Y >= b.Y)
196 return true;
197
198 CFixedVector2D abn = (b - a).Perpendicular();
199
200 for (size_t i = 0; i < edges.size(); ++i)
201 {
202 if (b.Y < edges[i].p0.Y)
203 continue;
204
205 CFixedVector2D p0 (edges[i].p0.X, edges[i].p0.Y);
206 fixed s = (p0 - a).Dot(abn);
207 if (s > fixed::Zero())
208 continue;
209
210 CFixedVector2D p1 (edges[i].c1, edges[i].p0.Y);
211 fixed t = (p1 - a).Dot(abn);
212 if (t < fixed::Zero())
213 continue;
214
215 return false;
216 }
217
218 return true;
219 }
220
CheckVisibilityTop(const CFixedVector2D & a,const CFixedVector2D & b,const std::vector<EdgeAA> & edges)221 inline static bool CheckVisibilityTop(const CFixedVector2D& a, const CFixedVector2D& b, const std::vector<EdgeAA>& edges)
222 {
223 if (a.Y <= b.Y)
224 return true;
225
226 CFixedVector2D abn = (b - a).Perpendicular();
227
228 for (size_t i = 0; i < edges.size(); ++i)
229 {
230 if (b.Y > edges[i].p0.Y)
231 continue;
232
233 CFixedVector2D p0 (edges[i].p0.X, edges[i].p0.Y);
234 fixed s = (p0 - a).Dot(abn);
235 if (s > fixed::Zero())
236 continue;
237
238 CFixedVector2D p1 (edges[i].c1, edges[i].p0.Y);
239 fixed t = (p1 - a).Dot(abn);
240 if (t < fixed::Zero())
241 continue;
242
243 return false;
244 }
245
246 return true;
247 }
248
249 typedef PriorityQueueHeap<u16, fixed, fixed> VertexPriorityQueue;
250
251 /**
252 * Add edges and vertexes to represent the boundaries between passable and impassable
253 * navcells (for impassable terrain).
254 * Navcells i0 <= i <= i1, j0 <= j <= j1 will be considered.
255 */
AddTerrainEdges(std::vector<Edge> & edges,std::vector<Vertex> & vertexes,int i0,int j0,int i1,int j1,pass_class_t passClass,const Grid<NavcellData> & grid)256 static void AddTerrainEdges(std::vector<Edge>& edges, std::vector<Vertex>& vertexes,
257 int i0, int j0, int i1, int j1,
258 pass_class_t passClass, const Grid<NavcellData>& grid)
259 {
260 PROFILE("AddTerrainEdges");
261
262 // Clamp the coordinates so we won't attempt to sample outside of the grid.
263 // (This assumes the outermost ring of navcells (which are always impassable)
264 // won't have a boundary with any passable navcells. TODO: is that definitely
265 // safe enough?)
266
267 i0 = clamp(i0, 1, grid.m_W-2);
268 j0 = clamp(j0, 1, grid.m_H-2);
269 i1 = clamp(i1, 1, grid.m_W-2);
270 j1 = clamp(j1, 1, grid.m_H-2);
271
272 for (int j = j0; j <= j1; ++j)
273 {
274 for (int i = i0; i <= i1; ++i)
275 {
276 if (IS_PASSABLE(grid.get(i, j), passClass))
277 continue;
278
279 if (IS_PASSABLE(grid.get(i+1, j), passClass) && IS_PASSABLE(grid.get(i, j+1), passClass) && IS_PASSABLE(grid.get(i+1, j+1), passClass))
280 {
281 Vertex vert;
282 vert.status = Vertex::UNEXPLORED;
283 vert.quadOutward = QUADRANT_ALL;
284 vert.quadInward = QUADRANT_BL;
285 vert.p = CFixedVector2D(fixed::FromInt(i+1)+EDGE_EXPAND_DELTA, fixed::FromInt(j+1)+EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE);
286 vertexes.push_back(vert);
287 }
288
289 if (IS_PASSABLE(grid.get(i-1, j), passClass) && IS_PASSABLE(grid.get(i, j+1), passClass) && IS_PASSABLE(grid.get(i-1, j+1), passClass))
290 {
291 Vertex vert;
292 vert.status = Vertex::UNEXPLORED;
293 vert.quadOutward = QUADRANT_ALL;
294 vert.quadInward = QUADRANT_BR;
295 vert.p = CFixedVector2D(fixed::FromInt(i)-EDGE_EXPAND_DELTA, fixed::FromInt(j+1)+EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE);
296 vertexes.push_back(vert);
297 }
298
299 if (IS_PASSABLE(grid.get(i+1, j), passClass) && IS_PASSABLE(grid.get(i, j-1), passClass) && IS_PASSABLE(grid.get(i+1, j-1), passClass))
300 {
301 Vertex vert;
302 vert.status = Vertex::UNEXPLORED;
303 vert.quadOutward = QUADRANT_ALL;
304 vert.quadInward = QUADRANT_TL;
305 vert.p = CFixedVector2D(fixed::FromInt(i+1)+EDGE_EXPAND_DELTA, fixed::FromInt(j)-EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE);
306 vertexes.push_back(vert);
307 }
308
309 if (IS_PASSABLE(grid.get(i-1, j), passClass) && IS_PASSABLE(grid.get(i, j-1), passClass) && IS_PASSABLE(grid.get(i-1, j-1), passClass))
310 {
311 Vertex vert;
312 vert.status = Vertex::UNEXPLORED;
313 vert.quadOutward = QUADRANT_ALL;
314 vert.quadInward = QUADRANT_TR;
315 vert.p = CFixedVector2D(fixed::FromInt(i)-EDGE_EXPAND_DELTA, fixed::FromInt(j)-EDGE_EXPAND_DELTA).Multiply(Pathfinding::NAVCELL_SIZE);
316 vertexes.push_back(vert);
317 }
318 }
319 }
320
321 // XXX rewrite this stuff
322 std::vector<u16> segmentsR;
323 std::vector<u16> segmentsL;
324 for (int j = j0; j < j1; ++j)
325 {
326 segmentsR.clear();
327 segmentsL.clear();
328 for (int i = i0; i <= i1; ++i)
329 {
330 bool a = IS_PASSABLE(grid.get(i, j+1), passClass);
331 bool b = IS_PASSABLE(grid.get(i, j), passClass);
332 if (a && !b)
333 segmentsL.push_back(i);
334 if (b && !a)
335 segmentsR.push_back(i);
336 }
337
338 if (!segmentsR.empty())
339 {
340 segmentsR.push_back(0); // sentinel value to simplify the loop
341 u16 ia = segmentsR[0];
342 u16 ib = ia + 1;
343 for (size_t n = 1; n < segmentsR.size(); ++n)
344 {
345 if (segmentsR[n] == ib)
346 ++ib;
347 else
348 {
349 CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(ia), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE);
350 CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(ib), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE);
351 edges.emplace_back(Edge{ v0, v1 });
352
353 ia = segmentsR[n];
354 ib = ia + 1;
355 }
356 }
357 }
358
359 if (!segmentsL.empty())
360 {
361 segmentsL.push_back(0); // sentinel value to simplify the loop
362 u16 ia = segmentsL[0];
363 u16 ib = ia + 1;
364 for (size_t n = 1; n < segmentsL.size(); ++n)
365 {
366 if (segmentsL[n] == ib)
367 ++ib;
368 else
369 {
370 CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(ib), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE);
371 CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(ia), fixed::FromInt(j+1)).Multiply(Pathfinding::NAVCELL_SIZE);
372 edges.emplace_back(Edge{ v0, v1 });
373
374 ia = segmentsL[n];
375 ib = ia + 1;
376 }
377 }
378 }
379 }
380 std::vector<u16> segmentsU;
381 std::vector<u16> segmentsD;
382 for (int i = i0; i < i1; ++i)
383 {
384 segmentsU.clear();
385 segmentsD.clear();
386 for (int j = j0; j <= j1; ++j)
387 {
388 bool a = IS_PASSABLE(grid.get(i+1, j), passClass);
389 bool b = IS_PASSABLE(grid.get(i, j), passClass);
390 if (a && !b)
391 segmentsU.push_back(j);
392 if (b && !a)
393 segmentsD.push_back(j);
394 }
395
396 if (!segmentsU.empty())
397 {
398 segmentsU.push_back(0); // sentinel value to simplify the loop
399 u16 ja = segmentsU[0];
400 u16 jb = ja + 1;
401 for (size_t n = 1; n < segmentsU.size(); ++n)
402 {
403 if (segmentsU[n] == jb)
404 ++jb;
405 else
406 {
407 CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(ja)).Multiply(Pathfinding::NAVCELL_SIZE);
408 CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(jb)).Multiply(Pathfinding::NAVCELL_SIZE);
409 edges.emplace_back(Edge{ v0, v1 });
410
411 ja = segmentsU[n];
412 jb = ja + 1;
413 }
414 }
415 }
416
417 if (!segmentsD.empty())
418 {
419 segmentsD.push_back(0); // sentinel value to simplify the loop
420 u16 ja = segmentsD[0];
421 u16 jb = ja + 1;
422 for (size_t n = 1; n < segmentsD.size(); ++n)
423 {
424 if (segmentsD[n] == jb)
425 ++jb;
426 else
427 {
428 CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(jb)).Multiply(Pathfinding::NAVCELL_SIZE);
429 CFixedVector2D v1 = CFixedVector2D(fixed::FromInt(i+1), fixed::FromInt(ja)).Multiply(Pathfinding::NAVCELL_SIZE);
430 edges.emplace_back(Edge{ v0, v1 });
431
432 ja = segmentsD[n];
433 jb = ja + 1;
434 }
435 }
436 }
437 }
438 }
439
SplitAAEdges(const CFixedVector2D & a,const std::vector<Edge> & edges,const std::vector<Square> & squares,std::vector<Edge> & edgesUnaligned,std::vector<EdgeAA> & edgesLeft,std::vector<EdgeAA> & edgesRight,std::vector<EdgeAA> & edgesBottom,std::vector<EdgeAA> & edgesTop)440 static void SplitAAEdges(const CFixedVector2D& a,
441 const std::vector<Edge>& edges,
442 const std::vector<Square>& squares,
443 std::vector<Edge>& edgesUnaligned,
444 std::vector<EdgeAA>& edgesLeft, std::vector<EdgeAA>& edgesRight,
445 std::vector<EdgeAA>& edgesBottom, std::vector<EdgeAA>& edgesTop)
446 {
447
448 for (const Square& square : squares)
449 {
450 if (a.X <= square.p0.X)
451 edgesLeft.emplace_back(EdgeAA{ square.p0, square.p1.Y });
452 if (a.X >= square.p1.X)
453 edgesRight.emplace_back(EdgeAA{ square.p1, square.p0.Y });
454 if (a.Y <= square.p0.Y)
455 edgesBottom.emplace_back(EdgeAA{ square.p0, square.p1.X });
456 if (a.Y >= square.p1.Y)
457 edgesTop.emplace_back(EdgeAA{ square.p1, square.p0.X });
458 }
459
460 for (const Edge& edge : edges)
461 {
462 if (edge.p0.X == edge.p1.X)
463 {
464 if (edge.p1.Y < edge.p0.Y)
465 {
466 if (!(a.X <= edge.p0.X))
467 continue;
468 edgesLeft.emplace_back(EdgeAA{ edge.p1, edge.p0.Y });
469 }
470 else
471 {
472 if (!(a.X >= edge.p0.X))
473 continue;
474 edgesRight.emplace_back(EdgeAA{ edge.p1, edge.p0.Y });
475 }
476 }
477 else if (edge.p0.Y == edge.p1.Y)
478 {
479 if (edge.p0.X < edge.p1.X)
480 {
481 if (!(a.Y <= edge.p0.Y))
482 continue;
483 edgesBottom.emplace_back(EdgeAA{ edge.p0, edge.p1.X });
484 }
485 else
486 {
487 if (!(a.Y >= edge.p0.Y))
488 continue;
489 edgesTop.emplace_back(EdgeAA{ edge.p0, edge.p1.X });
490 }
491 }
492 else
493 edgesUnaligned.push_back(edge);
494 }
495 }
496
497 /**
498 * Functor for sorting edge-squares by approximate proximity to a fixed point.
499 */
500 struct SquareSort
501 {
502 CFixedVector2D src;
SquareSortSquareSort503 SquareSort(CFixedVector2D src) : src(src) { }
operator ()SquareSort504 bool operator()(const Square& a, const Square& b) const
505 {
506 if ((a.p0 - src).CompareLength(b.p0 - src) < 0)
507 return true;
508 return false;
509 }
510 };
511
ComputeShortPath(const IObstructionTestFilter & filter,entity_pos_t x0,entity_pos_t z0,entity_pos_t clearance,entity_pos_t range,const PathGoal & goal,pass_class_t passClass,WaypointPath & path)512 void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
513 entity_pos_t x0, entity_pos_t z0, entity_pos_t clearance,
514 entity_pos_t range, const PathGoal& goal, pass_class_t passClass, WaypointPath& path)
515 {
516 PROFILE("ComputeShortPath");
517 m_DebugOverlayShortPathLines.clear();
518
519 if (m_DebugOverlay)
520 {
521 // Render the goal shape
522 m_DebugOverlayShortPathLines.push_back(SOverlayLine());
523 m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 0, 0, 1);
524 switch (goal.type)
525 {
526 case PathGoal::POINT:
527 {
528 SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), 0.2f, m_DebugOverlayShortPathLines.back(), true);
529 break;
530 }
531 case PathGoal::CIRCLE:
532 case PathGoal::INVERTED_CIRCLE:
533 {
534 SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat(), m_DebugOverlayShortPathLines.back(), true);
535 break;
536 }
537 case PathGoal::SQUARE:
538 case PathGoal::INVERTED_SQUARE:
539 {
540 float a = atan2f(goal.v.X.ToFloat(), goal.v.Y.ToFloat());
541 SimRender::ConstructSquareOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat()*2, goal.hh.ToFloat()*2, a, m_DebugOverlayShortPathLines.back(), true);
542 break;
543 }
544 }
545 }
546
547 // List of collision edges - paths must never cross these.
548 // (Edges are one-sided so intersections are fine in one direction, but not the other direction.)
549 edges.clear();
550 edgeSquares.clear(); // axis-aligned squares; equivalent to 4 edges
551
552 // Create impassable edges at the max-range boundary, so we can't escape the region
553 // where we're meant to be searching
554 fixed rangeXMin = x0 - range;
555 fixed rangeXMax = x0 + range;
556 fixed rangeZMin = z0 - range;
557 fixed rangeZMax = z0 + range;
558
559 // we don't actually add the "search space" edges as edges, since we may want to cross them
560 // in some cases (such as if we need to go around an obstruction that's partly out of the search range)
561
562 // List of obstruction vertexes (plus start/end points); we'll try to find paths through
563 // the graph defined by these vertexes
564 vertexes.clear();
565
566 // Add the start point to the graph
567 CFixedVector2D posStart(x0, z0);
568 fixed hStart = (posStart - goal.NearestPointOnGoal(posStart)).Length();
569 Vertex start = { posStart, fixed::Zero(), hStart, 0, Vertex::OPEN, QUADRANT_NONE, QUADRANT_ALL };
570 vertexes.push_back(start);
571 const size_t START_VERTEX_ID = 0;
572
573 // Add the goal vertex to the graph.
574 // Since the goal isn't always a point, this a special magic virtual vertex which moves around - whenever
575 // we look at it from another vertex, it is moved to be the closest point on the goal shape to that vertex.
576 Vertex end = { CFixedVector2D(goal.x, goal.z), fixed::Zero(), fixed::Zero(), 0, Vertex::UNEXPLORED, QUADRANT_NONE, QUADRANT_ALL };
577 vertexes.push_back(end);
578 const size_t GOAL_VERTEX_ID = 1;
579
580 // Find all the obstruction squares that might affect us
581 CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSystemEntity());
582 std::vector<ICmpObstructionManager::ObstructionSquare> squares;
583 size_t staticShapesNb = 0;
584 cmpObstructionManager->GetStaticObstructionsInRange(filter, rangeXMin - clearance, rangeZMin - clearance, rangeXMax + clearance, rangeZMax + clearance, squares);
585 staticShapesNb = squares.size();
586 cmpObstructionManager->GetUnitObstructionsInRange(filter, rangeXMin - clearance, rangeZMin - clearance, rangeXMax + clearance, rangeZMax + clearance, squares);
587
588 // Change array capacities to reduce reallocations
589 vertexes.reserve(vertexes.size() + squares.size()*4);
590 edgeSquares.reserve(edgeSquares.size() + squares.size()); // (assume most squares are AA)
591
592 entity_pos_t pathfindClearance = clearance;
593
594 // Convert each obstruction square into collision edges and search graph vertexes
595 for (size_t i = 0; i < squares.size(); ++i)
596 {
597 CFixedVector2D center(squares[i].x, squares[i].z);
598 CFixedVector2D u = squares[i].u;
599 CFixedVector2D v = squares[i].v;
600
601 if (i >= staticShapesNb)
602 pathfindClearance = clearance - entity_pos_t::FromInt(1)/2;
603
604 // Expand the vertexes by the moving unit's collision radius, to find the
605 // closest we can get to it
606
607 CFixedVector2D hd0(squares[i].hw + pathfindClearance + EDGE_EXPAND_DELTA, squares[i].hh + pathfindClearance + EDGE_EXPAND_DELTA);
608 CFixedVector2D hd1(squares[i].hw + pathfindClearance + EDGE_EXPAND_DELTA, -(squares[i].hh + pathfindClearance + EDGE_EXPAND_DELTA));
609
610 // Check whether this is an axis-aligned square
611 bool aa = (u.X == fixed::FromInt(1) && u.Y == fixed::Zero() && v.X == fixed::Zero() && v.Y == fixed::FromInt(1));
612
613 Vertex vert;
614 vert.status = Vertex::UNEXPLORED;
615 vert.quadInward = QUADRANT_NONE;
616 vert.quadOutward = QUADRANT_ALL;
617 vert.p.X = center.X - hd0.Dot(u); vert.p.Y = center.Y + hd0.Dot(v); if (aa) vert.quadInward = QUADRANT_BR; vertexes.push_back(vert);
618 if (vert.p.X < rangeXMin) rangeXMin = vert.p.X;
619 if (vert.p.Y < rangeZMin) rangeZMin = vert.p.Y;
620 if (vert.p.X > rangeXMax) rangeXMax = vert.p.X;
621 if (vert.p.Y > rangeZMax) rangeZMax = vert.p.Y;
622 vert.p.X = center.X - hd1.Dot(u); vert.p.Y = center.Y + hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_TR; vertexes.push_back(vert);
623 if (vert.p.X < rangeXMin) rangeXMin = vert.p.X;
624 if (vert.p.Y < rangeZMin) rangeZMin = vert.p.Y;
625 if (vert.p.X > rangeXMax) rangeXMax = vert.p.X;
626 if (vert.p.Y > rangeZMax) rangeZMax = vert.p.Y;
627 vert.p.X = center.X + hd0.Dot(u); vert.p.Y = center.Y - hd0.Dot(v); if (aa) vert.quadInward = QUADRANT_TL; vertexes.push_back(vert);
628 if (vert.p.X < rangeXMin) rangeXMin = vert.p.X;
629 if (vert.p.Y < rangeZMin) rangeZMin = vert.p.Y;
630 if (vert.p.X > rangeXMax) rangeXMax = vert.p.X;
631 if (vert.p.Y > rangeZMax) rangeZMax = vert.p.Y;
632 vert.p.X = center.X + hd1.Dot(u); vert.p.Y = center.Y - hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_BL; vertexes.push_back(vert);
633 if (vert.p.X < rangeXMin) rangeXMin = vert.p.X;
634 if (vert.p.Y < rangeZMin) rangeZMin = vert.p.Y;
635 if (vert.p.X > rangeXMax) rangeXMax = vert.p.X;
636 if (vert.p.Y > rangeZMax) rangeZMax = vert.p.Y;
637
638 // Add the edges:
639
640 CFixedVector2D h0(squares[i].hw + pathfindClearance, squares[i].hh + pathfindClearance);
641 CFixedVector2D h1(squares[i].hw + pathfindClearance, -(squares[i].hh + pathfindClearance));
642
643 CFixedVector2D ev0(center.X - h0.Dot(u), center.Y + h0.Dot(v));
644 CFixedVector2D ev1(center.X - h1.Dot(u), center.Y + h1.Dot(v));
645 CFixedVector2D ev2(center.X + h0.Dot(u), center.Y - h0.Dot(v));
646 CFixedVector2D ev3(center.X + h1.Dot(u), center.Y - h1.Dot(v));
647 if (aa)
648 edgeSquares.emplace_back(Square{ ev1, ev3 });
649 else
650 {
651 edges.emplace_back(Edge{ ev0, ev1 });
652 edges.emplace_back(Edge{ ev1, ev2 });
653 edges.emplace_back(Edge{ ev2, ev3 });
654 edges.emplace_back(Edge{ ev3, ev0 });
655 }
656
657 // TODO: should clip out vertexes and edges that are outside the range,
658 // to reduce the search space
659 }
660
661 // Add terrain obstructions
662 {
663 u16 i0, j0, i1, j1;
664 Pathfinding::NearestNavcell(rangeXMin, rangeZMin, i0, j0, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE);
665 Pathfinding::NearestNavcell(rangeXMax, rangeZMax, i1, j1, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE);
666 AddTerrainEdges(edges, vertexes, i0, j0, i1, j1, passClass, *m_TerrainOnlyGrid);
667 }
668
669 // Clip out vertices that are inside an edgeSquare (i.e. trivially unreachable)
670 for (size_t i = 0; i < edgeSquares.size(); ++i)
671 {
672 // If the start point is inside the square, ignore it
673 if (start.p.X >= edgeSquares[i].p0.X &&
674 start.p.Y >= edgeSquares[i].p0.Y &&
675 start.p.X <= edgeSquares[i].p1.X &&
676 start.p.Y <= edgeSquares[i].p1.Y)
677 continue;
678
679 // Remove every non-start/goal vertex that is inside an edgeSquare;
680 // since remove() would be inefficient, just mark it as closed instead.
681 for (size_t j = 2; j < vertexes.size(); ++j)
682 if (vertexes[j].p.X >= edgeSquares[i].p0.X &&
683 vertexes[j].p.Y >= edgeSquares[i].p0.Y &&
684 vertexes[j].p.X <= edgeSquares[i].p1.X &&
685 vertexes[j].p.Y <= edgeSquares[i].p1.Y)
686 vertexes[j].status = Vertex::CLOSED;
687 }
688
689 ENSURE(vertexes.size() < 65536); // we store array indexes as u16
690
691 // Render the debug overlay
692 if (m_DebugOverlay)
693 {
694 #define PUSH_POINT(p) STMT(xz.push_back(p.X.ToFloat()); xz.push_back(p.Y.ToFloat()))
695 // Render the vertexes as little Pac-Man shapes to indicate quadrant direction
696 for (size_t i = 0; i < vertexes.size(); ++i)
697 {
698 m_DebugOverlayShortPathLines.emplace_back();
699 m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 1, 0, 1);
700
701 float x = vertexes[i].p.X.ToFloat();
702 float z = vertexes[i].p.Y.ToFloat();
703
704 float a0 = 0, a1 = 0;
705 // Get arc start/end angles depending on quadrant (if any)
706 if (vertexes[i].quadInward == QUADRANT_BL) { a0 = -0.25f; a1 = 0.50f; }
707 else if (vertexes[i].quadInward == QUADRANT_TR) { a0 = 0.25f; a1 = 1.00f; }
708 else if (vertexes[i].quadInward == QUADRANT_TL) { a0 = -0.50f; a1 = 0.25f; }
709 else if (vertexes[i].quadInward == QUADRANT_BR) { a0 = 0.00f; a1 = 0.75f; }
710
711 if (a0 == a1)
712 SimRender::ConstructCircleOnGround(GetSimContext(), x, z, 0.5f,
713 m_DebugOverlayShortPathLines.back(), true);
714 else
715 SimRender::ConstructClosedArcOnGround(GetSimContext(), x, z, 0.5f,
716 a0 * ((float)M_PI*2.0f), a1 * ((float)M_PI*2.0f),
717 m_DebugOverlayShortPathLines.back(), true);
718 }
719
720 // Render the edges
721 for (size_t i = 0; i < edges.size(); ++i)
722 {
723 m_DebugOverlayShortPathLines.emplace_back();
724 m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
725 std::vector<float> xz;
726 PUSH_POINT(edges[i].p0);
727 PUSH_POINT(edges[i].p1);
728
729 // Add an arrowhead to indicate the direction
730 CFixedVector2D d = edges[i].p1 - edges[i].p0;
731 d.Normalize(fixed::FromInt(1)/8);
732 CFixedVector2D p2 = edges[i].p1 - d*2;
733 CFixedVector2D p3 = p2 + d.Perpendicular();
734 CFixedVector2D p4 = p2 - d.Perpendicular();
735 PUSH_POINT(p3);
736 PUSH_POINT(p4);
737 PUSH_POINT(edges[i].p1);
738
739 SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
740 }
741 #undef PUSH_POINT
742
743 // Render the axis-aligned squares
744 for (size_t i = 0; i < edgeSquares.size(); ++i)
745 {
746 m_DebugOverlayShortPathLines.push_back(SOverlayLine());
747 m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
748 std::vector<float> xz;
749 Square s = edgeSquares[i];
750 xz.push_back(s.p0.X.ToFloat());
751 xz.push_back(s.p0.Y.ToFloat());
752 xz.push_back(s.p0.X.ToFloat());
753 xz.push_back(s.p1.Y.ToFloat());
754 xz.push_back(s.p1.X.ToFloat());
755 xz.push_back(s.p1.Y.ToFloat());
756 xz.push_back(s.p1.X.ToFloat());
757 xz.push_back(s.p0.Y.ToFloat());
758 xz.push_back(s.p0.X.ToFloat());
759 xz.push_back(s.p0.Y.ToFloat());
760 SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
761 }
762 }
763
764 // Do an A* search over the vertex/visibility graph:
765
766 // Since we are just measuring Euclidean distance the heuristic is admissible,
767 // so we never have to re-examine a node once it's been moved to the closed set.
768
769 // To save time in common cases, we don't precompute a graph of valid edges between vertexes;
770 // we do it lazily instead. When the search algorithm reaches a vertex, we examine every other
771 // vertex and see if we can reach it without hitting any collision edges, and ignore the ones
772 // we can't reach. Since the algorithm can only reach a vertex once (and then it'll be marked
773 // as closed), we won't be doing any redundant visibility computations.
774
775 PROFILE_START("Short pathfinding - A*");
776
777 VertexPriorityQueue open;
778 VertexPriorityQueue::Item qiStart = { START_VERTEX_ID, start.h, start.h };
779 open.push(qiStart);
780
781 u16 idBest = START_VERTEX_ID;
782 fixed hBest = start.h;
783
784 while (!open.empty())
785 {
786 // Move best tile from open to closed
787 VertexPriorityQueue::Item curr = open.pop();
788 vertexes[curr.id].status = Vertex::CLOSED;
789
790 // If we've reached the destination, stop
791 if (curr.id == GOAL_VERTEX_ID)
792 {
793 idBest = curr.id;
794 break;
795 }
796
797 // Sort the edges by distance in order to check those first that have a high probability of blocking a ray.
798 // The heuristic based on distance is very rough, especially for squares that are further away;
799 // we're also only really interested in the closest squares since they are the only ones that block a lot of rays.
800 // Thus we only do a partial sort; the threshold is just a somewhat reasonable value.
801 if (edgeSquares.size() > 8)
802 std::partial_sort(edgeSquares.begin(), edgeSquares.begin() + 8, edgeSquares.end(), SquareSort(vertexes[curr.id].p));
803
804 edgesUnaligned.clear();
805 edgesLeft.clear();
806 edgesRight.clear();
807 edgesBottom.clear();
808 edgesTop.clear();
809 SplitAAEdges(vertexes[curr.id].p, edges, edgeSquares, edgesUnaligned, edgesLeft, edgesRight, edgesBottom, edgesTop);
810
811 // Check the lines to every other vertex
812 for (size_t n = 0; n < vertexes.size(); ++n)
813 {
814 if (vertexes[n].status == Vertex::CLOSED)
815 continue;
816
817 // If this is the magical goal vertex, move it to near the current vertex
818 CFixedVector2D npos;
819 if (n == GOAL_VERTEX_ID)
820 {
821 npos = goal.NearestPointOnGoal(vertexes[curr.id].p);
822
823 // To prevent integer overflows later on, we need to ensure all vertexes are
824 // 'close' to the source. The goal might be far away (not a good idea but
825 // sometimes it happens), so clamp it to the current search range
826 npos.X = clamp(npos.X, rangeXMin, rangeXMax);
827 npos.Y = clamp(npos.Y, rangeZMin, rangeZMax);
828 }
829 else
830 npos = vertexes[n].p;
831
832 // Work out which quadrant(s) we're approaching the new vertex from
833 u8 quad = 0;
834 if (vertexes[curr.id].p.X <= npos.X && vertexes[curr.id].p.Y <= npos.Y) quad |= QUADRANT_BL;
835 if (vertexes[curr.id].p.X >= npos.X && vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TR;
836 if (vertexes[curr.id].p.X <= npos.X && vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TL;
837 if (vertexes[curr.id].p.X >= npos.X && vertexes[curr.id].p.Y <= npos.Y) quad |= QUADRANT_BR;
838
839 // Check that the new vertex is in the right quadrant for the old vertex
840 if (!(vertexes[curr.id].quadOutward & quad))
841 {
842 // Hack: Always head towards the goal if possible, to avoid missing it if it's
843 // inside another unit
844 if (n != GOAL_VERTEX_ID)
845 continue;
846 }
847
848 bool visible =
849 CheckVisibilityLeft(vertexes[curr.id].p, npos, edgesLeft) &&
850 CheckVisibilityRight(vertexes[curr.id].p, npos, edgesRight) &&
851 CheckVisibilityBottom(vertexes[curr.id].p, npos, edgesBottom) &&
852 CheckVisibilityTop(vertexes[curr.id].p, npos, edgesTop) &&
853 CheckVisibility(vertexes[curr.id].p, npos, edgesUnaligned);
854
855 /*
856 // Render the edges that we examine
857 m_DebugOverlayShortPathLines.push_back(SOverlayLine());
858 m_DebugOverlayShortPathLines.back().m_Color = visible ? CColor(0, 1, 0, 0.5) : CColor(1, 0, 0, 0.5);
859 std::vector<float> xz;
860 xz.push_back(vertexes[curr.id].p.X.ToFloat());
861 xz.push_back(vertexes[curr.id].p.Y.ToFloat());
862 xz.push_back(npos.X.ToFloat());
863 xz.push_back(npos.Y.ToFloat());
864 SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), false);
865 */
866
867 if (visible)
868 {
869 fixed g = vertexes[curr.id].g + (vertexes[curr.id].p - npos).Length();
870
871 // If this is a new tile, compute the heuristic distance
872 if (vertexes[n].status == Vertex::UNEXPLORED)
873 {
874 // Add it to the open list:
875 vertexes[n].status = Vertex::OPEN;
876 vertexes[n].g = g;
877 vertexes[n].h = goal.DistanceToPoint(npos);
878 vertexes[n].pred = curr.id;
879
880 // If this is an axis-aligned shape, the path must continue in the same quadrant
881 // direction (but not go into the inside of the shape).
882 // Hack: If we started *inside* a shape then perhaps headed to its corner (e.g. the unit
883 // was very near another unit), don't restrict further pathing.
884 if (vertexes[n].quadInward && !(curr.id == START_VERTEX_ID && g < fixed::FromInt(8)))
885 vertexes[n].quadOutward = ((~vertexes[n].quadInward) & quad) & 0xF;
886
887 if (n == GOAL_VERTEX_ID)
888 vertexes[n].p = npos; // remember the new best goal position
889
890 VertexPriorityQueue::Item t = { (u16)n, g + vertexes[n].h, vertexes[n].h };
891 open.push(t);
892
893 // Remember the heuristically best vertex we've seen so far, in case we never actually reach the target
894 if (vertexes[n].h < hBest)
895 {
896 idBest = (u16)n;
897 hBest = vertexes[n].h;
898 }
899 }
900 else // must be OPEN
901 {
902 // If we've already seen this tile, and the new path to this tile does not have a
903 // better cost, then stop now
904 if (g >= vertexes[n].g)
905 continue;
906
907 // Otherwise, we have a better path, so replace the old one with the new cost/parent
908 fixed gprev = vertexes[n].g;
909 vertexes[n].g = g;
910 vertexes[n].pred = curr.id;
911
912 // If this is an axis-aligned shape, the path must continue in the same quadrant
913 // direction (but not go into the inside of the shape).
914 if (vertexes[n].quadInward)
915 vertexes[n].quadOutward = ((~vertexes[n].quadInward) & quad) & 0xF;
916
917 if (n == GOAL_VERTEX_ID)
918 vertexes[n].p = npos; // remember the new best goal position
919
920 open.promote((u16)n, gprev + vertexes[n].h, g + vertexes[n].h, vertexes[n].h);
921 }
922 }
923 }
924 }
925
926 // Reconstruct the path (in reverse)
927 for (u16 id = idBest; id != START_VERTEX_ID; id = vertexes[id].pred)
928 path.m_Waypoints.emplace_back(Waypoint{ vertexes[id].p.X, vertexes[id].p.Y });
929
930 PROFILE_END("Short pathfinding - A*");
931 }
932