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