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 "HierarchicalPathfinder.h"
21 :
22 : #include "graphics/Overlay.h"
23 : #include "ps/Profile.h"
24 : #include "renderer/Scene.h"
25 :
26 : #include "simulation2/helpers/Grid.h"
27 :
28 : // Find the root ID of a region, used by InitRegions
29 86 : inline u16 RootID(u16 x, const std::vector<u16>& v)
30 : {
31 112 : while (v[x] < x)
32 26 : x = v[x];
33 :
34 60 : return x;
35 : }
36 :
37 117 : void HierarchicalPathfinder::Chunk::InitRegions(int ci, int cj, Grid<NavcellData>* grid, pass_class_t passClass)
38 : {
39 117 : ENSURE(ci < 256 && cj < 256); // avoid overflows
40 117 : m_ChunkI = ci;
41 117 : m_ChunkJ = cj;
42 :
43 117 : memset(m_Regions, 0, sizeof(m_Regions));
44 :
45 117 : int i0 = ci * CHUNK_SIZE;
46 117 : int j0 = cj * CHUNK_SIZE;
47 117 : int i1 = std::min(i0 + CHUNK_SIZE, (int)grid->m_W);
48 117 : int j1 = std::min(j0 + CHUNK_SIZE, (int)grid->m_H);
49 :
50 : // Efficiently flood-fill the m_Regions grid
51 :
52 117 : int regionID = 0;
53 234 : std::vector<u16> connect;
54 :
55 117 : u16* pCurrentID = NULL;
56 117 : u16 LeftID = 0;
57 117 : u16 DownID = 0;
58 117 : bool Checked = false; // prevent some unneccessary RootID calls
59 :
60 117 : connect.reserve(32); // TODO: What's a sensible number?
61 117 : connect.push_back(0); // connect[0] = 0
62 :
63 : // Start by filling the grid with 0 for blocked,
64 : // and regionID for unblocked
65 9252 : for (int j = j0; j < j1; ++j)
66 : {
67 746490 : for (int i = i0; i < i1; ++i)
68 : {
69 737355 : pCurrentID = &m_Regions[j-j0][i-i0];
70 1054694 : if (!IS_PASSABLE(grid->get(i, j), passClass))
71 : {
72 317339 : *pCurrentID = 0;
73 317339 : continue;
74 : }
75 :
76 420016 : if (j > j0)
77 414693 : DownID = m_Regions[j-1-j0][i-i0];
78 :
79 420016 : if (i == i0)
80 4323 : LeftID = 0;
81 : else
82 415693 : LeftID = m_Regions[j-j0][i-1-i0];
83 :
84 420016 : if (LeftID > 0)
85 : {
86 414206 : *pCurrentID = LeftID;
87 414206 : if (*pCurrentID != DownID && DownID > 0 && !Checked)
88 : {
89 20 : u16 id0 = RootID(DownID, connect);
90 20 : u16 id1 = RootID(LeftID, connect);
91 20 : Checked = true; // this avoids repeatedly connecting the same IDs
92 :
93 20 : if (id0 < id1)
94 16 : connect[id1] = id0;
95 4 : else if (id0 > id1)
96 4 : connect[id0] = id1;
97 : }
98 414186 : else if (DownID == 0)
99 5658 : Checked = false;
100 : }
101 5810 : else if (DownID > 0)
102 : {
103 5714 : *pCurrentID = DownID;
104 5714 : Checked = false;
105 : }
106 : else
107 : {
108 : // New ID
109 96 : *pCurrentID = ++regionID;
110 96 : connect.push_back(regionID);
111 96 : Checked = false;
112 : }
113 : }
114 : }
115 :
116 : // Mark connected regions as being the same ID (i.e. the lowest)
117 117 : m_RegionsID.clear();
118 213 : for (u16 i = 1; i < regionID+1; ++i)
119 : {
120 96 : if (connect[i] != i)
121 20 : connect[i] = RootID(i, connect);
122 : else
123 76 : m_RegionsID.push_back(connect[i]);
124 : }
125 :
126 : // Replace IDs by the root ID
127 11349 : for (int j = 0; j < CHUNK_SIZE; ++j)
128 1089504 : for (int i = 0; i < CHUNK_SIZE; ++i)
129 1078272 : m_Regions[j][i] = connect[m_Regions[j][i]];
130 117 : }
131 :
132 : /**
133 : * Returns a RegionID for the given global navcell coords
134 : * (which must be inside this chunk);
135 : */
136 451790 : HierarchicalPathfinder::RegionID HierarchicalPathfinder::Chunk::Get(int i, int j) const
137 : {
138 451790 : ENSURE(i < CHUNK_SIZE && j < CHUNK_SIZE);
139 451790 : return RegionID(m_ChunkI, m_ChunkJ, m_Regions[j][i]);
140 : }
141 :
142 : /**
143 : * Return the global navcell coords that correspond roughly to the
144 : * center of the given region in this chunk.
145 : * (This is not guaranteed to be actually inside the region.)
146 : */
147 0 : void HierarchicalPathfinder::Chunk::RegionCenter(u16 r, int& i_out, int& j_out) const
148 : {
149 : // Find the mean of i,j coords of navcells in this region:
150 :
151 0 : u32 si = 0, sj = 0; // sum of i,j coords
152 0 : u32 n = 0; // number of navcells in region
153 :
154 : cassert(CHUNK_SIZE < 256); // conservative limit to ensure si and sj don't overflow
155 :
156 0 : for (int j = 0; j < CHUNK_SIZE; ++j)
157 : {
158 0 : for (int i = 0; i < CHUNK_SIZE; ++i)
159 : {
160 0 : if (m_Regions[j][i] == r)
161 : {
162 0 : si += i;
163 0 : sj += j;
164 0 : n += 1;
165 : }
166 : }
167 : }
168 :
169 : // Avoid divide-by-zero
170 0 : if (n == 0)
171 0 : n = 1;
172 :
173 0 : i_out = m_ChunkI * CHUNK_SIZE + si / n;
174 0 : j_out = m_ChunkJ * CHUNK_SIZE + sj / n;
175 0 : }
176 :
177 : /**
178 : * Returns the global navcell coords, and the squared distance to the goal
179 : * navcell, of whichever navcell inside the given region is closest to
180 : * that goal.
181 : */
182 93 : void HierarchicalPathfinder::Chunk::RegionNavcellNearest(u16 r, int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const
183 : {
184 93 : iBest = 0;
185 93 : jBest = 0;
186 93 : dist2Best = std::numeric_limits<u32>::max();
187 :
188 9021 : for (int j = 0; j < CHUNK_SIZE; ++j)
189 : {
190 866016 : for (int i = 0; i < CHUNK_SIZE; ++i)
191 : {
192 857088 : if (m_Regions[j][i] != r)
193 298332 : continue;
194 :
195 1117512 : u32 dist2 = (i + m_ChunkI*CHUNK_SIZE - iGoal)*(i + m_ChunkI*CHUNK_SIZE - iGoal)
196 558756 : + (j + m_ChunkJ*CHUNK_SIZE - jGoal)*(j + m_ChunkJ*CHUNK_SIZE - jGoal);
197 :
198 558756 : if (dist2 < dist2Best)
199 : {
200 7926 : iBest = i + m_ChunkI*CHUNK_SIZE;
201 7926 : jBest = j + m_ChunkJ*CHUNK_SIZE;
202 7926 : dist2Best = dist2;
203 : }
204 : }
205 : }
206 93 : }
207 :
208 : /**
209 : * Gives the global navcell coords, and the squared distance to the (i0,j0)
210 : * navcell, of whichever navcell inside the given region and inside the given goal
211 : * is closest to (i0,j0)
212 : * Returns true if the goal is inside the region, false otherwise.
213 : */
214 4 : bool HierarchicalPathfinder::Chunk::RegionNearestNavcellInGoal(u16 r, u16 i0, u16 j0, const PathGoal& goal, u16& iOut, u16& jOut, u32& dist2Best) const
215 : {
216 : // TODO: this should be optimized further.
217 : // Most used cases empirically seem to be SQUARE, INVERTED_CIRCLE and then POINT and CIRCLE somehwat equally
218 4 : iOut = 0;
219 4 : jOut = 0;
220 4 : dist2Best = std::numeric_limits<u32>::max();
221 :
222 : // Calculate the navcell that contains the center of the goal.
223 4 : int gi = (goal.x >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
224 4 : int gj = (goal.z >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
225 :
226 4 : switch(goal.type)
227 : {
228 0 : case PathGoal::POINT:
229 : {
230 : // i and j can be equal to CHUNK_SIZE on the top and right borders of the map,
231 : // specially when mapSize is a multiple of CHUNK_SIZE
232 0 : int i = std::min((int)CHUNK_SIZE - 1, gi - m_ChunkI * CHUNK_SIZE);
233 0 : int j = std::min((int)CHUNK_SIZE - 1, gj - m_ChunkJ * CHUNK_SIZE);
234 0 : if (m_Regions[j][i] == r)
235 : {
236 0 : iOut = gi;
237 0 : jOut = gj;
238 0 : dist2Best = (gi - i0)*(gi - i0)
239 0 : + (gj - j0)*(gj - j0);
240 0 : return true;
241 : }
242 0 : return false;
243 : }
244 4 : case PathGoal::CIRCLE:
245 : case PathGoal::SQUARE:
246 : {
247 : // restrict ourselves to a square surrounding the goal.
248 4 : int radius = (std::max(goal.hw*3/2,goal.hh*3/2) >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToInfinity();
249 4 : int imin = std::max(0, gi-m_ChunkI*CHUNK_SIZE-radius);
250 4 : int imax = std::min((int)CHUNK_SIZE, gi-m_ChunkI*CHUNK_SIZE+radius+1);
251 4 : int jmin = std::max(0, gj-m_ChunkJ*CHUNK_SIZE-radius);
252 4 : int jmax = std::min((int)CHUNK_SIZE, gj-m_ChunkJ*CHUNK_SIZE+radius+1);
253 4 : bool found = false;
254 4 : u32 dist2 = std::numeric_limits<u32>::max();
255 132 : for (u16 j = jmin; j < jmax; ++j)
256 : {
257 3867 : for (u16 i = imin; i < imax; ++i)
258 : {
259 3739 : if (m_Regions[j][i] != r)
260 1447 : continue;
261 :
262 2292 : if (found)
263 : {
264 3534 : dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0)
265 1767 : + (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0);
266 1767 : if (dist2 >= dist2Best)
267 1607 : continue;
268 : }
269 :
270 685 : if (goal.NavcellContainsGoal(m_ChunkI * CHUNK_SIZE + i, m_ChunkJ * CHUNK_SIZE + j))
271 : {
272 11 : if (!found)
273 : {
274 1 : found = true;
275 2 : dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0)
276 1 : + (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0);
277 : }
278 11 : iOut = i + m_ChunkI*CHUNK_SIZE;
279 11 : jOut = j + m_ChunkJ*CHUNK_SIZE;
280 11 : dist2Best = dist2;
281 : }
282 : }
283 : }
284 4 : return found;
285 : }
286 0 : case PathGoal::INVERTED_CIRCLE:
287 : case PathGoal::INVERTED_SQUARE:
288 : {
289 0 : bool found = false;
290 0 : u32 dist2 = std::numeric_limits<u32>::max();
291 : // loop over all navcells.
292 0 : for (u16 j = 0; j < CHUNK_SIZE; ++j)
293 : {
294 0 : for (u16 i = 0; i < CHUNK_SIZE; ++i)
295 : {
296 0 : if (m_Regions[j][i] != r)
297 0 : continue;
298 :
299 0 : if (found)
300 : {
301 0 : dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0)
302 0 : + (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0);
303 0 : if (dist2 >= dist2Best)
304 0 : continue;
305 : }
306 :
307 0 : if (goal.NavcellContainsGoal(m_ChunkI * CHUNK_SIZE + i, m_ChunkJ * CHUNK_SIZE + j))
308 : {
309 0 : if (!found)
310 : {
311 0 : found = true;
312 0 : dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0)
313 0 : + (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0);
314 : }
315 0 : iOut = i + m_ChunkI*CHUNK_SIZE;
316 0 : jOut = j + m_ChunkJ*CHUNK_SIZE;
317 0 : dist2Best = dist2;
318 : }
319 : }
320 : }
321 0 : return found;
322 : }
323 : }
324 0 : return false;
325 : }
326 :
327 6 : HierarchicalPathfinder::HierarchicalPathfinder() : m_DebugOverlay(NULL)
328 : {
329 6 : }
330 :
331 12 : HierarchicalPathfinder::~HierarchicalPathfinder()
332 : {
333 6 : SAFE_DELETE(m_DebugOverlay);
334 6 : }
335 :
336 0 : void HierarchicalPathfinder::SetDebugOverlay(bool enabled, const CSimContext* simContext)
337 : {
338 0 : if (enabled && !m_DebugOverlay)
339 : {
340 0 : m_DebugOverlay = new HierarchicalOverlay(*this);
341 0 : m_DebugOverlayLines.clear();
342 0 : m_SimContext = simContext;
343 0 : AddDebugEdges(GetPassabilityClass("default"));
344 : }
345 0 : else if (!enabled && m_DebugOverlay)
346 : {
347 0 : SAFE_DELETE(m_DebugOverlay);
348 0 : m_DebugOverlayLines.clear();
349 0 : m_SimContext = NULL;
350 : }
351 0 : }
352 :
353 0 : void HierarchicalPathfinder::RenderSubmit(SceneCollector& collector)
354 : {
355 0 : if (!m_DebugOverlay)
356 0 : return;
357 :
358 0 : for (size_t i = 0; i < m_DebugOverlayLines.size(); ++i)
359 0 : collector.Submit(&m_DebugOverlayLines[i]);
360 : }
361 :
362 3 : void HierarchicalPathfinder::Recompute(Grid<NavcellData>* grid,
363 : const std::map<std::string, pass_class_t>& nonPathfindingPassClassMasks,
364 : const std::map<std::string, pass_class_t>& pathfindingPassClassMasks)
365 : {
366 6 : PROFILE2("Hierarchical Recompute");
367 :
368 3 : m_PassClassMasks = pathfindingPassClassMasks;
369 :
370 6 : std::map<std::string, pass_class_t> allPassClasses = m_PassClassMasks;
371 3 : allPassClasses.insert(nonPathfindingPassClassMasks.begin(), nonPathfindingPassClassMasks.end());
372 :
373 3 : m_W = grid->m_W;
374 3 : m_H = grid->m_H;
375 :
376 3 : ENSURE((grid->m_W + CHUNK_SIZE - 1) / CHUNK_SIZE < 256 && (grid->m_H + CHUNK_SIZE - 1) / CHUNK_SIZE < 256); // else the u8 Chunk::m_ChunkI will overflow
377 :
378 : // Divide grid into chunks with round-to-positive-infinity
379 3 : m_ChunksW = (grid->m_W + CHUNK_SIZE - 1) / CHUNK_SIZE;
380 3 : m_ChunksH = (grid->m_H + CHUNK_SIZE - 1) / CHUNK_SIZE;
381 :
382 3 : m_Chunks.clear();
383 3 : m_Edges.clear();
384 :
385 : // Reset global regions.
386 3 : m_NextGlobalRegionID = 1;
387 :
388 12 : for (auto& passClassMask : allPassClasses)
389 : {
390 9 : pass_class_t passClass = passClassMask.second;
391 :
392 : // Compute the regions within each chunk
393 9 : m_Chunks[passClass].resize(m_ChunksW*m_ChunksH);
394 30 : for (int cj = 0; cj < m_ChunksH; ++cj)
395 : {
396 78 : for (int ci = 0; ci < m_ChunksW; ++ci)
397 : {
398 57 : m_Chunks[passClass].at(cj*m_ChunksW + ci).InitRegions(ci, cj, grid, passClass);
399 : }
400 : }
401 :
402 : // Construct the search graph over the regions.
403 9 : EdgesMap& edges = m_Edges[passClass];
404 9 : RecomputeAllEdges(passClass, edges);
405 :
406 : // Spread global regions.
407 9 : std::map<RegionID, GlobalRegionID>& globalRegion = m_GlobalRegions[passClass];
408 9 : globalRegion.clear();
409 30 : for (u8 cj = 0; cj < m_ChunksH; ++cj)
410 78 : for (u8 ci = 0; ci < m_ChunksW; ++ci)
411 99 : for (u16 rid : GetChunk(ci, cj, passClass).m_RegionsID)
412 : {
413 42 : RegionID reg{ci,cj,rid};
414 42 : if (globalRegion.find(reg) == globalRegion.end())
415 : {
416 8 : GlobalRegionID ID = m_NextGlobalRegionID++;
417 :
418 8 : globalRegion.insert({ reg, ID });
419 : // Avoid creating an empty link if possible, FindReachableRegions uses [] which calls the default constructor.
420 8 : if (edges.find(reg) != edges.end())
421 : {
422 8 : std::set<RegionID> reachable;
423 4 : FindReachableRegions(reg, reachable, passClass);
424 42 : for (const RegionID& region : reachable)
425 38 : globalRegion.insert({ region, ID });
426 : }
427 : }
428 : }
429 : }
430 :
431 3 : if (m_DebugOverlay)
432 : {
433 0 : m_DebugOverlayLines.clear();
434 0 : AddDebugEdges(GetPassabilityClass("default"));
435 : }
436 3 : }
437 :
438 7 : void HierarchicalPathfinder::Update(Grid<NavcellData>* grid, const Grid<u8>& dirtinessGrid)
439 : {
440 14 : PROFILE3("Hierarchical Update");
441 :
442 7 : ASSERT(m_NextGlobalRegionID < std::numeric_limits<GlobalRegionID>::max());
443 :
444 14 : std::map<pass_class_t, std::vector<RegionID> > needNewGlobalRegionMap;
445 :
446 : // Algorithm for the partial update:
447 : // 1. Loop over chunks.
448 : // 2. For any dirty chunk:
449 : // - remove all regions from the global region map
450 : // - remove all edges, by removing the neighbor connection with them and then deleting us
451 : // - recreate regions inside the chunk
452 : // - reconnect the regions. We may do too much work if we reconnect with a dirty chunk, but that's fine.
453 : // 3. Recreate global regions.
454 : // This means that if any chunk changes, we may need to flood (at most once) the whole map.
455 : // That's quite annoying, but I can't think of an easy way around it.
456 : // If we could be sure that a region's topology hasn't changed, we could skip removing its global region
457 : // but that's non trivial as we have no easy way to determine said topology (regions could "switch" IDs on update for now).
458 28 : for (u8 cj = 0; cj < m_ChunksH; ++cj)
459 : {
460 21 : int j0 = cj * CHUNK_SIZE;
461 21 : int j1 = std::min(j0 + CHUNK_SIZE, (int)dirtinessGrid.m_H);
462 84 : for (u8 ci = 0; ci < m_ChunksW; ++ci)
463 : {
464 : // Skip chunks where no navcells are dirty.
465 63 : int i0 = ci * CHUNK_SIZE;
466 63 : int i1 = std::min(i0 + CHUNK_SIZE, (int)dirtinessGrid.m_W);
467 63 : if (!dirtinessGrid.any_set_in_square(i0, j0, i1, j1))
468 33 : continue;
469 :
470 90 : for (const std::pair<const std::string, pass_class_t>& passClassMask : m_PassClassMasks)
471 : {
472 60 : pass_class_t passClass = passClassMask.second;
473 60 : Chunk& a = m_Chunks[passClass].at(ci + cj*m_ChunksW);
474 :
475 : // Clean up edges and global region ID
476 60 : EdgesMap& edgeMap = m_Edges[passClass];
477 94 : for (u16 i : a.m_RegionsID)
478 : {
479 34 : RegionID reg{ci, cj, i};
480 34 : m_GlobalRegions[passClass].erase(reg);
481 124 : for (const RegionID& neighbor : edgeMap[reg])
482 : {
483 90 : edgeMap[neighbor].erase(reg);
484 90 : if (edgeMap[neighbor].empty())
485 0 : edgeMap.erase(neighbor);
486 : }
487 34 : edgeMap.erase(reg);
488 : }
489 :
490 : // Recompute regions inside this chunk.
491 60 : a.InitRegions(ci, cj, grid, passClass);
492 :
493 94 : for (u16 i : a.m_RegionsID)
494 34 : needNewGlobalRegionMap[passClass].push_back(RegionID{ci, cj, i});
495 :
496 60 : UpdateEdges(ci, cj, passClass, edgeMap);
497 : }
498 : }
499 : }
500 :
501 7 : UpdateGlobalRegions(needNewGlobalRegionMap);
502 :
503 7 : if (m_DebugOverlay)
504 : {
505 0 : m_DebugOverlayLines.clear();
506 0 : AddDebugEdges(GetPassabilityClass("default"));
507 : }
508 7 : }
509 :
510 254 : void HierarchicalPathfinder::ComputeNeighbors(EdgesMap& edges, Chunk& a, Chunk& b, bool transpose, bool opposite) const
511 : {
512 : // For each edge between chunks, we loop over every adjacent pair of
513 : // navcells in the two chunks. If they are both in valid regions
514 : // (i.e. are passable navcells) then add a graph edge between those regions.
515 : // (We don't need to test for duplicates since EdgesMap already uses a
516 : // std::set which will drop duplicate entries.)
517 : // But as set.insert can be quite slow on large collection, and that we usually
518 : // try to insert the same values, we cache the previous one for a fast test.
519 254 : RegionID raPrev(0,0,0);
520 254 : RegionID rbPrev(0,0,0);
521 24638 : for (int k = 0; k < CHUNK_SIZE; ++k)
522 : {
523 24384 : u8 aSide = opposite ? CHUNK_SIZE - 1 : 0;
524 24384 : u8 bSide = CHUNK_SIZE - 1 - aSide;
525 24384 : RegionID ra = transpose ? a.Get(k, aSide) : a.Get(aSide, k);
526 24384 : RegionID rb = transpose ? b.Get(k, bSide) : b.Get(bSide, k);
527 24384 : if (ra.r && rb.r)
528 : {
529 9890 : if (ra == raPrev && rb == rbPrev)
530 9751 : continue;
531 139 : edges[ra].insert(rb);
532 139 : edges[rb].insert(ra);
533 139 : raPrev = ra;
534 139 : rbPrev = rb;
535 : }
536 : }
537 254 : }
538 :
539 : /**
540 : * Connect a chunk's regions to their neighbors. Not optimised for global recomputing.
541 : */
542 60 : void HierarchicalPathfinder::UpdateEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges)
543 : {
544 60 : std::vector<Chunk>& chunks = m_Chunks[passClass];
545 :
546 60 : Chunk& a = chunks.at(cj*m_ChunksW + ci);
547 :
548 60 : if (ci > 0)
549 60 : ComputeNeighbors(edges, a, chunks.at(cj*m_ChunksW + (ci-1)), false, false);
550 :
551 60 : if (ci < m_ChunksW-1)
552 42 : ComputeNeighbors(edges, a, chunks.at(cj*m_ChunksW + (ci+1)), false, true);
553 :
554 60 : if (cj > 0)
555 40 : ComputeNeighbors(edges, a, chunks.at((cj-1)*m_ChunksW + ci), true, false);
556 :
557 60 : if (cj < m_ChunksH - 1)
558 40 : ComputeNeighbors(edges, a, chunks.at((cj+1)*m_ChunksW + ci), true, true);
559 60 : }
560 :
561 : /**
562 : * Find edges between regions in all chunks, in an optimised manner (only look at top/left)
563 : */
564 9 : void HierarchicalPathfinder::RecomputeAllEdges(pass_class_t passClass, EdgesMap& edges)
565 : {
566 9 : std::vector<Chunk>& chunks = m_Chunks[passClass];
567 :
568 9 : edges.clear();
569 :
570 30 : for (int cj = 0; cj < m_ChunksH; ++cj)
571 : {
572 78 : for (int ci = 0; ci < m_ChunksW; ++ci)
573 : {
574 57 : Chunk& a = chunks.at(cj*m_ChunksW + ci);
575 :
576 57 : if (ci > 0)
577 36 : ComputeNeighbors(edges, a, chunks.at(cj*m_ChunksW + (ci-1)), false, false);
578 :
579 57 : if (cj > 0)
580 36 : ComputeNeighbors(edges, a, chunks.at((cj-1)*m_ChunksW + ci), true, false);
581 : }
582 : }
583 9 : }
584 :
585 : /**
586 : * Debug visualisation of graph edges between regions.
587 : */
588 0 : void HierarchicalPathfinder::AddDebugEdges(pass_class_t passClass)
589 : {
590 0 : const EdgesMap& edges = m_Edges[passClass];
591 0 : const std::vector<Chunk>& chunks = m_Chunks[passClass];
592 :
593 0 : for (auto& edge : edges)
594 : {
595 0 : for (const RegionID& region: edge.second)
596 : {
597 : // Draw a line between the two regions' centers
598 :
599 : int i0, j0, i1, j1;
600 0 : chunks[edge.first.cj * m_ChunksW + edge.first.ci].RegionCenter(edge.first.r, i0, j0);
601 0 : chunks[region.cj * m_ChunksW + region.ci].RegionCenter(region.r, i1, j1);
602 :
603 0 : CFixedVector2D a, b;
604 0 : Pathfinding::NavcellCenter(i0, j0, a.X, a.Y);
605 0 : Pathfinding::NavcellCenter(i1, j1, b.X, b.Y);
606 :
607 : // Push the endpoints inwards a little to avoid overlaps
608 0 : CFixedVector2D d = b - a;
609 0 : d.Normalize(entity_pos_t::FromInt(1));
610 0 : a += d;
611 0 : b -= d;
612 :
613 0 : std::vector<float> xz;
614 0 : xz.push_back(a.X.ToFloat());
615 0 : xz.push_back(a.Y.ToFloat());
616 0 : xz.push_back(b.X.ToFloat());
617 0 : xz.push_back(b.Y.ToFloat());
618 :
619 0 : m_DebugOverlayLines.emplace_back();
620 0 : m_DebugOverlayLines.back().m_Color = CColor(1.0, 1.0, 1.0, 1.0);
621 0 : SimRender::ConstructLineOnGround(*m_SimContext, xz, m_DebugOverlayLines.back(), true);
622 : }
623 : }
624 0 : }
625 :
626 7 : void HierarchicalPathfinder::UpdateGlobalRegions(const std::map<pass_class_t, std::vector<RegionID> >& needNewGlobalRegionMap)
627 : {
628 : // Use FindReachableRegions because we cannot be sure, even if we find a non-dirty chunk nearby,
629 : // that we weren't the only bridge connecting that chunk to the rest of the global region.
630 14 : for (const std::pair<const pass_class_t, std::vector<RegionID>>& regionsInNeed : needNewGlobalRegionMap)
631 : {
632 41 : for (const RegionID& reg : regionsInNeed.second)
633 : {
634 34 : std::map<RegionID, GlobalRegionID>& globalRegions = m_GlobalRegions[regionsInNeed.first];
635 : // If we have already been given a region, skip us.
636 34 : if (globalRegions.find(reg) != globalRegions.end())
637 25 : continue;
638 :
639 18 : std::set<RegionID> reachable;
640 9 : FindReachableRegions(reg, reachable, regionsInNeed.first);
641 :
642 9 : GlobalRegionID ID = m_NextGlobalRegionID++;
643 :
644 76 : for (const RegionID& regionId : reachable)
645 67 : globalRegions[regionId] = ID;
646 : }
647 : }
648 7 : }
649 :
650 403022 : HierarchicalPathfinder::RegionID HierarchicalPathfinder::Get(u16 i, u16 j, pass_class_t passClass) const
651 : {
652 403022 : int ci = i / CHUNK_SIZE;
653 403022 : int cj = j / CHUNK_SIZE;
654 403022 : ENSURE(ci < m_ChunksW && cj < m_ChunksH);
655 403022 : return m_Chunks.at(passClass)[cj*m_ChunksW + ci].Get(i % CHUNK_SIZE, j % CHUNK_SIZE);
656 : }
657 :
658 402734 : HierarchicalPathfinder::GlobalRegionID HierarchicalPathfinder::GetGlobalRegion(u16 i, u16 j, pass_class_t passClass) const
659 : {
660 402734 : return GetGlobalRegion(Get(i, j, passClass), passClass);
661 : }
662 :
663 402740 : HierarchicalPathfinder::GlobalRegionID HierarchicalPathfinder::GetGlobalRegion(RegionID region, pass_class_t passClass) const
664 : {
665 402740 : return region.r == 0 ? GlobalRegionID(0) : m_GlobalRegions.at(passClass).at(region);
666 : }
667 :
668 6 : void CreatePointGoalAt(u16 i, u16 j, PathGoal& goal)
669 : {
670 6 : PathGoal newGoal;
671 6 : newGoal.type = PathGoal::POINT;
672 6 : Pathfinding::NavcellCenter(i, j, newGoal.x, newGoal.z);
673 6 : goal = newGoal;
674 6 : }
675 :
676 11 : bool HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const
677 : {
678 22 : PROFILE2("MakeGoalReachable");
679 :
680 : u16 iGoal, jGoal;
681 11 : Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H);
682 :
683 22 : std::set<InterestingRegion, SortByBestToPoint> goalRegions(SortByBestToPoint(i0, j0));
684 : // This returns goal regions ordered by distance from the best navcell in each region.
685 11 : FindGoalRegionsAndBestNavcells(i0, j0, iGoal, jGoal, goal, goalRegions, passClass);
686 :
687 : // Because of the sorting above, we can stop as soon as the first reachable goal region is found.
688 11 : for (const InterestingRegion& region : goalRegions)
689 6 : if (GetGlobalRegion(region.region, passClass) == GetGlobalRegion(i0, j0, passClass))
690 : {
691 6 : iGoal = region.bestI;
692 6 : jGoal = region.bestJ;
693 :
694 : // No need to move reachable point goals.
695 6 : if (goal.type != PathGoal::POINT)
696 1 : CreatePointGoalAt(iGoal, jGoal, goal);
697 6 : return true;
698 : }
699 :
700 : // Goal wasn't reachable - get the closest navcell in the nearest reachable region.
701 10 : std::set<RegionID, SortByCenterToPoint> reachableRegions(SortByCenterToPoint(iGoal, jGoal));
702 5 : FindReachableRegions(Get(i0, j0, passClass), reachableRegions, passClass);
703 :
704 5 : FindNearestNavcellInRegions(reachableRegions, iGoal, jGoal, passClass);
705 5 : CreatePointGoalAt(iGoal, jGoal, goal);
706 5 : return false;
707 : }
708 :
709 :
710 0 : bool HierarchicalPathfinder::IsGoalReachable(u16 i0, u16 j0, const PathGoal& goal, pass_class_t passClass) const
711 : {
712 0 : PROFILE2("IsGoalReachable");
713 :
714 : u16 iGoal, jGoal;
715 0 : Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H);
716 :
717 0 : std::set<InterestingRegion, SortByBestToPoint> goalRegions(SortByBestToPoint(i0, j0));
718 : // This returns goal regions ordered by distance from the best navcell in each region.
719 0 : FindGoalRegionsAndBestNavcells(i0, j0, iGoal, jGoal, goal, goalRegions, passClass);
720 :
721 : // Because of the sorting above, we can stop as soon as the first reachable goal region is found.
722 0 : for (const InterestingRegion& region : goalRegions)
723 0 : if (GetGlobalRegion(region.region, passClass) == GetGlobalRegion(i0, j0, passClass))
724 0 : return true;
725 0 : return false;
726 : }
727 :
728 17 : void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) const
729 : {
730 34 : std::set<RegionID, SortByCenterToPoint> regions(SortByCenterToPoint(i, j));
731 :
732 : // Construct a set of all regions of all chunks for this pass class
733 170 : for (const Chunk& chunk : m_Chunks.at(passClass))
734 336 : for (int r : chunk.m_RegionsID)
735 183 : regions.insert(RegionID(chunk.m_ChunkI, chunk.m_ChunkJ, r));
736 :
737 17 : FindNearestNavcellInRegions(regions, i, j, passClass);
738 17 : }
739 :
740 22 : void HierarchicalPathfinder::FindNearestNavcellInRegions(const std::set<RegionID, SortByCenterToPoint>& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const
741 : {
742 22 : u16 bestI = iGoal, bestJ = jGoal; // Somewhat sensible default-values should regions() be passed empty.
743 22 : u32 bestDist = std::numeric_limits<u32>::max();
744 :
745 : // Because regions are sorted by increasing distance, we can ignore regions that are obviously farther than the current best point.
746 : // Since regions are squares, that happens when the center of a region is at least sqrt(2) * CHUNK_SIZE farther than the current best point.
747 : // Add one to avoid cases where the center navcell is actually slightly off-center (= CHUNK_SIZE is even)
748 22 : u32 maxDistFromBest = (fixed::FromInt(3) / 2 * CHUNK_SIZE).ToInt_RoundToInfinity() + 1;
749 : // TODO: update to static_assert with constexpr
750 22 : ENSURE(maxDistFromBest < std::numeric_limits<u16>::max());
751 22 : maxDistFromBest *= maxDistFromBest;
752 :
753 115 : for (const RegionID& region : regions)
754 : {
755 115 : u32 chunkDist = region.DistanceTo(iGoal, jGoal);
756 : // This might overflow, but only if we are already close to the maximal possible distance, so the condition would probably be false anyways.
757 : // It's also a bit pessimistic, so we'll still consider a few too many regions.
758 115 : if (bestDist < std::numeric_limits<u32>::max() && chunkDist > maxDistFromBest + bestDist)
759 22 : break; // Break, the set is ordered by increased distance so a closer region will not be found.
760 :
761 : int ri, rj;
762 : u32 dist;
763 93 : GetChunk(region.ci, region.cj, passClass).RegionNavcellNearest(region.r, iGoal, jGoal, ri, rj, dist);
764 93 : if (dist < bestDist)
765 : {
766 22 : bestI = ri;
767 22 : bestJ = rj;
768 22 : bestDist = dist;
769 : }
770 : }
771 22 : iGoal = bestI;
772 22 : jGoal = bestJ;
773 22 : }
774 :
775 11 : void HierarchicalPathfinder::FindGoalRegionsAndBestNavcells(u16 i0, u16 j0, u16 gi, u16 gj, const PathGoal& goal, std::set<InterestingRegion, SortByBestToPoint>& regions, pass_class_t passClass) const
776 : {
777 11 : if (goal.type == PathGoal::POINT)
778 : {
779 8 : RegionID region = Get(gi, gj, passClass);
780 8 : if (region.r > 0)
781 5 : regions.insert({region, gi, gj});
782 8 : return;
783 : }
784 :
785 : // For non-point cases, we'll test each region inside the bounds of the goal.
786 : // we might occasionally test a few too many for circles but it's not too bad.
787 : // Note that this also works in the Inverse-circle / Inverse-square case
788 : // Since our ranges are inclusive, we will necessarily test at least the perimeter/outer bound of the goal.
789 : // If we find a navcell, great, if not, well then we'll be surrounded by an impassable barrier.
790 : // Since in the Inverse-XX case we're supposed to start inside, then we can't ever reach the goal so it's good enough.
791 : // It's not worth it to skip the "inner" regions since we'd need ranges above CHUNK_SIZE for that to start mattering
792 : // (and even then not always) and that just doesn't happen for Inverse-XX goals
793 3 : int size = (std::max(goal.hh, goal.hw) * 3 / 2).ToInt_RoundToInfinity();
794 :
795 : u16 bestI, bestJ;
796 : u32 c; // Unused.
797 :
798 6 : for (u8 sz = std::max(0,(gj - size) / CHUNK_SIZE); sz <= std::min(m_ChunksH-1, (gj + size + 1) / CHUNK_SIZE); ++sz)
799 7 : for (u8 sx = std::max(0,(gi - size) / CHUNK_SIZE); sx <= std::min(m_ChunksW-1, (gi + size + 1) / CHUNK_SIZE); ++sx)
800 : {
801 4 : const Chunk& chunk = GetChunk(sx, sz, passClass);
802 8 : for (u16 i : chunk.m_RegionsID)
803 4 : if (chunk.RegionNearestNavcellInGoal(i, i0, j0, goal, bestI, bestJ, c))
804 1 : regions.insert({RegionID{sx, sz, i}, bestI, bestJ});
805 : }
806 : }
807 :
808 0 : void HierarchicalPathfinder::FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid<u16>& grid) const
809 : {
810 0 : ENSURE(grid.m_W == m_W && grid.m_H == m_H);
811 :
812 0 : int i0 = region.ci * CHUNK_SIZE;
813 0 : int j0 = region.cj * CHUNK_SIZE;
814 :
815 0 : const Chunk& c = m_Chunks.at(passClass)[region.cj * m_ChunksW + region.ci];
816 :
817 0 : for (int j = 0; j < CHUNK_SIZE; ++j)
818 0 : for (int i = 0; i < CHUNK_SIZE; ++i)
819 0 : if (c.m_Regions[j][i] == region.r)
820 0 : grid.set(i0 + i, j0 + j, value);
821 0 : }
822 :
823 0 : Grid<u16> HierarchicalPathfinder::GetConnectivityGrid(pass_class_t passClass) const
824 : {
825 0 : Grid<u16> connectivityGrid(m_W, m_H);
826 0 : connectivityGrid.reset();
827 :
828 0 : u16 idx = 1;
829 :
830 0 : for (u16 i = 0; i < m_W; ++i)
831 : {
832 0 : for (u16 j = 0; j < m_H; ++j)
833 : {
834 0 : if (connectivityGrid.get(i, j) != 0)
835 0 : continue;
836 :
837 0 : RegionID from = Get(i, j, passClass);
838 0 : if (from.r == 0)
839 0 : continue;
840 :
841 0 : std::set<RegionID> reachable;
842 0 : FindReachableRegions(from, reachable, passClass);
843 :
844 0 : for (const RegionID& region : reachable)
845 0 : FillRegionOnGrid(region, passClass, idx, connectivityGrid);
846 :
847 0 : ++idx;
848 : }
849 : }
850 :
851 0 : return connectivityGrid;
852 0 : }
|
