Optimise MakeGoalReachable and FindNearestNavcellInRegions (D53 outtake)
By leveraging custom ordering in std::set, MakeGoalReachable and FindNearestNavcellInRegions can be optimised and the code simplified. Differential Revision: https://code.wildfiregames.com/D1882 This was SVN commit r22817.
This commit is contained in:
parent
30dcd696eb
commit
208fc30ddd
@ -388,11 +388,12 @@ public:
|
||||
|
||||
// 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 @@ public:
|
||||
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);
|
||||
|
||||
@ -669,122 +669,86 @@ void CreatePointGoalAt(u16 i, u16 j, PathGoal& goal)
|
||||
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<RegionID> goalRegions;
|
||||
FindGoalRegions(iGoal, jGoal, goal, goalRegions, passClass);
|
||||
std::set<InterestingRegion, SortByBestToPoint> 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);
|
||||
|
||||
// Add all reachable goal regions to the set of regions we want to look at.
|
||||
std::set<RegionID> interestingRegions;
|
||||
for (const RegionID& r : goalRegions)
|
||||
if (GetGlobalRegion(r, passClass) == GetGlobalRegion(i0, j0, 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))
|
||||
{
|
||||
reachable = true;
|
||||
interestingRegions.insert(r);
|
||||
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<RegionID, SortByCenterToPoint> 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<u32>::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<RegionID> regions;
|
||||
FindPassableRegions(regions, passClass);
|
||||
std::set<RegionID, SortByCenterToPoint> 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<RegionID>& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const
|
||||
void HierarchicalPathfinder::FindNearestNavcellInRegions(const std::set<RegionID, SortByCenterToPoint>& 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<u32>::max();
|
||||
|
||||
std::vector<std::pair<u32, RegionID> > 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<u16>::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;
|
||||
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<u32>::max() && chunkDist > maxDistFromBest + bestDist)
|
||||
break; // Break, the set is ordered by increased distance so a closer region will not be found.
|
||||
|
||||
// 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<u32>::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)
|
||||
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<RegionID>& reachable, pass_class_t passClass) const
|
||||
template<typename Ordering>
|
||||
void HierarchicalPathfinder::FindReachableRegions(RegionID from, std::set<RegionID, Ordering>& 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::FindReachableRegions(RegionID from, std::set<Region
|
||||
}
|
||||
}
|
||||
|
||||
void HierarchicalPathfinder::FindPassableRegions(std::set<RegionID>& 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<HierarchicalPathfinder::RegionID>& regions, pass_class_t passClass) const
|
||||
void HierarchicalPathfinder::FindGoalRegionsAndBestNavcells(u16 i0, u16 j0, u16 gi, u16 gj, const PathGoal& goal, std::set<InterestingRegion, SortByBestToPoint>& 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 @@ void HierarchicalPathfinder::FindGoalRegions(u16 gi, u16 gj, const PathGoal& goa
|
||||
// (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});
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -87,6 +87,14 @@ public:
|
||||
{
|
||||
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 @@ public:
|
||||
*
|
||||
* 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 @@ private:
|
||||
#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<RegionID, std::set<RegionID> > EdgesMap;
|
||||
|
||||
void ComputeNeighbors(EdgesMap& edges, Chunk& a, Chunk& b, bool transpose, bool opposite) const;
|
||||
@ -179,23 +194,51 @@ private:
|
||||
|
||||
void UpdateGlobalRegions(const std::map<pass_class_t, std::vector<RegionID> >& needNewGlobalRegionMap);
|
||||
|
||||
void FindReachableRegions(RegionID from, std::set<RegionID>& reachable, pass_class_t passClass) const;
|
||||
|
||||
void FindPassableRegions(std::set<RegionID>& regions, pass_class_t passClass) const;
|
||||
|
||||
void FindGoalRegions(u16 gi, u16 gj, const PathGoal& goal, std::set<RegionID>& 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.)
|
||||
* Returns all reachable regions, optionally ordered in a specific manner.
|
||||
*/
|
||||
void FindNearestNavcellInRegions(const std::set<RegionID>& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const;
|
||||
template<typename Ordering>
|
||||
void FindReachableRegions(RegionID from, std::set<RegionID, Ordering>& reachable, pass_class_t passClass) const;
|
||||
|
||||
const Chunk& GetChunk(u8 ci, u8 cj, pass_class_t passClass) const
|
||||
struct SortByCenterToPoint
|
||||
{
|
||||
return m_Chunks.at(passClass).at(cj * m_ChunksW + ci);
|
||||
}
|
||||
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 FindNearestNavcellInRegions(const std::set<RegionID, SortByCenterToPoint>& regions,
|
||||
u16& iGoal, u16& jGoal, pass_class_t passClass) const;
|
||||
|
||||
struct InterestingRegion {
|
||||
RegionID region;
|
||||
u16 bestI;
|
||||
u16 bestJ;
|
||||
};
|
||||
|
||||
struct SortByBestToPoint
|
||||
{
|
||||
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<InterestingRegion, SortByBestToPoint>& regions, pass_class_t passClass) const;
|
||||
|
||||
void FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid<u16>& grid) const;
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user