Index: ps/trunk/source/simulation2/components/tests/test_HierPathfinder.h =================================================================== --- ps/trunk/source/simulation2/components/tests/test_HierPathfinder.h +++ ps/trunk/source/simulation2/components/tests/test_HierPathfinder.h @@ -71,6 +71,15 @@ void assert_blank(HierarchicalPathfinder& hierPath) { + // test that the map has the same global region everywhere + HierarchicalPathfinder::GlobalRegionID globalRegionID = hierPath.GetGlobalRegion(35, 23, PASS_1); + for (size_t i = 0; i < mapSize; ++i) + for (size_t j = 0; j < mapSize; ++j) + { + TS_ASSERT(globalRegionID == hierPath.GetGlobalRegion(i, j, PASS_1)); + TS_ASSERT(hierPath.GetGlobalRegion(i, j, PASS_2) == 0); + } + u16 i = 89; u16 j = 34; hierPath.FindNearestPassableNavcell(i, j, PASS_1); @@ -125,8 +134,25 @@ hierPath.Update(&grid, dirtyGrid); + // Global region: check we are now split in two. + TS_ASSERT(hierPath.GetGlobalRegion(50, 50, PASS_1) != hierPath.GetGlobalRegion(150, 50, PASS_1)); for (size_t j = 0; j < mapSize; ++j) + { TS_ASSERT(hierPath.Get(125, j, PASS_1).r == 0); + TS_ASSERT(hierPath.GetGlobalRegion(125, j, PASS_1) == 0); + } + for (size_t i = 0; i < 125; ++i) + for (size_t j = 0; j < mapSize; ++j) + { + TS_ASSERT(hierPath.GetGlobalRegion(50, 50, PASS_1) == hierPath.GetGlobalRegion(i, j, PASS_1)); + TS_ASSERT(hierPath.GetGlobalRegion(i, j, PASS_2) == 0); + } + for (size_t i = 126; i < mapSize; ++i) + for (size_t j = 0; j < mapSize; ++j) + { + TS_ASSERT(hierPath.GetGlobalRegion(150, 50, PASS_1) == hierPath.GetGlobalRegion(i, j, PASS_1)); + TS_ASSERT(hierPath.GetGlobalRegion(i, j, PASS_2) == 0); + } // number of connected regions: 3 in the middle (both sides), 2 in the corners. TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(120, 120, PASS_1)].size() == 3); @@ -213,6 +239,8 @@ } hierPath.Update(&grid, dirtyGrid); + TS_ASSERT(hierPath.GetGlobalRegion(120, 120, PASS_1) != hierPath.GetGlobalRegion(150, 50, PASS_1)); + reachables.clear(); hierPath.FindReachableRegions(hierPath.Get(170, 120, PASS_1), reachables, PASS_1); TS_ASSERT(reachables.size() == 9); @@ -233,6 +261,7 @@ } hierPath.Update(&grid, dirtyGrid); + TS_ASSERT(hierPath.GetGlobalRegion(120, 120, PASS_1) == hierPath.GetGlobalRegion(150, 50, PASS_1)); reachables.clear(); hierPath.FindReachableRegions(hierPath.Get(170, 120, PASS_1), reachables, PASS_1); TS_ASSERT(reachables.size() == 9); Index: ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.h =================================================================== --- ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.h +++ ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.h @@ -35,11 +35,14 @@ * Each region is a vertex in the hierarchical pathfinder's graph. * When two regions in adjacent chunks are connected by passable navcells, * the graph contains an edge between the corresponding two vertexes. - * (There will never be an edge between two regions in the same chunk.) + * By design, there can never be an edge between two regions in the same chunk. + * + * Those fixed-size chunks are used to efficiently compute "global regions" by effectively flood-filling. + * Those can then be used to immediately determine if two reachables points are connected. + * + * The main use of this class is to convert an arbitrary PathGoal to a reachable navcell. + * This happens in MakeGoalReachable. * - * Since regions are typically fairly large, it is possible to determine - * connectivity between any two navcells by mapping them onto their appropriate - * region and then doing a relatively small graph search. */ #ifdef TEST @@ -57,6 +60,8 @@ friend class TestHierarchicalPathfinder; #endif public: + typedef u32 GlobalRegionID; + struct RegionID { u8 ci, cj; // chunk ID @@ -98,6 +103,9 @@ RegionID Get(u16 i, u16 j, pass_class_t passClass) const; + GlobalRegionID GetGlobalRegion(u16 i, u16 j, pass_class_t passClass) const; + GlobalRegionID GetGlobalRegion(RegionID region, pass_class_t passClass) const; + /** * Updates @p goal so that it's guaranteed to be reachable from the navcell * @p i0, @p j0 (which is assumed to be on a passable navcell). @@ -169,10 +177,14 @@ void RecomputeAllEdges(pass_class_t passClass, EdgesMap& edges); void UpdateEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges); + void UpdateGlobalRegions(const std::map >& needNewGlobalRegionMap); + 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. @@ -193,6 +205,9 @@ std::map m_Edges; + std::map > m_GlobalRegions; + GlobalRegionID m_NextGlobalRegionID; + // Passability classes for which grids will be updated when calling Update std::map m_PassClassMasks; Index: ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.cpp =================================================================== --- ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.cpp +++ ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.cpp @@ -380,6 +380,9 @@ m_Chunks.clear(); m_Edges.clear(); + // Reset global regions. + m_NextGlobalRegionID = 1; + for (auto& passClassMask : allPassClasses) { pass_class_t passClass = passClassMask.second; @@ -394,10 +397,33 @@ } } - // Construct the search graph over the regions + // Construct the search graph over the regions. EdgesMap& edges = m_Edges[passClass]; RecomputeAllEdges(passClass, edges); + // Spread global regions. + std::map& globalRegion = m_GlobalRegions[passClass]; + globalRegion.clear(); + for (u8 cj = 0; cj < m_ChunksH; ++cj) + for (u8 ci = 0; ci < m_ChunksW; ++ci) + for (u16 rid : GetChunk(ci, cj, passClass).m_RegionsID) + { + RegionID reg{ci,cj,rid}; + if (globalRegion.find(reg) == globalRegion.end()) + { + GlobalRegionID ID = m_NextGlobalRegionID++; + + globalRegion.insert({ reg, ID }); + // Avoid creating an empty link if possible, FindReachableRegions uses [] which calls the default constructor. + if (edges.find(reg) != edges.end()) + { + std::set reachable; + FindReachableRegions(reg, reachable, passClass); + for (const RegionID& region : reachable) + globalRegion.insert({ region, ID }); + } + } + } } if (m_DebugOverlay) @@ -411,12 +437,22 @@ { PROFILE3("Hierarchical Update"); + ASSERT(m_NextGlobalRegionID < std::numeric_limits::max()); + + std::map > needNewGlobalRegionMap; + // Algorithm for the partial update: // 1. Loop over chunks. // 2. For any dirty chunk: + // - remove all regions from the global region map // - remove all edges, by removing the neighbor connection with them and then deleting us // - recreate regions inside the chunk // - reconnect the regions. We may do too much work if we reconnect with a dirty chunk, but that's fine. + // 3. Recreate global regions. + // This means that if any chunk changes, we may need to flood (at most once) the whole map. + // That's quite annoying, but I can't think of an easy way around it. + // If we could be sure that a region's topology hasn't changed, we could skip removing its global region + // but that's non trivial as we have no easy way to determine said topology (regions could "switch" IDs on update for now). for (u8 cj = 0; cj < m_ChunksH; ++cj) { int j0 = cj * CHUNK_SIZE; @@ -434,11 +470,12 @@ pass_class_t passClass = passClassMask.second; Chunk& a = m_Chunks[passClass].at(ci + cj*m_ChunksW); - // Clean up edges ID + // Clean up edges and global region ID EdgesMap& edgeMap = m_Edges[passClass]; for (u16 i : a.m_RegionsID) { RegionID reg{ci, cj, i}; + m_GlobalRegions[passClass].erase(reg); for (const RegionID& neighbor : edgeMap[reg]) { edgeMap[neighbor].erase(reg); @@ -451,12 +488,16 @@ // Recompute regions inside this chunk. a.InitRegions(ci, cj, grid, passClass); + for (u16 i : a.m_RegionsID) + needNewGlobalRegionMap[passClass].push_back(RegionID{ci, cj, i}); UpdateEdges(ci, cj, passClass, edgeMap); } } } + UpdateGlobalRegions(needNewGlobalRegionMap); + if (m_DebugOverlay) { m_DebugOverlayLines.clear(); @@ -580,6 +621,28 @@ } } +void HierarchicalPathfinder::UpdateGlobalRegions(const std::map >& needNewGlobalRegionMap) +{ + // Use FindReachableRegions because we cannot be sure, even if we find a non-dirty chunk nearby, + // that we weren't the only bridge connecting that chunk to the rest of the global region. + for (const std::pair >& regionsInNeed : needNewGlobalRegionMap) + for (const RegionID& reg : regionsInNeed.second) + { + std::map& globalRegions = m_GlobalRegions[regionsInNeed.first]; + // If we have already been given a region, skip us. + if (globalRegions.find(reg) != globalRegions.end()) + continue; + + std::set reachable; + FindReachableRegions(reg, reachable, regionsInNeed.first); + + GlobalRegionID ID = m_NextGlobalRegionID++; + + for (const RegionID& reg : reachable) + globalRegions[reg] = ID; + } +} + HierarchicalPathfinder::RegionID HierarchicalPathfinder::Get(u16 i, u16 j, pass_class_t passClass) const { int ci = i / CHUNK_SIZE; @@ -588,73 +651,73 @@ return m_Chunks.at(passClass)[cj*m_ChunksW + ci].Get(i % CHUNK_SIZE, j % CHUNK_SIZE); } -void HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const +HierarchicalPathfinder::GlobalRegionID HierarchicalPathfinder::GetGlobalRegion(u16 i, u16 j, pass_class_t passClass) const { - PROFILE2("MakeGoalReachable"); - RegionID source = Get(i0, j0, passClass); + return GetGlobalRegion(Get(i, j, passClass), passClass); +} - // Find everywhere that's reachable - std::set reachableRegions; - FindReachableRegions(source, reachableRegions, passClass); +HierarchicalPathfinder::GlobalRegionID HierarchicalPathfinder::GetGlobalRegion(RegionID region, pass_class_t passClass) const +{ + return region.r == 0 ? GlobalRegionID(0) : m_GlobalRegions.at(passClass).at(region); +} - // Check whether any reachable region contains the goal - // And get the navcell that's the closest to the point +void CreatePointGoalAt(u16 i, u16 j, PathGoal& goal) +{ + PathGoal newGoal; + newGoal.type = PathGoal::POINT; + Pathfinding::NavcellCenter(i, j, newGoal.x, newGoal.z); + goal = newGoal; +} - u16 bestI = 0; - u16 bestJ = 0; - u32 dist2Best = std::numeric_limits::max(); +void HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const +{ + PROFILE2("MakeGoalReachable"); - 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 iGoal, jGoal; + Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H); - u16 i,j; - u32 dist2; + bool reachable = false; + std::set goalRegions; + FindGoalRegions(iGoal, jGoal, goal, goalRegions, passClass); - // 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; - } + // Add all reachable goal regions to the set of regions we want to look at. + std::set interestingRegions; + for (const RegionID& r : goalRegions) + if (GetGlobalRegion(r, passClass) == GetGlobalRegion(i0, j0, passClass)) + { + reachable = true; + interestingRegions.insert(r); } - } - // 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; + // If the goal is unreachable, expand to all reachable regions. + if (!reachable) + { + FindReachableRegions(Get(i0, j0, passClass), interestingRegions, passClass); + FindNearestNavcellInRegions(interestingRegions, iGoal, jGoal, passClass); + CreatePointGoalAt(iGoal, jGoal, goal); return; } - ENSURE(dist2Best != std::numeric_limits::max()); - PathGoal newGoal; - newGoal.type = PathGoal::POINT; - Pathfinding::NavcellCenter(bestI, bestJ, newGoal.x, newGoal.z); - goal = newGoal; + if (goal.type == PathGoal::POINT) + return; // Reachable point goal, no need to move it. + + u16 bestI = 0, bestJ = 0; + u32 dist2Best = std::numeric_limits::max(); + + // Test each reachable goal region and return the navcell closest to the cneter. + for (const RegionID& region : interestingRegions) + { + u16 ri, rj; + u32 dist; + GetChunk(region.ci, region.cj, passClass).RegionNearestNavcellInGoal(region.r, i0, j0, goal, ri, rj, dist); + if (dist < dist2Best) + { + bestI = ri; + bestJ = rj; + dist2Best = dist; + } + } + return CreatePointGoalAt(bestI, bestJ, goal); } void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) const @@ -756,6 +819,38 @@ regions.insert(RegionID(chunk.m_ChunkI, chunk.m_ChunkJ, r)); } +void HierarchicalPathfinder::FindGoalRegions(u16 gi, u16 gj, const PathGoal& goal, std::set& regions, pass_class_t passClass) const +{ + 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 { ENSURE(grid.m_W == m_W && grid.m_H == m_H);