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 @@ -388,11 +388,12 @@ // from the left of the C, goal is unreachable, expect closest navcell to goal oi = 5 * 5 + 3; oj = 3 * 5 + 3; - pi = 5 * 5 + 3; pj = 7 * 5 + 2; goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj); + pi = 5 * 5 + 3; pj = 6 * 5 + 2; goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj); hierPath.MakeGoalReachable(oi, oj, goal, PASS_1); hierPath.FindNearestPassableNavcell(pi, pj, PASS_1); - TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity()); + TS_ASSERT_EQUALS(pi, goal.x.ToInt_RoundToNegInfinity()); + TS_ASSERT_EQUALS(pj, goal.z.ToInt_RoundToNegInfinity()); // random reachable point. oi = 5 * 5 + 3; oj = 3 * 5 + 3; @@ -465,6 +466,7 @@ pi = 36 * 5 + 3; pj = 7 * 5 + 2; goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj); hierPath.MakeGoalReachable(oi, oj, goal, PASS_1); + // bit of leeway for cell placement TS_ASSERT(std::fabs(euclidian(goal.x.ToInt_RoundToNegInfinity(), goal.z.ToInt_RoundToNegInfinity(), pi, pj)-20) < 1.5f); TS_ASSERT(std::fabs(euclidian(goal.x.ToInt_RoundToNegInfinity(), goal.z.ToInt_RoundToNegInfinity(), oi, oj) - euclidian(pi, pj, oi, oj)) < 22.0f); Index: ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.h =================================================================== --- ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.h +++ ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.h @@ -87,6 +87,14 @@ { return ((ci == b.ci) && (cj == b.cj) && (r == b.r)); } + + // Returns the distance from the center to the point (i, j) + inline u32 DistanceTo(u16 i, u16 j) const + { + return (ci * CHUNK_SIZE + CHUNK_SIZE/2 - i) * (ci * CHUNK_SIZE + CHUNK_SIZE/2 - i) + + (cj * CHUNK_SIZE + CHUNK_SIZE/2 - j) * (cj * CHUNK_SIZE + CHUNK_SIZE/2 - j); + } + }; HierarchicalPathfinder(); @@ -115,8 +123,10 @@ * * 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) @@ -171,6 +181,11 @@ #endif }; + const Chunk& GetChunk(u8 ci, u8 cj, pass_class_t passClass) const + { + return m_Chunks.at(passClass).at(cj * m_ChunksW + ci); + } + typedef std::map > EdgesMap; void ComputeNeighbors(EdgesMap& edges, Chunk& a, Chunk& b, bool transpose, bool opposite) const; @@ -179,23 +194,51 @@ void UpdateGlobalRegions(const std::map >& needNewGlobalRegionMap); - void FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass) const; + /** + * Returns all reachable regions, optionally ordered in a specific manner. + */ + template + void FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass) const; - void FindPassableRegions(std::set& regions, pass_class_t passClass) const; + struct SortByCenterToPoint + { + SortByCenterToPoint(u16 i, u16 j): gi(i), gj(j) {}; + bool operator()(const HierarchicalPathfinder::RegionID& a, const HierarchicalPathfinder::RegionID& b) const + { + if (a.DistanceTo(gi, gj) < b.DistanceTo(gi, gj)) + return true; + if (a.DistanceTo(gi, gj) > b.DistanceTo(gi, gj)) + return false; + return a.r < b.r; + } + u16 gi, gj; + }; - void FindGoalRegions(u16 gi, u16 gj, const PathGoal& goal, std::set& regions, pass_class_t passClass) const; + void FindNearestNavcellInRegions(const std::set& regions, + u16& iGoal, u16& jGoal, 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. - * (Assumes @p regions is non-empty.) - */ - void FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const; + struct InterestingRegion { + RegionID region; + u16 bestI; + u16 bestJ; + }; - const Chunk& GetChunk(u8 ci, u8 cj, pass_class_t passClass) const + struct SortByBestToPoint { - return m_Chunks.at(passClass).at(cj * m_ChunksW + ci); - } + SortByBestToPoint(u16 i, u16 j): gi(i), gj(j) {}; + bool operator()(const InterestingRegion& a, const InterestingRegion& b) const + { + if ((a.bestI - gi) * (a.bestI - gi) + (a.bestJ - gj) * (a.bestJ - gj) < (b.bestI - gi) * (b.bestI - gi) + (b.bestJ - gj) * (b.bestJ - gj)) + return true; + if ((a.bestI - gi) * (a.bestI - gi) + (a.bestJ - gj) * (a.bestJ - gj) > (b.bestI - gi) * (b.bestI - gi) + (b.bestJ - gj) * (b.bestJ - gj)) + return false; + return a.region.r < b.region.r; + } + u16 gi, gj; + }; + + // Returns the region along with the best cell for optimisation. + void FindGoalRegionsAndBestNavcells(u16 i0, u16 j0, u16 gi, u16 gj, const PathGoal& goal, std::set& regions, pass_class_t passClass) const; void FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid& grid) const; Index: ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.cpp =================================================================== --- ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.cpp +++ ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.cpp @@ -669,122 +669,86 @@ goal = newGoal; } -void HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const +bool HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const { PROFILE2("MakeGoalReachable"); u16 iGoal, jGoal; Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H); - bool reachable = false; - std::set goalRegions; - FindGoalRegions(iGoal, jGoal, goal, goalRegions, passClass); - - // 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); + std::set goalRegions(SortByBestToPoint(i0, j0)); + // This returns goal regions ordered by distance from the best navcell in each region. + FindGoalRegionsAndBestNavcells(i0, j0, iGoal, jGoal, goal, goalRegions, passClass); + + // Because of the sorting above, we can stop as soon as the first reachable goal region is found. + for (const InterestingRegion& region : goalRegions) + if (GetGlobalRegion(region.region, passClass) == GetGlobalRegion(i0, j0, passClass)) + { + iGoal = region.bestI; + jGoal = region.bestJ; + + // No need to move reachable point goals. + if (goal.type != PathGoal::POINT) + CreatePointGoalAt(iGoal, jGoal, goal); + return true; } - // 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; - } + // Goal wasn't reachable - get the closest navcell in the nearest reachable region. + std::set reachableRegions(SortByCenterToPoint(i0, j0)); + FindReachableRegions(Get(i0, j0, passClass), reachableRegions, passClass); - 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); + FindNearestNavcellInRegions(reachableRegions, iGoal, jGoal, passClass); + CreatePointGoalAt(iGoal, jGoal, goal); + return false; } void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) const { - std::set regions; - FindPassableRegions(regions, passClass); + std::set regions(SortByCenterToPoint(i, j)); + + // 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)); + FindNearestNavcellInRegions(regions, i, j, passClass); } -void HierarchicalPathfinder::FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const +void HierarchicalPathfinder::FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const { - // Find the navcell in the given regions that's nearest to the goal navcell: - // * For each region, record the (squared) minimal distance to the goal point - // * Sort regions by that underestimated distance - // * For each region, find the actual nearest navcell - // * Stop when the underestimated distances are worse than the best real distance + u16 bestI, bestJ; + u32 bestDist = std::numeric_limits::max(); - std::vector > regionDistEsts; // pair of (distance^2, region) + // Because regions are sorted by increasing distance, we can ignore regions that are obviously farther than the current best point. + // Since regions are squares, that happens when the center of a region is at least √2 * CHUNK_SIZE farther than the current best point. + // Add one to avoid cases where the center navcell is actually slightly off-center (= CHUNK_SIZE is even) + u32 maxDistFromBest = (fixed::FromInt(3) / 2 * CHUNK_SIZE).ToInt_RoundToInfinity() + 1; + ENSURE(maxDistFromBest < std::numeric_limits::max()); + maxDistFromBest *= maxDistFromBest; for (const RegionID& region : regions) { - int i0 = region.ci * CHUNK_SIZE; - int j0 = region.cj * CHUNK_SIZE; - int i1 = i0 + CHUNK_SIZE - 1; - int j1 = j0 + CHUNK_SIZE - 1; - - // Pick the point in the chunk nearest the goal - int iNear = Clamp((int)iGoal, i0, i1); - int jNear = Clamp((int)jGoal, j0, j1); - - int dist2 = (iNear - iGoal)*(iNear - iGoal) - + (jNear - jGoal)*(jNear - jGoal); - - regionDistEsts.emplace_back(dist2, region); - } - - // Sort by increasing distance (tie-break on RegionID) - std::sort(regionDistEsts.begin(), regionDistEsts.end()); - - int iBest = iGoal; - int jBest = jGoal; - u32 dist2Best = std::numeric_limits::max(); - - for (auto& pair : regionDistEsts) - { - if (pair.first >= dist2Best) - break; - - RegionID region = pair.second; - - int i, j; - u32 dist2; - GetChunk(region.ci, region.cj, passClass).RegionNavcellNearest(region.r, iGoal, jGoal, i, j, dist2); + u32 chunkDist = region.DistanceTo(iGoal, jGoal); + // This might overflow, but only if we are already close to the maximal possible distance, so the condition would probably be false anyways. + if (bestDist < std::numeric_limits::max() && chunkDist > maxDistFromBest + bestDist) + break; // Break, the set is ordered by increased distance so a closer region will not be found. - if (dist2 < dist2Best) + int ri, rj; + u32 dist; + GetChunk(region.ci, region.cj, passClass).RegionNavcellNearest(region.r, iGoal, jGoal, ri, rj, dist); + if (dist < bestDist) { - iBest = i; - jBest = j; - dist2Best = dist2; + bestI = ri; + bestJ = rj; + bestDist = dist; } } - - iGoal = iBest; - jGoal = jBest; + iGoal = bestI; + jGoal = bestJ; } -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 @@ -811,21 +775,13 @@ } } -void HierarchicalPathfinder::FindPassableRegions(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)); -} - -void HierarchicalPathfinder::FindGoalRegions(u16 gi, u16 gj, const PathGoal& goal, std::set& regions, pass_class_t passClass) const +void HierarchicalPathfinder::FindGoalRegionsAndBestNavcells(u16 i0, u16 j0, 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); + regions.insert({region, gi, gj}); return; } @@ -839,15 +795,16 @@ // (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 + u16 bestI, bestJ; + u32 c; // Unused. 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}); + if (chunk.RegionNearestNavcellInGoal(i, i0, j0, goal, bestI, bestJ, c)) + regions.insert({RegionID{sx, sz, i}, bestI, bestJ}); } }