Index: source/simulation2/components/tests/test_HierPathfinder.h =================================================================== --- source/simulation2/components/tests/test_HierPathfinder.h +++ source/simulation2/components/tests/test_HierPathfinder.h @@ -350,13 +350,15 @@ u16 i = 5, j = 5; hierPath.FindNearestPassableNavcell(i, j, PASS_1); TS_ASSERT(i == 5 && j == 5); + hierPath.FindNearestPassableNavcell(i, j, PASS_1, true); + TS_ASSERT(i == 5 && j == 5); // use a macro so the lines reported by tests are accurate -#define check_closest_passable(i, j, expected_manhattan) \ +#define check_closest_passable(i, j, expected_manhattan, limited) \ oi = i; oj = j; \ pi = i; pj = j; \ TS_ASSERT(!IS_PASSABLE(grid.get(pi, pj), PASS_1)); \ - hierPath.FindNearestPassableNavcell(pi, pj, PASS_1); \ + hierPath.FindNearestPassableNavcell(pi, pj, PASS_1, limited); \ \ if (expected_manhattan == -1) \ { \ @@ -367,13 +369,23 @@ } u16 oi, oj, pi, pj; - check_closest_passable(4 * 5, 4 * 5, 1); - check_closest_passable(4 * 5 + 1, 4 * 5 + 1, 2); - check_closest_passable(14 * 5 + 2, 7 * 5 + 2, 8); - check_closest_passable(14 * 5 + 2, 7 * 5 + 4, 6); - check_closest_passable(14 * 5 + 2, 7 * 5 + 5, 5); - check_closest_passable(14 * 5 + 2, 7 * 5 + 6, 4); - check_closest_passable(5 * 5 + 3, 7 * 5 + 2, 2); + check_closest_passable(4 * 5, 4 * 5, 1, false); + check_closest_passable(4 * 5, 4 * 5, 1, true); + + check_closest_passable(4 * 5 + 1, 4 * 5 + 1, 2, false); + check_closest_passable(4 * 5 + 1, 4 * 5 + 1, 2, true); + + check_closest_passable(14 * 5 + 2, 7 * 5 + 2, 8, false); + check_closest_passable(14 * 5 + 2, 7 * 5 + 2, -1, true); + check_closest_passable(14 * 5 + 2, 7 * 5 + 4, 6, false); + check_closest_passable(14 * 5 + 2, 7 * 5 + 4, -1, true); + check_closest_passable(14 * 5 + 2, 7 * 5 + 5, 5, false); + check_closest_passable(14 * 5 + 2, 7 * 5 + 5, 5, true); + check_closest_passable(14 * 5 + 2, 7 * 5 + 6, 4, false); + check_closest_passable(14 * 5 + 2, 7 * 5 + 6, 4, true); + + check_closest_passable(5 * 5 + 3, 7 * 5 + 2, 2, false); + check_closest_passable(5 * 5 + 3, 7 * 5 + 2, 2, true); #undef check_closest_passable PathGoal goal; Index: source/simulation2/components/tests/test_Pathfinder.h =================================================================== --- source/simulation2/components/tests/test_Pathfinder.h +++ source/simulation2/components/tests/test_Pathfinder.h @@ -137,6 +137,35 @@ } } + void test_reachability_sanity_test() + { + // Goal of this test: validate that we stay on the same cell. This isn't 100% coverage but if the point goal gets broken, things will be _bad_. + PathGoal tgoal{PathGoal::POINT, fixed::FromInt(2), fixed::FromInt(2), fixed::Zero(), fixed::Zero(), CFixedVector2D(fixed::FromInt(1), fixed::Zero()),CFixedVector2D(fixed::Zero(), fixed::FromInt(1)), fixed::Zero()}; + + PathGoal ogoal = tgoal; + + // End up on the top left edge. + CreatePointGoalAt(5, 6, tgoal); + TS_ASSERT_EQUALS(tgoal.x, fixed::FromInt(5)); + TS_ASSERT_EQUALS(tgoal.z, fixed::FromInt(6)); + tgoal = ogoal; + // End up as close as possible from (2,2) on the same cell. + CreatePointGoalAt(1, 1, tgoal); + TS_ASSERT_EQUALS(tgoal.x, fixed::FromInt(2) - fixed::Epsilon()); + TS_ASSERT_EQUALS(tgoal.z, fixed::FromInt(2) - fixed::Epsilon()); + tgoal = ogoal; + // End up on the goal. + CreatePointGoalAt(2, 2, tgoal); + TS_ASSERT_EQUALS(tgoal.x, fixed::FromInt(2)); + TS_ASSERT_EQUALS(tgoal.z, fixed::FromInt(2)); + tgoal = ogoal; + tgoal.x = fixed::FromInt(5)/2; + // End up on the goal. + CreatePointGoalAt(2, 2, tgoal); + TS_ASSERT_EQUALS(tgoal.x, fixed::FromInt(5)/2); + TS_ASSERT_EQUALS(tgoal.z, fixed::FromInt(2)); + } + void test_performance_DISABLED() { CTerrain terrain; Index: source/simulation2/helpers/HierarchicalPathfinder.h =================================================================== --- source/simulation2/helpers/HierarchicalPathfinder.h +++ source/simulation2/helpers/HierarchicalPathfinder.h @@ -48,6 +48,7 @@ #ifdef TEST class TestCmpPathfinder; class TestHierarchicalPathfinder; +void CreatePointGoalAt(u16 i, u16 j, PathGoal& goal); #endif class HierarchicalOverlay; @@ -105,22 +106,24 @@ GlobalRegionID GetGlobalRegion(u16 i, u16 j, pass_class_t passClass) const; /** - * Updates @p goal so that it's guaranteed to be reachable from the navcell + * Updates @p goal to a point goal guaranteed to be reachable from the original navcell * @p i0, @p j0 (which is assumed to be on a passable navcell). * - * If the goal is not reachable, it is replaced with a point goal nearest to - * the goal center. + * If the goal is not reachable, it is replaced with an acceptable point goal. + * This function does not necessarily return the absolute closest navcell to the goal. * * In the case of a non-point reachable goal, it is replaced with a point goal * at the reachable navcell of the goal which is nearest to the starting navcell. + * + * @returns true if the goal was reachable, false otherwise. */ - void MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const; + bool MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const; /** - * Updates @p i, @p j (which is assumed to be an impassable navcell) - * to the nearest passable navcell. + * Updates @p i, @p j to the nearest passable navcell. + * @param limited: only search a few tiles around (i, j) */ - void FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) const; + void FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass, bool limited = false) const; /** * Generates the connectivity grid associated with the given pass_class @@ -155,6 +158,8 @@ void RegionCenter(u16 r, int& i, int& j) const; + void NearestNavcell(int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const; + void RegionNavcellNearest(u16 r, int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const; bool RegionNearestNavcellInGoal(u16 r, u16 i0, u16 j0, const PathGoal& goal, u16& iOut, u16& jOut, u32& dist2Best) const; @@ -174,10 +179,13 @@ void UpdateGlobalRegions(const std::map>& needNewGlobalRegionMap); - void FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass) const; + template + void FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass) const; void FindPassableRegions(std::set& regions, pass_class_t passClass) const; + void FindGoalRegions(u16 gi, u16 gj, const PathGoal& goal, std::set& regions, pass_class_t passClass) const; + /** * Updates @p iGoal and @p jGoal to the navcell that is the nearest to the * initial goal coordinates, in one of the given @p regions. Index: source/simulation2/helpers/HierarchicalPathfinder.cpp =================================================================== --- source/simulation2/helpers/HierarchicalPathfinder.cpp +++ source/simulation2/helpers/HierarchicalPathfinder.cpp @@ -22,6 +22,8 @@ #include "graphics/Overlay.h" #include "ps/Profile.h" +#include "Geometry.h" + // Find the root ID of a region, used by InitRegions inline u16 RootID(u16 x, const std::vector& v) { @@ -171,6 +173,53 @@ j_out = m_ChunkJ * CHUNK_SIZE + sj / n; } +/** + * Returns the global navcell coords, and the squared distance to the goal + * navcell, of whichever navcell inside the given chunk is closest to + * that goal. + */ +void HierarchicalPathfinder::Chunk::NearestNavcell(int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const +{ + iBest = 0; + jBest = 0; + dist2Best = std::numeric_limits::max(); + u32 rowBest = std::numeric_limits::max(); + + // TODO: it might be faster to try the best navcell first, since we can compute that. + for (int j = 0; j < CHUNK_SIZE; ++j) + { + for (int i = 0; i < CHUNK_SIZE; ++i) + { + if (m_Regions[j][i] == 0) + continue; + + /** + * Line-point distance. Over a single row, dist2 is first monotonically decreasing, then monotonically increasing + * Thus once we stop improving, we can safely break. + * However, we cannot compare across lines because obstructed chunks may skew numbers. + * Imagine a situation like this (G is the goal, X a passable cell, - an impassable one): + * ----G- + * -X---- + * X---X- + * Despite the first X on the third row being farther, the third row has a better navcell. + */ + u32 dist2 = (i + m_ChunkI*CHUNK_SIZE - iGoal)*(i + m_ChunkI*CHUNK_SIZE - iGoal) + + (j + m_ChunkJ*CHUNK_SIZE - jGoal)*(j + m_ChunkJ*CHUNK_SIZE - jGoal); + if (dist2 < dist2Best) + { + iBest = i + m_ChunkI*CHUNK_SIZE; + jBest = j + m_ChunkJ*CHUNK_SIZE; + dist2Best = dist2; + rowBest = dist2; + } + else if (dist2 < rowBest) + rowBest = dist2; + else // dist2 >= rowBest: we can carry on on the next line. + continue; + } + } +} + /** * Returns the global navcell coords, and the squared distance to the goal * navcell, of whichever navcell inside the given region is closest to @@ -739,90 +788,248 @@ return m_GlobalRegions.at(passClass).at(region); } -void HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const +void CreatePointGoalAt(u16 i, u16 j, PathGoal& goal) { - PROFILE2("MakeGoalReachable"); - RegionID source = Get(i0, j0, passClass); - - // If the goal is a point-goal and reachable, just return early. - if (goal.type == PathGoal::POINT) - { - u16 gi, gj; - Pathfinding::NearestNavcell(goal.x, goal.z, gi, gj, m_W, m_H); - GlobalRegionID goalRegion = GetGlobalRegion(gi, gj, passClass); - if (GetGlobalRegion(i0, j0, passClass) == goalRegion && goalRegion > 0) - return; - } - - // Find everywhere that's reachable - std::set reachableRegions; - FindReachableRegions(source, reachableRegions, passClass); - - // Check whether any reachable region contains the goal - // And get the navcell that's the closest to the point - - u16 bestI = 0; - u16 bestJ = 0; - u32 dist2Best = std::numeric_limits::max(); - - for (const RegionID& region : reachableRegions) - { - // Skip region if its chunk doesn't contain the goal area - entity_pos_t x0 = Pathfinding::NAVCELL_SIZE * (region.ci * CHUNK_SIZE); - entity_pos_t z0 = Pathfinding::NAVCELL_SIZE * (region.cj * CHUNK_SIZE); - entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE * CHUNK_SIZE; - entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE * CHUNK_SIZE; - if (!goal.RectContainsGoal(x0, z0, x1, z1)) - continue; - - u16 i,j; - u32 dist2; - - // If the region contains the goal area, the goal is reachable - // Remember the best point for optimization. - if (GetChunk(region.ci, region.cj, passClass).RegionNearestNavcellInGoal(region.r, i0, j0, goal, i, j, dist2)) - { - // If it's a point, no need to move it, we're done - if (goal.type == PathGoal::POINT) - return; - if (dist2 < dist2Best) - { - bestI = i; - bestJ = j; - dist2Best = dist2; - } - } - } - - // If the goal area wasn't reachable, - // find the navcell that's nearest to the goal's center - if (dist2Best == std::numeric_limits::max()) - { - u16 iGoal, jGoal; - Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H); - - FindNearestNavcellInRegions(reachableRegions, iGoal, jGoal, passClass); - - // Construct a new point goal at the nearest reachable navcell - PathGoal newGoal; - newGoal.type = PathGoal::POINT; - Pathfinding::NavcellCenter(iGoal, jGoal, newGoal.x, newGoal.z); - goal = newGoal; - return; - } - - ENSURE(dist2Best != std::numeric_limits::max()); + // recreate the goal as a point-goal. PathGoal newGoal; newGoal.type = PathGoal::POINT; - Pathfinding::NavcellCenter(bestI, bestJ, newGoal.x, newGoal.z); + Pathfinding::NavcellCenter(i, j, newGoal.x, newGoal.z); + + CFixedVector2D relativePos(goal.x - newGoal.x, goal.z - newGoal.z); + CFixedVector2D u(fixed::FromInt(1), fixed::FromInt(0)); + CFixedVector2D v(fixed::FromInt(0), fixed::FromInt(1)); + CFixedVector2D halfsize(fixed::FromInt(1)/2, fixed::FromInt(1)/2); + + CFixedVector2D bottomRight = CFixedVector2D(newGoal.x, newGoal.z) + halfsize; + + // Try to get close to the goal. If the goal center is in the navcell, pick that, else choose an edge. + if (Geometry::PointIsInSquare(relativePos, u, v, halfsize)) + { + newGoal.x = goal.x; + newGoal.z = goal.z; + } + else + { + relativePos = Geometry::NearestPointOnSquare(relativePos, u, v, halfsize); + newGoal.x += relativePos.X; + newGoal.z += relativePos.Y; + } + // We must not change the navcell we're on, and this could (since we could be on the bottom/right edge). + // NavcellContainsGoal returns true only for the left/upper edges, + // so we can just epsilon to the left/up. + if (newGoal.x >= bottomRight.X) + newGoal.x -= fixed::Epsilon(); + if (newGoal.z >= bottomRight.Y) + newGoal.z -= fixed::Epsilon(); + goal = newGoal; } -void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) const +bool HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const { - std::set regions; - FindPassableRegions(regions, passClass); - FindNearestNavcellInRegions(regions, i, j, passClass); + PROFILE2("MakeGoalReachable"); + + // Make the goal reachable. + // In case the goal is a passable navcell, we'll just compare global regions and return immediately. + // If the goal isn't a passable navcell, we'll try to make it one using the limited FindNearestPassableNavcell + // This optimises for a common case where we want to go to a tree and similar things. + // If the goal is decidedly not passable, we'll revert to a flood-fill. + + //////////////////////////////// + // determine if we will be able to reach the goal. + u16 gi, gj; + Pathfinding::NearestNavcell(goal.x, goal.z, gi, gj, m_W, m_H); + + std::set goalRegions; + FindGoalRegions(gi, gj, goal, goalRegions, passClass); + + std::vector targetRegions; + + GlobalRegionID startID = GetGlobalRegion(i0, j0, passClass); + bool reachable = false; + for (const RegionID& r : goalRegions) + if (m_GlobalRegions.at(passClass).at(r) == startID) + { + reachable = true; + targetRegions.push_back(r); + } + //////////////////////////////// + // if the goal is a point-goal and reachable, just return early. + if (reachable && goal.type == PathGoal::POINT) + { + CreatePointGoalAt(gi, gj, goal); + return reachable; + } + + //////////////////////////////// + // If the goal is unreachable, we'd still like to get at least somewhere on the closest reachable region. + // Use a limited FindNearestPassableNavcell search, for the 5 neighboring navcells (common case: a tree…) + // if that returns a passable, reachable navcell, go there instead. + if (!reachable) + { + u16 ngi = gi, ngj = gj; + FindNearestPassableNavcell(ngi, ngj, passClass, true); + + if (GetGlobalRegion(ngi, ngj, passClass) == startID) + { + gi = ngi; + gj = ngj; + CreatePointGoalAt(gi, gj, goal); + return reachable; + } + } + + //////////////////////////////// + // Loop through regions and get the best navcell. + u16 bestI = 0, bestJ = 0; + u32 bestDist = std::numeric_limits::max(); + if (reachable) + { + // If we have target regions, we can just check them individually, which is reasonably fast. + for (const RegionID& region : targetRegions) + { + u16 ri, rj; + u32 dist; + GetChunk(region.ci, region.cj, passClass).RegionNearestNavcellInGoal(region.r, i0, j0, goal, ri, rj, dist); + if (dist < bestDist) + { + bestI = ri; + bestJ = rj; + bestDist = dist; + } + } + } + else + { + // Otherwise we should try all reachable regions. + // To avoid doing too much work, we'll sort them by distance from chunk center + // If we've already found a point from a region and our current chunk center is at least √2*region_width farther + // we can be sure any navcell in this region will not be better, so we can exit. + + struct RegionComparator + { + RegionComparator() = delete; + RegionComparator(u16 i, u16 j): gi (i), gj(j) {}; + + u32 dist(const HierarchicalPathfinder::RegionID& region) const + { + return (region.ci * CHUNK_SIZE + CHUNK_SIZE/2 - gi) * (region.ci * CHUNK_SIZE + CHUNK_SIZE/2 - gi) + + (region.cj * CHUNK_SIZE + CHUNK_SIZE/2 - gj) * (region.cj * CHUNK_SIZE + CHUNK_SIZE/2 - gj); + } + bool operator()(const HierarchicalPathfinder::RegionID& a, const HierarchicalPathfinder::RegionID& b) const + { + if (dist(a) < dist(b)) + return true; + if (dist(a) > dist(b)) + return false; + return a.r < b.r; + } + u16 gi, gj; + }; + + // TODO: if this becomes too slow, perhaps roll-out an optimised insert-sort here. + RegionComparator comparator(gi, gj); + std::set reachableRegions(comparator); + FindReachableRegions(Get(i0, j0, passClass), reachableRegions, passClass); + + u32 bestChunkCenterDist = std::numeric_limits::max(); + u32 maxDistFromChunkCenter = (fixed::FromInt(3) / 2 * HierarchicalPathfinder::CHUNK_SIZE).ToInt_RoundToInfinity(); + ENSURE(maxDistFromChunkCenter < std::numeric_limits::max()); + maxDistFromChunkCenter *= maxDistFromChunkCenter; + for (const RegionID& region : reachableRegions) + { + u32 chunkDist = comparator.dist(region); + if (bestDist < std::numeric_limits::max() && (ssize_t)chunkDist > (ssize_t)bestChunkCenterDist + maxDistFromChunkCenter) + break; // break instead of continue as the set is ordered in increased distance. + + int ri, rj; + u32 dist; + GetChunk(region.ci, region.cj, passClass).RegionNavcellNearest(region.r, gi, gj, ri, rj, dist); + if (dist < bestDist) + { + bestI = ri; + bestJ = rj; + bestDist = dist; + bestChunkCenterDist = chunkDist; + } + } + } + CreatePointGoalAt(bestI, bestJ, goal); + + return reachable; +} + +void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass, bool limited) const +{ + // If our current region is not 0, then this is passable, return immediately. + if (Get(i, j, passClass).r != 0) + return; + + u32 dist2Best = std::numeric_limits::max(); + + u16 oi = i, oj = j; + // in most cases we're < 5 cells away so go for this first + for (u16 tj = std::max(0, oj-5); tj <= std::min(m_H-1, oj+5); ++tj) + for (u16 ti = std::max(0, oi-5); ti <= std::min(m_W-1, oi+5); ++ti) + if (Get(ti, tj, passClass).r != 0) + { + u32 dist = (ti-oi)*(ti-oi) + (tj-oj)*(tj-oj); + if (dist < dist2Best) + { + i = ti; + j = tj; + dist2Best = dist; + } + } + if (dist2Best < std::numeric_limits::max() || limited) + return; + + // We are on an impassable area, so we cannot rely on our accessibility. + // This function does not necessarily need to return the absolute closest navcell, + // since standing on impassable terrain is already a little wonky + // We'll expand in a cross-wise way, returning the best passable cell after each expansion (if any) + // this will ensure that we return an acceptable close navcell in general. + // since the first time we could be close to an edge, we'll expand at least once. + + u16 iBest = i; + u16 jBest = j; + dist2Best = std::numeric_limits::max(); + + u16 si = i/CHUNK_SIZE, sj = j/CHUNK_SIZE; + std::set> visited = { { si, sj } }; + std::vector> openList = { { si, sj } }, openListTemp; + static const int expander[4][2] = { { 1, 0 },{ -1, 0 },{ 0, 1 },{ 0, -1 } }; + for (size_t step = 0;;++step) + { + openListTemp.clear(); + for (std::pair chunk : openList) + { + // go through all regions of a chunk + int tempi, tempj; + u32 dist2; + GetChunk(chunk.first, chunk.second, passClass).NearestNavcell(i, j, tempi, tempj, dist2); + + if (dist2 < dist2Best) + { + iBest = tempi; + jBest = tempj; + dist2Best = dist2; + } + // expand cross + for (const int* dir : expander) + { + u8 x = (u8)std::min(std::max((int)si + dir[0], 0), (int)(m_ChunksW-1)); + u8 z = (u8)std::min(std::max((int)sj + dir[1], 0), (int)(m_ChunksH-1)); + if (visited.insert({ x, z }).second) + openListTemp.push_back({ x, z }); + } + } + if (openListTemp.empty() || (step > 0 && dist2Best < std::numeric_limits::max())) + break; + openList.swap(openListTemp); + } + i = iBest; + j = jBest; } void HierarchicalPathfinder::FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const @@ -882,7 +1089,8 @@ jGoal = jBest; } -void HierarchicalPathfinder::FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass) const +template +void HierarchicalPathfinder::FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass) const { // Flood-fill the region graph, starting at 'from', // collecting all the regions that are reachable via edges @@ -909,12 +1117,36 @@ } } -void HierarchicalPathfinder::FindPassableRegions(std::set& regions, pass_class_t passClass) const +void HierarchicalPathfinder::FindGoalRegions(u16 gi, u16 gj, const PathGoal& goal, std::set& regions, pass_class_t passClass) const { - // Construct a set of all regions of all chunks for this pass class - for (const Chunk& chunk : m_Chunks.at(passClass)) - for (int r : chunk.m_RegionsID) - regions.insert(RegionID(chunk.m_ChunkI, chunk.m_ChunkJ, r)); + if (goal.type == PathGoal::POINT) + { + RegionID region = Get(gi, gj, passClass); + if (region.r > 0) + regions.insert(region); + return; + } + + // For non-point cases, we'll test each region inside the bounds of the goal. + // we might occasionally test a few too many for circles but it's not too bad. + // Note that this also works in the Inverse-circle / Inverse-square case + // Since our ranges are inclusive, we will necessarily test at least the perimeter/outer bound of the goal. + // If we find a navcell, great, if not, well then we'll be surrounded by an impassable barrier. + // 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. + // It's not worth it to skip the "inner" regions since we'd need ranges above CHUNK_SIZE for that to start mattering + // (and even then not always) and that just doesn't happen for Inverse-XX goals + int size = (std::max(goal.hh, goal.hw) * 3 / 2).ToInt_RoundToInfinity(); + + u16 a,b; u32 c; // unused params for RegionNearestNavcellInGoal + + for (u8 sz = std::max(0,(gj - size) / CHUNK_SIZE); sz <= std::min(m_ChunksH-1, (gj + size + 1) / CHUNK_SIZE); ++sz) + for (u8 sx = std::max(0,(gi - size) / CHUNK_SIZE); sx <= std::min(m_ChunksW-1, (gi + size + 1) / CHUNK_SIZE); ++sx) + { + const Chunk& chunk = GetChunk(sx, sz, passClass); + for (u16 i : chunk.m_RegionsID) + if (chunk.RegionNearestNavcellInGoal(i, 0, 0, goal, a, b, c)) + regions.insert(RegionID{sx, sz, i}); + } } void HierarchicalPathfinder::FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid& grid) const