/* Copyright (C) 2021 Wildfire Games.
* This file is part of 0 A.D.
*
* 0 A.D. is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 2 of the License, or
* (at your option) any later version.
*
* 0 A.D. is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with 0 A.D. If not, see .
*/
#include "precompiled.h"
#include "HierarchicalPathfinder.h"
#include "graphics/Overlay.h"
#include "ps/Profile.h"
#include "renderer/Scene.h"
#include "simulation2/helpers/Grid.h"
// Find the root ID of a region, used by InitRegions
inline u16 RootID(u16 x, const std::vector& v)
{
while (v[x] < x)
x = v[x];
return x;
}
void HierarchicalPathfinder::Chunk::InitRegions(int ci, int cj, Grid* grid, pass_class_t passClass)
{
ENSURE(ci < 256 && cj < 256); // avoid overflows
m_ChunkI = ci;
m_ChunkJ = cj;
memset(m_Regions, 0, sizeof(m_Regions));
int i0 = ci * CHUNK_SIZE;
int j0 = cj * CHUNK_SIZE;
int i1 = std::min(i0 + CHUNK_SIZE, (int)grid->m_W);
int j1 = std::min(j0 + CHUNK_SIZE, (int)grid->m_H);
// Efficiently flood-fill the m_Regions grid
int regionID = 0;
std::vector connect;
u16* pCurrentID = NULL;
u16 LeftID = 0;
u16 DownID = 0;
bool Checked = false; // prevent some unneccessary RootID calls
connect.reserve(32); // TODO: What's a sensible number?
connect.push_back(0); // connect[0] = 0
// Start by filling the grid with 0 for blocked,
// and regionID for unblocked
for (int j = j0; j < j1; ++j)
{
for (int i = i0; i < i1; ++i)
{
pCurrentID = &m_Regions[j-j0][i-i0];
if (!IS_PASSABLE(grid->get(i, j), passClass))
{
*pCurrentID = 0;
continue;
}
if (j > j0)
DownID = m_Regions[j-1-j0][i-i0];
if (i == i0)
LeftID = 0;
else
LeftID = m_Regions[j-j0][i-1-i0];
if (LeftID > 0)
{
*pCurrentID = LeftID;
if (*pCurrentID != DownID && DownID > 0 && !Checked)
{
u16 id0 = RootID(DownID, connect);
u16 id1 = RootID(LeftID, connect);
Checked = true; // this avoids repeatedly connecting the same IDs
if (id0 < id1)
connect[id1] = id0;
else if (id0 > id1)
connect[id0] = id1;
}
else if (DownID == 0)
Checked = false;
}
else if (DownID > 0)
{
*pCurrentID = DownID;
Checked = false;
}
else
{
// New ID
*pCurrentID = ++regionID;
connect.push_back(regionID);
Checked = false;
}
}
}
// Mark connected regions as being the same ID (i.e. the lowest)
m_RegionsID.clear();
for (u16 i = 1; i < regionID+1; ++i)
{
if (connect[i] != i)
connect[i] = RootID(i, connect);
else
m_RegionsID.push_back(connect[i]);
}
// Replace IDs by the root ID
for (int j = 0; j < CHUNK_SIZE; ++j)
for (int i = 0; i < CHUNK_SIZE; ++i)
m_Regions[j][i] = connect[m_Regions[j][i]];
}
/**
* Returns a RegionID for the given global navcell coords
* (which must be inside this chunk);
*/
HierarchicalPathfinder::RegionID HierarchicalPathfinder::Chunk::Get(int i, int j) const
{
ENSURE(i < CHUNK_SIZE && j < CHUNK_SIZE);
return RegionID(m_ChunkI, m_ChunkJ, m_Regions[j][i]);
}
/**
* Return the global navcell coords that correspond roughly to the
* center of the given region in this chunk.
* (This is not guaranteed to be actually inside the region.)
*/
void HierarchicalPathfinder::Chunk::RegionCenter(u16 r, int& i_out, int& j_out) const
{
// Find the mean of i,j coords of navcells in this region:
u32 si = 0, sj = 0; // sum of i,j coords
u32 n = 0; // number of navcells in region
cassert(CHUNK_SIZE < 256); // conservative limit to ensure si and sj don't overflow
for (int j = 0; j < CHUNK_SIZE; ++j)
{
for (int i = 0; i < CHUNK_SIZE; ++i)
{
if (m_Regions[j][i] == r)
{
si += i;
sj += j;
n += 1;
}
}
}
// Avoid divide-by-zero
if (n == 0)
n = 1;
i_out = m_ChunkI * CHUNK_SIZE + si / n;
j_out = m_ChunkJ * CHUNK_SIZE + sj / n;
}
/**
* Returns the global navcell coords, and the squared distance to the goal
* navcell, of whichever navcell inside the given region is closest to
* that goal.
*/
void HierarchicalPathfinder::Chunk::RegionNavcellNearest(u16 r, int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const
{
iBest = 0;
jBest = 0;
dist2Best = std::numeric_limits::max();
for (int j = 0; j < CHUNK_SIZE; ++j)
{
for (int i = 0; i < CHUNK_SIZE; ++i)
{
if (m_Regions[j][i] != r)
continue;
u32 dist2 = (i + m_ChunkI*CHUNK_SIZE - iGoal)*(i + m_ChunkI*CHUNK_SIZE - iGoal)
+ (j + m_ChunkJ*CHUNK_SIZE - jGoal)*(j + m_ChunkJ*CHUNK_SIZE - jGoal);
if (dist2 < dist2Best)
{
iBest = i + m_ChunkI*CHUNK_SIZE;
jBest = j + m_ChunkJ*CHUNK_SIZE;
dist2Best = dist2;
}
}
}
}
/**
* 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::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:
{
// i and j can be equal to CHUNK_SIZE on the top and right borders of the map,
// specially when mapSize is a multiple of CHUNK_SIZE
int i = std::min((int)CHUNK_SIZE - 1, gi - m_ChunkI * CHUNK_SIZE);
int j = std::min((int)CHUNK_SIZE - 1, gj - m_ChunkJ * CHUNK_SIZE);
if (m_Regions[j][i] == 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::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::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;
}
HierarchicalPathfinder::HierarchicalPathfinder() : m_DebugOverlay(NULL)
{
}
HierarchicalPathfinder::~HierarchicalPathfinder()
{
SAFE_DELETE(m_DebugOverlay);
}
void HierarchicalPathfinder::SetDebugOverlay(bool enabled, const CSimContext* simContext)
{
if (enabled && !m_DebugOverlay)
{
m_DebugOverlay = new HierarchicalOverlay(*this);
m_DebugOverlayLines.clear();
m_SimContext = simContext;
AddDebugEdges(GetPassabilityClass("default"));
}
else if (!enabled && m_DebugOverlay)
{
SAFE_DELETE(m_DebugOverlay);
m_DebugOverlayLines.clear();
m_SimContext = NULL;
}
}
void HierarchicalPathfinder::RenderSubmit(SceneCollector& collector)
{
if (!m_DebugOverlay)
return;
for (size_t i = 0; i < m_DebugOverlayLines.size(); ++i)
collector.Submit(&m_DebugOverlayLines[i]);
}
void HierarchicalPathfinder::Recompute(Grid* grid,
const std::map& nonPathfindingPassClassMasks,
const std::map& pathfindingPassClassMasks)
{
PROFILE2("Hierarchical Recompute");
m_PassClassMasks = pathfindingPassClassMasks;
std::map allPassClasses = m_PassClassMasks;
allPassClasses.insert(nonPathfindingPassClassMasks.begin(), nonPathfindingPassClassMasks.end());
m_W = grid->m_W;
m_H = grid->m_H;
ENSURE((grid->m_W + CHUNK_SIZE - 1) / CHUNK_SIZE < 256 && (grid->m_H + CHUNK_SIZE - 1) / CHUNK_SIZE < 256); // else the u8 Chunk::m_ChunkI will overflow
// Divide grid into chunks with round-to-positive-infinity
m_ChunksW = (grid->m_W + CHUNK_SIZE - 1) / CHUNK_SIZE;
m_ChunksH = (grid->m_H + CHUNK_SIZE - 1) / CHUNK_SIZE;
m_Chunks.clear();
m_Edges.clear();
// Reset global regions.
m_NextGlobalRegionID = 1;
for (auto& passClassMask : allPassClasses)
{
pass_class_t passClass = passClassMask.second;
// Compute the regions within each chunk
m_Chunks[passClass].resize(m_ChunksW*m_ChunksH);
for (int cj = 0; cj < m_ChunksH; ++cj)
{
for (int ci = 0; ci < m_ChunksW; ++ci)
{
m_Chunks[passClass].at(cj*m_ChunksW + ci).InitRegions(ci, cj, grid, passClass);
}
}
// Construct the search graph over the regions.
EdgesMap& edges = m_Edges[passClass];
RecomputeAllEdges(passClass, edges);
// Spread global regions.
std::map& globalRegion = m_GlobalRegions[passClass];
globalRegion.clear();
for (u8 cj = 0; cj < m_ChunksH; ++cj)
for (u8 ci = 0; ci < m_ChunksW; ++ci)
for (u16 rid : GetChunk(ci, cj, passClass).m_RegionsID)
{
RegionID reg{ci,cj,rid};
if (globalRegion.find(reg) == globalRegion.end())
{
GlobalRegionID ID = m_NextGlobalRegionID++;
globalRegion.insert({ reg, ID });
// Avoid creating an empty link if possible, FindReachableRegions uses [] which calls the default constructor.
if (edges.find(reg) != edges.end())
{
std::set reachable;
FindReachableRegions(reg, reachable, passClass);
for (const RegionID& region : reachable)
globalRegion.insert({ region, ID });
}
}
}
}
if (m_DebugOverlay)
{
m_DebugOverlayLines.clear();
AddDebugEdges(GetPassabilityClass("default"));
}
}
void HierarchicalPathfinder::Update(Grid* grid, const Grid& dirtinessGrid)
{
PROFILE3("Hierarchical Update");
ASSERT(m_NextGlobalRegionID < std::numeric_limits::max());
std::map > needNewGlobalRegionMap;
// Algorithm for the partial update:
// 1. Loop over chunks.
// 2. For any dirty chunk:
// - remove all regions from the global region map
// - remove all edges, by removing the neighbor connection with them and then deleting us
// - recreate regions inside the chunk
// - reconnect the regions. We may do too much work if we reconnect with a dirty chunk, but that's fine.
// 3. Recreate global regions.
// This means that if any chunk changes, we may need to flood (at most once) the whole map.
// That's quite annoying, but I can't think of an easy way around it.
// If we could be sure that a region's topology hasn't changed, we could skip removing its global region
// but that's non trivial as we have no easy way to determine said topology (regions could "switch" IDs on update for now).
for (u8 cj = 0; cj < m_ChunksH; ++cj)
{
int j0 = cj * CHUNK_SIZE;
int j1 = std::min(j0 + CHUNK_SIZE, (int)dirtinessGrid.m_H);
for (u8 ci = 0; ci < m_ChunksW; ++ci)
{
// Skip chunks where no navcells are dirty.
int i0 = ci * CHUNK_SIZE;
int i1 = std::min(i0 + CHUNK_SIZE, (int)dirtinessGrid.m_W);
if (!dirtinessGrid.any_set_in_square(i0, j0, i1, j1))
continue;
for (const std::pair& passClassMask : m_PassClassMasks)
{
pass_class_t passClass = passClassMask.second;
Chunk& a = m_Chunks[passClass].at(ci + cj*m_ChunksW);
// Clean up edges and global region ID
EdgesMap& edgeMap = m_Edges[passClass];
for (u16 i : a.m_RegionsID)
{
RegionID reg{ci, cj, i};
m_GlobalRegions[passClass].erase(reg);
for (const RegionID& neighbor : edgeMap[reg])
{
edgeMap[neighbor].erase(reg);
if (edgeMap[neighbor].empty())
edgeMap.erase(neighbor);
}
edgeMap.erase(reg);
}
// Recompute regions inside this chunk.
a.InitRegions(ci, cj, grid, passClass);
for (u16 i : a.m_RegionsID)
needNewGlobalRegionMap[passClass].push_back(RegionID{ci, cj, i});
UpdateEdges(ci, cj, passClass, edgeMap);
}
}
}
UpdateGlobalRegions(needNewGlobalRegionMap);
if (m_DebugOverlay)
{
m_DebugOverlayLines.clear();
AddDebugEdges(GetPassabilityClass("default"));
}
}
void HierarchicalPathfinder::ComputeNeighbors(EdgesMap& edges, Chunk& a, Chunk& b, bool transpose, bool opposite) const
{
// For each edge between chunks, we loop over every adjacent pair of
// navcells in the two chunks. If they are both in valid regions
// (i.e. are passable navcells) then add a graph edge between those regions.
// (We don't need to test for duplicates since EdgesMap already uses a
// std::set which will drop duplicate entries.)
// But as set.insert can be quite slow on large collection, and that we usually
// try to insert the same values, we cache the previous one for a fast test.
RegionID raPrev(0,0,0);
RegionID rbPrev(0,0,0);
for (int k = 0; k < CHUNK_SIZE; ++k)
{
u8 aSide = opposite ? CHUNK_SIZE - 1 : 0;
u8 bSide = CHUNK_SIZE - 1 - aSide;
RegionID ra = transpose ? a.Get(k, aSide) : a.Get(aSide, k);
RegionID rb = transpose ? b.Get(k, bSide) : b.Get(bSide, k);
if (ra.r && rb.r)
{
if (ra == raPrev && rb == rbPrev)
continue;
edges[ra].insert(rb);
edges[rb].insert(ra);
raPrev = ra;
rbPrev = rb;
}
}
}
/**
* Connect a chunk's regions to their neighbors. Not optimised for global recomputing.
*/
void HierarchicalPathfinder::UpdateEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges)
{
std::vector& chunks = m_Chunks[passClass];
Chunk& a = chunks.at(cj*m_ChunksW + ci);
if (ci > 0)
ComputeNeighbors(edges, a, chunks.at(cj*m_ChunksW + (ci-1)), false, false);
if (ci < m_ChunksW-1)
ComputeNeighbors(edges, a, chunks.at(cj*m_ChunksW + (ci+1)), false, true);
if (cj > 0)
ComputeNeighbors(edges, a, chunks.at((cj-1)*m_ChunksW + ci), true, false);
if (cj < m_ChunksH - 1)
ComputeNeighbors(edges, a, chunks.at((cj+1)*m_ChunksW + ci), true, true);
}
/**
* Find edges between regions in all chunks, in an optimised manner (only look at top/left)
*/
void HierarchicalPathfinder::RecomputeAllEdges(pass_class_t passClass, EdgesMap& edges)
{
std::vector& chunks = m_Chunks[passClass];
edges.clear();
for (int cj = 0; cj < m_ChunksH; ++cj)
{
for (int ci = 0; ci < m_ChunksW; ++ci)
{
Chunk& a = chunks.at(cj*m_ChunksW + ci);
if (ci > 0)
ComputeNeighbors(edges, a, chunks.at(cj*m_ChunksW + (ci-1)), false, false);
if (cj > 0)
ComputeNeighbors(edges, a, chunks.at((cj-1)*m_ChunksW + ci), true, false);
}
}
}
/**
* Debug visualisation of graph edges between regions.
*/
void HierarchicalPathfinder::AddDebugEdges(pass_class_t passClass)
{
const EdgesMap& edges = m_Edges[passClass];
const std::vector& chunks = m_Chunks[passClass];
for (auto& edge : edges)
{
for (const RegionID& region: edge.second)
{
// Draw a line between the two regions' centers
int i0, j0, i1, j1;
chunks[edge.first.cj * m_ChunksW + edge.first.ci].RegionCenter(edge.first.r, i0, j0);
chunks[region.cj * m_ChunksW + region.ci].RegionCenter(region.r, i1, j1);
CFixedVector2D a, b;
Pathfinding::NavcellCenter(i0, j0, a.X, a.Y);
Pathfinding::NavcellCenter(i1, j1, b.X, b.Y);
// Push the endpoints inwards a little to avoid overlaps
CFixedVector2D d = b - a;
d.Normalize(entity_pos_t::FromInt(1));
a += d;
b -= d;
std::vector xz;
xz.push_back(a.X.ToFloat());
xz.push_back(a.Y.ToFloat());
xz.push_back(b.X.ToFloat());
xz.push_back(b.Y.ToFloat());
m_DebugOverlayLines.emplace_back();
m_DebugOverlayLines.back().m_Color = CColor(1.0, 1.0, 1.0, 1.0);
SimRender::ConstructLineOnGround(*m_SimContext, xz, m_DebugOverlayLines.back(), true);
}
}
}
void HierarchicalPathfinder::UpdateGlobalRegions(const std::map >& needNewGlobalRegionMap)
{
// Use FindReachableRegions because we cannot be sure, even if we find a non-dirty chunk nearby,
// that we weren't the only bridge connecting that chunk to the rest of the global region.
for (const std::pair>& regionsInNeed : needNewGlobalRegionMap)
{
for (const RegionID& reg : regionsInNeed.second)
{
std::map& globalRegions = m_GlobalRegions[regionsInNeed.first];
// If we have already been given a region, skip us.
if (globalRegions.find(reg) != globalRegions.end())
continue;
std::set reachable;
FindReachableRegions(reg, reachable, regionsInNeed.first);
GlobalRegionID ID = m_NextGlobalRegionID++;
for (const RegionID& regionId : reachable)
globalRegions[regionId] = ID;
}
}
}
HierarchicalPathfinder::RegionID HierarchicalPathfinder::Get(u16 i, u16 j, pass_class_t passClass) const
{
int ci = i / CHUNK_SIZE;
int cj = j / CHUNK_SIZE;
ENSURE(ci < m_ChunksW && cj < m_ChunksH);
return m_Chunks.at(passClass)[cj*m_ChunksW + ci].Get(i % CHUNK_SIZE, j % CHUNK_SIZE);
}
HierarchicalPathfinder::GlobalRegionID HierarchicalPathfinder::GetGlobalRegion(u16 i, u16 j, pass_class_t passClass) const
{
return GetGlobalRegion(Get(i, j, passClass), passClass);
}
HierarchicalPathfinder::GlobalRegionID HierarchicalPathfinder::GetGlobalRegion(RegionID region, pass_class_t passClass) const
{
return region.r == 0 ? GlobalRegionID(0) : m_GlobalRegions.at(passClass).at(region);
}
void CreatePointGoalAt(u16 i, u16 j, PathGoal& goal)
{
PathGoal newGoal;
newGoal.type = PathGoal::POINT;
Pathfinding::NavcellCenter(i, j, newGoal.x, newGoal.z);
goal = newGoal;
}
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);
std::set 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);
// 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))
{
iGoal = region.bestI;
jGoal = region.bestJ;
// No need to move reachable point goals.
if (goal.type != PathGoal::POINT)
CreatePointGoalAt(iGoal, jGoal, goal);
return true;
}
// Goal wasn't reachable - get the closest navcell in the nearest reachable region.
std::set reachableRegions(SortByCenterToPoint(iGoal, jGoal));
FindReachableRegions(Get(i0, j0, passClass), reachableRegions, passClass);
FindNearestNavcellInRegions(reachableRegions, iGoal, jGoal, passClass);
CreatePointGoalAt(iGoal, jGoal, goal);
return false;
}
bool HierarchicalPathfinder::IsGoalReachable(u16 i0, u16 j0, const PathGoal& goal, pass_class_t passClass) const
{
PROFILE2("IsGoalReachable");
u16 iGoal, jGoal;
Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H);
std::set 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);
// 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))
return true;
return false;
}
void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) const
{
std::set 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& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const
{
u16 bestI = iGoal, bestJ = jGoal; // Somewhat sensible default-values should regions() be passed empty.
u32 bestDist = std::numeric_limits::max();
// 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 sqrt(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;
// TODO: update to static_assert with constexpr
ENSURE(maxDistFromBest < std::numeric_limits::max());
maxDistFromBest *= maxDistFromBest;
for (const RegionID& region : regions)
{
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.
// It's also a bit pessimistic, so we'll still consider a few too many regions.
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)
{
bestI = ri;
bestJ = rj;
bestDist = dist;
}
}
iGoal = bestI;
jGoal = bestJ;
}
void HierarchicalPathfinder::FindGoalRegionsAndBestNavcells(u16 i0, u16 j0, u16 gi, u16 gj, const PathGoal& goal, std::set& regions, pass_class_t passClass) const
{
if (goal.type == PathGoal::POINT)
{
RegionID region = Get(gi, gj, passClass);
if (region.r > 0)
regions.insert({region, gi, gj});
return;
}
// For non-point cases, we'll test each region inside the bounds of the goal.
// we might occasionally test a few too many for circles but it's not too bad.
// Note that this also works in the Inverse-circle / Inverse-square case
// Since our ranges are inclusive, we will necessarily test at least the perimeter/outer bound of the goal.
// If we find a navcell, great, if not, well then we'll be surrounded by an impassable barrier.
// Since in the Inverse-XX case we're supposed to start inside, then we can't ever reach the goal so it's good enough.
// It's not worth it to skip the "inner" regions since we'd need ranges above CHUNK_SIZE for that to start mattering
// (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 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, i0, j0, goal, bestI, bestJ, c))
regions.insert({RegionID{sx, sz, i}, bestI, bestJ});
}
}
void HierarchicalPathfinder::FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid& grid) const
{
ENSURE(grid.m_W == m_W && grid.m_H == m_H);
int i0 = region.ci * CHUNK_SIZE;
int j0 = region.cj * CHUNK_SIZE;
const Chunk& c = m_Chunks.at(passClass)[region.cj * m_ChunksW + region.ci];
for (int j = 0; j < CHUNK_SIZE; ++j)
for (int i = 0; i < CHUNK_SIZE; ++i)
if (c.m_Regions[j][i] == region.r)
grid.set(i0 + i, j0 + j, value);
}
Grid HierarchicalPathfinder::GetConnectivityGrid(pass_class_t passClass) const
{
Grid connectivityGrid(m_W, m_H);
connectivityGrid.reset();
u16 idx = 1;
for (u16 i = 0; i < m_W; ++i)
{
for (u16 j = 0; j < m_H; ++j)
{
if (connectivityGrid.get(i, j) != 0)
continue;
RegionID from = Get(i, j, passClass);
if (from.r == 0)
continue;
std::set reachable;
FindReachableRegions(from, reachable, passClass);
for (const RegionID& region : reachable)
FillRegionOnGrid(region, passClass, idx, connectivityGrid);
++idx;
}
}
return connectivityGrid;
}