/**
* Returns the global navcell coords, and the squared distance to the (i0,j0)
* navcell, of whichever navcell inside the given region and inside the given goal
* is closest to (i0,j0)
*/
void HierarchicalPathfinder::Chunk::RegionNearestNavcellInGoal(u16 r, u16 i0, u16 j0, const PathGoal& goal, u16& iOut, u16& jOut, u32& dist2Best) const
{
iOut = 0;
jOut = 0;
dist2Best = std::numeric_limits<u32>::max();
for (int j = 0; j < CHUNK_SIZE; ++j)
{
for (int i = 0; i < CHUNK_SIZE; ++i)
{
if (m_Regions[j][i] != r || !goal.NavcellContainsGoal(i + m_ChunkI*CHUNK_SIZE, j + m_ChunkJ*CHUNK_SIZE))
continue;
u32 dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0)
+ (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0);
if (dist2 < dist2Best)
{
iOut = i + m_ChunkI*CHUNK_SIZE;
jOut = j + m_ChunkJ*CHUNK_SIZE;
dist2Best = dist2;
}
}
}
}
/**
* Returns whether any navcell in the given region is inside the goal.
*/
bool HierarchicalPathfinder::Chunk::RegionContainsGoal(u16 r, const PathGoal& goal) const
{
// Inefficiently check every single navcell:
for (u16 j = 0; j < CHUNK_SIZE; ++j)
{
for (u16 i = 0; i < CHUNK_SIZE; ++i)
{
if (m_Regions[j][i] == r)
{
if (goal.NavcellContainsGoal(m_ChunkI * CHUNK_SIZE + i, m_ChunkJ * CHUNK_SIZE + j))
return true;
}
}
}
return false;
}
bool HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass)
{
RegionID source = Get(i0, j0, passClass);
// Find everywhere that's reachable
std::set<RegionID> reachableRegions;
FindReachableRegions(source, reachableRegions, passClass);
// Check whether any reachable region contains the goal
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;
// If the region contains the goal area, the goal is reachable
// and we don't need to move it
if (GetChunk(region.ci, region.cj, passClass).RegionContainsGoal(region.r, goal))
return false;
}
// The goal area wasn't reachable,
// so find the navcell that's nearest to the goal's center
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;
return true;
}
void HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass)
{
PROFILE2("MakeGoalReachable");
RegionID source = Get(i0, j0, passClass);
// Find everywhere that's reachable
std::set<RegionID> reachableRegions;
FindReachableRegions(source, reachableRegions, passClass);
// Check whether any reachable region contains the goal
// And get the navcell that's the closest to the point
u16 bestI = 0;
u16 bestJ = 0;
u32 dist2Best = std::numeric_limits<u32>::max();
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 i,j;
u32 dist2;
// 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;
}
}
}
// If the goal area wasn't reachable,
// find the navcell that's nearest to the goal's center
if (dist2Best == std::numeric_limits<u32>::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;
return;
}
ENSURE(dist2Best != std::numeric_limits<u32>::max());
PathGoal newGoal;
newGoal.type = PathGoal::POINT;
Pathfinding::NavcellCenter(bestI, bestJ, newGoal.x, newGoal.z);
goal = newGoal;
}
/**
* Gives the global navcell coords, and the squared distance to the (i0,j0)
* navcell, of whichever navcell inside the given region and inside the given goal
* is closest to (i0,j0)
* Returns true if the goal is inside the region, false otherwise.
*/
bool HierarchicalPathfinder::Chunk::RegionNearestNavcellInGoal(u16 r, u16 i0, u16 j0, const PathGoal& goal, u16& iOut, u16& jOut, u32& dist2Best) const
{
// TODO: this should be optimized further.
// Most used cases empirically seem to be SQUARE, INVERTED_CIRCLE and then POINT and CIRCLE somehwat equally
iOut = 0;
jOut = 0;
dist2Best = std::numeric_limits<u32>::max();
// Calculate the navcell that contains the center of the goal.
int gi = (goal.x >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
int gj = (goal.z >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
switch(goal.type)
{
case PathGoal::POINT:
{
if (m_Regions[gj-m_ChunkJ * CHUNK_SIZE][gi-m_ChunkI * CHUNK_SIZE] == r)
{
iOut = gi;
jOut = gj;
dist2Best = (gi - i0)*(gi - i0)
+ (gj - j0)*(gj - j0);
return true;
}
return false;
}
case PathGoal::CIRCLE:
case PathGoal::SQUARE:
{
// restrict ourselves to a square surrounding the goal.
int radius = (std::max(goal.hw*3/2,goal.hh*3/2) >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToInfinity();
int imin = std::max(0, gi-m_ChunkI*CHUNK_SIZE-radius);
int imax = std::min((int)CHUNK_SIZE, gi-m_ChunkI*CHUNK_SIZE+radius+1);
int jmin = std::max(0, gj-m_ChunkJ*CHUNK_SIZE-radius);
int jmax = std::min((int)CHUNK_SIZE, gj-m_ChunkJ*CHUNK_SIZE+radius+1);
bool found = false;
u32 dist2 = std::numeric_limits<u32>::max();
for (u16 j = jmin; j < jmax; ++j)
{
for (u16 i = imin; i < imax; ++i)
{
if (m_Regions[j][i] != r)
continue;
if (found)
{
dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0)
+ (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0);
if (dist2 >= dist2Best)
continue;
}
if (goal.NavcellContainsGoal(m_ChunkI * CHUNK_SIZE + i, m_ChunkJ * CHUNK_SIZE + j))
{
if (!found)
{
found = true;
dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0)
+ (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0);
}
iOut = i + m_ChunkI*CHUNK_SIZE;
jOut = j + m_ChunkJ*CHUNK_SIZE;
dist2Best = dist2;
}
}
}
return found;
}
case PathGoal::INVERTED_CIRCLE:
case PathGoal::INVERTED_SQUARE:
{
bool found = false;
u32 dist2 = std::numeric_limits<u32>::max();
// loop over all navcells.
for (u16 j = 0; j < CHUNK_SIZE; ++j)
{
for (u16 i = 0; i < CHUNK_SIZE; ++i)
{
if (m_Regions[j][i] != r)
continue;
if (found)
{
dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0)
+ (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0);
if (dist2 >= dist2Best)
continue;
}
if (goal.NavcellContainsGoal(m_ChunkI * CHUNK_SIZE + i, m_ChunkJ * CHUNK_SIZE + j))
{
if (!found)
{
found = true;
dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0)
+ (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0);
}
iOut = i + m_ChunkI*CHUNK_SIZE;
jOut = j + m_ChunkJ*CHUNK_SIZE;
dist2Best = dist2;
}
}
}
return found;
}
}
return false;
}
void HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass)
{
RegionID source = Get(i0, j0, passClass);
// Find everywhere that's reachable
std::set<RegionID> reachableRegions;
FindReachableRegions(source, reachableRegions, passClass);
// Check whether any reachable region contains the goal
std::set<RegionID> reachableContainingGoal;
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;
// If the region contains the goal area, the goal is reachable
if (GetChunk(region.ci, region.cj, passClass).RegionContainsGoal(region.r, goal))
{
// If it's a point, no need to move it, we're done
if (goal.type == PathGoal::POINT)
return;
// Else remember this region
reachableContainingGoal.insert(region);
}
}
// If the goal area wasn't reachable,
// find the navcell that's nearest to the goal's center
if (reachableContainingGoal.empty())
{
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;
return;
}
// Non-point goals:
// For each region, find the closest navcell to (i0,j0) which is also in the goal,
// and use the best of all regions
ENSURE(goal.type != PathGoal::POINT);
u16 bestI, bestJ;
u32 dist2Best = std::numeric_limits<u32>::max();
for (const RegionID& region : reachableContainingGoal)
{
u16 i, j;
u32 dist2;
GetChunk(region.ci, region.cj, passClass).RegionNearestNavcellInGoal(region.r, i0, j0, goal, i, j, dist2);
if (dist2 < dist2Best)
{
bestI = i;
bestJ = j;
dist2Best = dist2;
}
}
PathGoal newGoal;
newGoal.type = PathGoal::POINT;
Pathfinding::NavcellCenter(bestI, bestJ, newGoal.x, newGoal.z);
goal = newGoal;
}