/**
* 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;
}
/**
* 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;
}