Differential D1882 Diff 8004 source/simulation2/helpers/HierarchicalPathfinder.cpp

Changeset View

Standalone View

source/simulation2/helpers/HierarchicalPathfinder.cpp

Show First 20 Lines • Show All 660 Lines • ▼ Show 20 Lines
void CreatePointGoalAt(u16 i, u16 j, PathGoal& goal)		void CreatePointGoalAt(u16 i, u16 j, PathGoal& goal)
{		{
PathGoal newGoal;		PathGoal newGoal;
newGoal.type = PathGoal::POINT;		newGoal.type = PathGoal::POINT;
Pathfinding::NavcellCenter(i, j, newGoal.x, newGoal.z);		Pathfinding::NavcellCenter(i, j, newGoal.x, newGoal.z);
goal = newGoal;		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");		PROFILE2("MakeGoalReachable");

u16 iGoal, jGoal;		u16 iGoal, jGoal;
Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H);		Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H);

bool reachable = false;		bool reachable = false;
std::set<RegionID> goalRegions;		std::set<RegionID> goalRegions;
FindGoalRegions(iGoal, jGoal, goal, goalRegions, passClass);		FindGoalRegions(iGoal, jGoal, goal, goalRegions, passClass);
wraitiiAuthorUnsubmitted Done Inline Actions Notice that this computed the best cell for each region, but discarded that data, then recomputes it below. wraitii: Notice that this computed the best cell for each region, but discarded that data, then…

// Add all reachable goal regions to the set of regions we want to look at.		// Sort interesting regions by distance go the goal for optimisations later
std::set<RegionID> interestingRegions;		std::set<RegionID, SortByClosestToPoint> interestingRegions(SortByClosestToPoint(iGoal, jGoal));

for (const RegionID& r : goalRegions)		for (const RegionID& r : goalRegions)
if (GetGlobalRegion(r, passClass) == GetGlobalRegion(i0, j0, passClass))		if (GetGlobalRegion(r, passClass) == GetGlobalRegion(i0, j0, passClass))
{		{
		wraitiiAuthorUnsubmitted Done Inline Actions Because now we store more data, we can return in a single, very cheap loop if the goal is reachable. wraitii: Because now we store more data, we can return in a single, very cheap loop if the goal is…
reachable = true;		reachable = true;
interestingRegions.insert(r);		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 the goal is unreachable, expand to all reachable regions.
if (!reachable)		if (!reachable)
{
FindReachableRegions(Get(i0, j0, passClass), interestingRegions, passClass);		FindReachableRegions(Get(i0, j0, passClass), interestingRegions, passClass);

FindNearestNavcellInRegions(interestingRegions, iGoal, jGoal, passClass);		FindNearestNavcellInRegions(interestingRegions, iGoal, jGoal, passClass);
CreatePointGoalAt(iGoal, jGoal, goal);		CreatePointGoalAt(iGoal, jGoal, goal);
return;		return reachable;
}

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);
}		}

void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) const		void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) const
{		{
std::set<RegionID> regions;		std::set<RegionID, SortByClosestToPoint> regions(SortByClosestToPoint(i, j));
FindPassableRegions(regions, passClass);		FindPassableRegions(regions, passClass);
FindNearestNavcellInRegions(regions, i, j, passClass);		FindNearestNavcellInRegions(regions, i, j, passClass);
}		}
		wraitiiAuthorUnsubmitted Done Inline Actions FindPassableRegions inlined (and deleted) since it only was called here, was quite short, and the name was sort of confusing (at first sight, it seems similar to FindReachableRegions, but it's really not). wraitii: FindPassableRegions inlined (and deleted) since it only was called here, was quite short, and…

void HierarchicalPathfinder::FindNearestNavcellInRegions(const std::set<RegionID>& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const		void HierarchicalPathfinder::FindNearestNavcellInRegions(const std::set<RegionID, SortByClosestToPoint>& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const
		wraitiiAuthorUnsubmitted Done Inline Actions Explanation for why this is an optimisation: Since the set is already ordered by underestimated distance, we don't have to recompute such distances, this removes a loop. The actual break logic is similar. wraitii: Explanation for why this is an optimisation: Since the set is already ordered by underestimated…
{		{
// Find the navcell in the given regions that's nearest to the goal navcell:		u16 bestI, bestJ;
// * For each region, record the (squared) minimal distance to the goal point		u32 bestDist = std::numeric_limits<u32>::max();
// * 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

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)		for (const RegionID& region : regions)
{		{
int i0 = region.ci * CHUNK_SIZE;		u32 chunkDist = region.DistanceTo(iGoal, jGoal);
int j0 = region.cj * CHUNK_SIZE;		// This might overflow, but only if we are already close to the maximal possible distance, so the condition would probably be false anyways.
int i1 = i0 + CHUNK_SIZE - 1;		if (bestDist < std::numeric_limits<u32>::max() && chunkDist > maxDistFromBest + bestDist)
int j1 = j0 + CHUNK_SIZE - 1;		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 ri, rj;
		u32 dist;
int i, j;		GetChunk(region.ci, region.cj, passClass).RegionNavcellNearest(region.r, iGoal, jGoal, ri, rj, dist);
u32 dist2;		if (dist < bestDist)
GetChunk(region.ci, region.cj, passClass).RegionNavcellNearest(region.r, iGoal, jGoal, i, j, dist2);

if (dist2 < dist2Best)
{		{
iBest = i;		bestI = ri;
jBest = j;		bestJ = rj;
dist2Best = dist2;		bestDist = dist;
}		}
}		}
		iGoal = bestI;
iGoal = iBest;		jGoal = bestJ;
jGoal = jBest;
}		}

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',		// Flood-fill the region graph, starting at 'from',
// collecting all the regions that are reachable via edges		// collecting all the regions that are reachable via edges
reachable.insert(from);		reachable.insert(from);

const EdgesMap& edgeMap = m_Edges.at(passClass);		const EdgesMap& edgeMap = m_Edges.at(passClass);
if (edgeMap.find(from) == edgeMap.end())		if (edgeMap.find(from) == edgeMap.end())
return;		return;
Show All 10 Lines	while (!open.empty())
for (const RegionID& region : edgeMap.at(curr))		for (const RegionID& region : edgeMap.at(curr))
// Add to the reachable set; if this is the first time we added		// Add to the reachable set; if this is the first time we added
// it then also add it to the open list		// it then also add it to the open list
if (reachable.insert(region).second)		if (reachable.insert(region).second)
open.push_back(region);		open.push_back(region);
}		}
}		}

