Index: source/simulation2/helpers/HierarchicalPathfinder.h =================================================================== --- source/simulation2/helpers/HierarchicalPathfinder.h +++ 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) @@ -179,18 +189,32 @@ 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; + template + 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; + template + 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. - * (Assumes @p regions is non-empty.) - */ - void FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const; + struct SortByClosestToPoint + { + SortByClosestToPoint() = delete; + SortByClosestToPoint(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 FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const; const Chunk& GetChunk(u8 ci, u8 cj, pass_class_t passClass) const { Index: source/simulation2/helpers/HierarchicalPathfinder.cpp =================================================================== --- source/simulation2/helpers/HierarchicalPathfinder.cpp +++ source/simulation2/helpers/HierarchicalPathfinder.cpp @@ -669,7 +669,7 @@ 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"); @@ -680,8 +680,9 @@ 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; + // Sort interesting regions by distance go the goal for optimisations later + std::set interestingRegions(SortByClosestToPoint(iGoal, jGoal)); + for (const RegionID& r : goalRegions) if (GetGlobalRegion(r, passClass) == GetGlobalRegion(i0, j0, passClass)) { @@ -689,102 +690,60 @@ interestingRegions.insert(r); } + if (reachable && goal.type == PathGoal::POINT) + return true; // Reachable point goal, no need to move it. + // 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; - } - 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(interestingRegions, iGoal, jGoal, passClass); + CreatePointGoalAt(iGoal, jGoal, goal); + return reachable; } void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) const { - std::set regions; + std::set regions(SortByClosestToPoint(i, j)); FindPassableRegions(regions, passClass); 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); - - if (dist2 < dist2Best) + 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. + + 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,7 +770,8 @@ } } -void HierarchicalPathfinder::FindPassableRegions(std::set& regions, pass_class_t passClass) const +template +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)) @@ -819,7 +779,8 @@ 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 +template +void HierarchicalPathfinder::FindGoalRegions(u16 gi, u16 gj, const PathGoal& goal, std::set& regions, pass_class_t passClass) const { if (goal.type == PathGoal::POINT) {