void HierarchicalPathfinder::FindPassableRegions(std::set<RegionID>& regions, pass_class_t passClass) const		template<typename Ordering>
		void HierarchicalPathfinder::FindPassableRegions(std::set<RegionID, Ordering>& regions, pass_class_t passClass) const
{		{
// Construct a set of all regions of all chunks for this pass class		// Construct a set of all regions of all chunks for this pass class
for (const Chunk& chunk : m_Chunks.at(passClass))		for (const Chunk& chunk : m_Chunks.at(passClass))
for (int r : chunk.m_RegionsID)		for (int r : chunk.m_RegionsID)
regions.insert(RegionID(chunk.m_ChunkI, chunk.m_ChunkJ, r));		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		template<typename Ordering>
		void HierarchicalPathfinder::FindGoalRegions(u16 gi, u16 gj, const PathGoal& goal, std::set<RegionID, Ordering>& regions, pass_class_t passClass) const
{		{
if (goal.type == PathGoal::POINT)		if (goal.type == PathGoal::POINT)
{		{
RegionID region = Get(gi, gj, passClass);		RegionID region = Get(gi, gj, passClass);
if (region.r > 0)		if (region.r > 0)
regions.insert(region);		regions.insert(region);
return;		return;
}		}
Show All 12 Lines	void HierarchicalPathfinder::FindGoalRegions(u16 gi, u16 gj, const PathGoal& goal, std::set<RegionID, Ordering>& regions, pass_class_t passClass) const

for (u8 sz = std::max(0,(gj - size) / CHUNK_SIZE); sz <= std::min(m_ChunksH-1, (gj + size + 1) / CHUNK_SIZE); ++sz)		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)		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);		const Chunk& chunk = GetChunk(sx, sz, passClass);
for (u16 i : chunk.m_RegionsID)		for (u16 i : chunk.m_RegionsID)
if (chunk.RegionNearestNavcellInGoal(i, 0, 0, goal, a, b, c))		if (chunk.RegionNearestNavcellInGoal(i, 0, 0, goal, a, b, c))
regions.insert(RegionID{sx, sz, i});		regions.insert(RegionID{sx, sz, i});
}		}
		wraitiiAuthorUnsubmitted Done Inline Actions We are now using the result of `RegionNearestNavcellInGoal`, which is a rather expensive operation. wraitii: We are now using the result of `RegionNearestNavcellInGoal`, which is a rather expensive…
}		}

void HierarchicalPathfinder::FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid<u16>& grid) const		void HierarchicalPathfinder::FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid<u16>& grid) const
{		{
ENSURE(grid.m_W == m_W && grid.m_H == m_H);		ENSURE(grid.m_W == m_W && grid.m_H == m_H);

int i0 = region.ci * CHUNK_SIZE;		int i0 = region.ci * CHUNK_SIZE;
int j0 = region.cj * CHUNK_SIZE;		int j0 = region.cj * CHUNK_SIZE;
Show All 39 Lines