Differential D53 Diff 6525 source/simulation2/helpers/HierarchicalPathfinder.cpp

Changeset View

Standalone View

source/simulation2/helpers/HierarchicalPathfinder.cpp

Show All 16 Lines

#include "precompiled.h"		#include "precompiled.h"

#include "HierarchicalPathfinder.h"		#include "HierarchicalPathfinder.h"

#include "graphics/Overlay.h"		#include "graphics/Overlay.h"
#include "ps/Profile.h"		#include "ps/Profile.h"

		#include "Geometry.h"

// Find the root ID of a region, used by InitRegions		// Find the root ID of a region, used by InitRegions
inline u16 RootID(u16 x, const std::vector<u16>& v)		inline u16 RootID(u16 x, const std::vector<u16>& v)
{		{
while (v[x] < x)		while (v[x] < x)
x = v[x];		x = v[x];

return x;		return x;
}		}

void HierarchicalPathfinder::Chunk::InitRegions(int ci, int cj, Grid<NavcellData>* grid, pass_class_t passClass)		void HierarchicalPathfinder::Chunk::InitRegions(int ci, int cj, Grid<NavcellData>* grid, pass_class_t passClass)
{		{
ENSURE(ci < 256 && cj < 256); // avoid overflows		ENSURE(ci < 256 && cj < 256); // avoid overflows
m_ChunkI = ci;		m_ChunkI = ci;
m_ChunkJ = cj;		m_ChunkJ = cj;

memset(m_Regions, 0, sizeof(m_Regions));		memset(m_Regions, 0, sizeof(m_Regions));

		m_RegionsID.clear();

int i0 = ci * CHUNK_SIZE;		int i0 = ci * CHUNK_SIZE;
int j0 = cj * CHUNK_SIZE;		int j0 = cj * CHUNK_SIZE;
int i1 = std::min(i0 + CHUNK_SIZE, (int)grid->m_W);		int i1 = std::min(i0 + CHUNK_SIZE, (int)grid->m_W);
int j1 = std::min(j0 + CHUNK_SIZE, (int)grid->m_H);		int j1 = std::min(j0 + CHUNK_SIZE, (int)grid->m_H);

// Efficiently flood-fill the m_Regions grid		// Efficiently flood-fill the m_Regions grid

int regionID = 0;		int regionID = 0;
▲ Show 20 Lines • Show All 55 Lines • ▼ Show 20 Lines	for (int i = i0; i < i1; ++i)
// New ID		// New ID
*pCurrentID = ++regionID;		*pCurrentID = ++regionID;
connect.push_back(regionID);		connect.push_back(regionID);
Checked = false;		Checked = false;
}		}
}		}
}		}

// Directly point the root ID		// Mark connected regions as being the same ID (i.e. the lowest)
m_NumRegions = 0;
for (u16 i = 1; i < regionID+1; ++i)		for (u16 i = 1; i < regionID+1; ++i)
{		{
if (connect[i] == i)		if (connect[i] != i)
++m_NumRegions;
else
connect[i] = RootID(i, connect);		connect[i] = RootID(i, connect);
		else
		m_RegionsID.push_back(connect[i]);
		templeUnsubmitted Done Inline Actions Was there a problem with the old way? Just `else m_RegionsID.push_back(i)`? temple: Was there a problem with the old way? Just `else m_RegionsID.push_back(i)`?
		wraitiiAuthorUnsubmitted Done Inline Actions I honestly don't really remember, so I'll get back to you on this, but I believe since I'm using a vector for m_RegionsID I need to check for duplicatas. wraitii: I honestly don't really remember, so I'll get back to you on this, but I believe since I'm…
		wraitiiAuthorUnsubmitted Done Inline Actions After checking, I do indeed need the unicity check here: there could be several i for which RootID(i, connect) returns the same value, and thus the m_RegionsID vector would have duplicates. I guess I could just transform it in a separate pass, but in general this shouldn't really be a speed issue. wraitii: After checking, I do indeed need the unicity check here: there could be several i for which…
		templeUnsubmitted Done Inline Actions I meant if (connect[i] != i) connect[i] = RootID(i, connect); else m_RegionsID.push_back(i); And this won't have duplicates. However, on `Update` you call `InitRegions`, and it doesn't clear `m_RegionsID`, so in that case it would be adding duplicates to the end of the vector. Currently you add new ids but don't delete the unused ones, I don't know if this was your intent. temple: I meant ``` if (connect[i] != i) connect[i] = RootID(i, connect); else m_RegionsID.push_back…
		wraitiiAuthorUnsubmitted Done Inline Actions I think I may have just misunderstood what this code was doing... I've changed it to what you suggest and also reset m_RegionsID explicitly when this is called. wraitii: I think I may have just misunderstood what this code was doing... I've changed it to what you…
}		}

// Replace IDs by the root ID		// Replace IDs by the root ID
for (int j = 0; j < CHUNK_SIZE; ++j)		for (int j = 0; j < CHUNK_SIZE; ++j)
for (int i = 0; i < CHUNK_SIZE; ++i)		for (int i = 0; i < CHUNK_SIZE; ++i)
m_Regions[j][i] = connect[m_Regions[j][i]];		m_Regions[j][i] = connect[m_Regions[j][i]];
}		}

Show All 39 Lines	if (n == 0)
n = 1;		n = 1;

i_out = m_ChunkI * CHUNK_SIZE + si / n;		i_out = m_ChunkI * CHUNK_SIZE + si / n;
j_out = m_ChunkJ * CHUNK_SIZE + sj / n;		j_out = m_ChunkJ * CHUNK_SIZE + sj / n;
}		}

/**		/**
* Returns the global navcell coords, and the squared distance to the goal		* Returns the global navcell coords, and the squared distance to the goal
		* navcell, of whichever navcell inside the given chunk is closest to
		* that goal.
		*/
		void HierarchicalPathfinder::Chunk::NearestNavcell(int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const
		{
		iBest = 0;
		jBest = 0;
		dist2Best = std::numeric_limits<u32>::max();
		u32 rowBest = std::numeric_limits<u32>::max();

		// TODO: it might be faster to try the best navcell first, since we can compute that.
		for (int j = 0; j < CHUNK_SIZE; ++j)
		{
		for (int i = 0; i < CHUNK_SIZE; ++i)
		{
		if (m_Regions[j][i] == 0)
		continue;

		/**
		* Line-point distance. Over a single row, dist2 is first monotonically decreasing, then monotonically increasing
		* Thus once we stop improving, we can safely break.
		* However, we cannot compare across lines because obstructed chunks may skew numbers.
		* Imagine a situation like this (G is the goal, X a passable cell, - an impassable one):
		* ----G-
		* -X----
		* X---X-
		* Despite the first X on the third row being farther, the third row has a better navcell.
		*/
		u32 dist2 = (i + m_ChunkICHUNK_SIZE - iGoal)(i + m_ChunkI*CHUNK_SIZE - iGoal)
		+ (j + m_ChunkJCHUNK_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;
		rowBest = dist2;
		}
		else if (dist2 < rowBest)
		rowBest = dist2;
		else
		break;
		}
		}
		}

		/**
		* Returns the global navcell coords, and the squared distance to the goal
* navcell, of whichever navcell inside the given region is closest to		* navcell, of whichever navcell inside the given region is closest to
* that goal.		* that goal.
*/		*/
void HierarchicalPathfinder::Chunk::RegionNavcellNearest(u16 r, int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const		void HierarchicalPathfinder::Chunk::RegionNavcellNearest(u16 r, int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const
		wraitiiAuthorUnsubmitted Not Done Inline Actions Need to figure out if I'm still using this. wraitii: Need to figure out if I'm still using this.
{		{
iBest = 0;		iBest = 0;
jBest = 0;		jBest = 0;
dist2Best = std::numeric_limits<u32>::max();		dist2Best = std::numeric_limits<u32>::max();

for (int j = 0; j < CHUNK_SIZE; ++j)		for (int j = 0; j < CHUNK_SIZE; ++j)
{		{
for (int i = 0; i < CHUNK_SIZE; ++i)		for (int i = 0; i < CHUNK_SIZE; ++i)
{		{
if (m_Regions[j][i] != r)		if (m_Regions[j][i] != r)
continue;		continue;

u32 dist2 = (i + m_ChunkICHUNK_SIZE - iGoal)(i + m_ChunkI*CHUNK_SIZE - iGoal)		u32 dist2 = (i + m_ChunkICHUNK_SIZE - iGoal)(i + m_ChunkI*CHUNK_SIZE - iGoal)
+ (j + m_ChunkJCHUNK_SIZE - jGoal)(j + m_ChunkJ*CHUNK_SIZE - jGoal);		+ (j + m_ChunkJCHUNK_SIZE - jGoal)(j + m_ChunkJ*CHUNK_SIZE - jGoal);

if (dist2 < dist2Best)		if (dist2 < dist2Best)
{		{
iBest = i + m_ChunkI*CHUNK_SIZE;		iBest = i + m_ChunkI*CHUNK_SIZE;
jBest = j + m_ChunkJ*CHUNK_SIZE;		jBest = j + m_ChunkJ*CHUNK_SIZE;
dist2Best = dist2;		dist2Best = dist2;
}		}
}		}
}		}
}		}
		templeUnsubmitted Done Inline Actions Within each row the distance-squared will be monotone (it'll get smaller then get larger), but I don't think you can't compare from one row to the next. temple: Within each row the distance-squared will be monotone (it'll get smaller then get larger), but…
		wraitiiAuthorUnsubmitted Done Inline Actions Hm, I'm not, am I? Been a while since I've coded this, but the break is only for the inner loop I believe? wraitii: Hm, I'm not, am I? Been a while since I've coded this, but the break is only for the inner loop…
		wraitiiAuthorUnsubmitted Done Inline Actions This is referring to L203 (phabricator messed up). As said above, I'm indeed only breaking out of the inner loop, so the monotony applies unless I've done something stupid. wraitii: This is referring to L203 (phabricator messed up). As said above, I'm indeed only breaking out…
		templeUnsubmitted Done Inline Actions But `dist2Best` is for all rows. So it'll find the best the best value in the first row but after that it could quit early with `i=0` because that value of `dist2` is too large even those some bigger `i` could produce a `dist2 < dist2Best`. temple: But `dist2Best` is for all rows. So it'll find the best the best value in the first row but…
		wraitiiAuthorUnsubmitted Not Done Inline Actions Ah, I finally get what you mean. You're right, thanks for noticing this. I forgot to account for blocked navcells (otherwise we would also have row-wise monotony, but then the problem is trivial anyways) wraitii: Ah, I finally get what you mean. You're right, thanks for noticing this. I forgot to account…

/**		/**
* Gives the global navcell coords, and the squared distance to the (i0,j0)		* 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		* navcell, of whichever navcell inside the given region and inside the given goal
* is closest to (i0,j0)		* is closest to (i0,j0)
* Returns true if the goal is inside the region, false otherwise.		* 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		bool HierarchicalPathfinder::Chunk::RegionNearestNavcellInGoal(u16 r, u16 i0, u16 j0, const PathGoal& goal, u16& iOut, u16& jOut, u32& dist2Best) const
Show All 11 Lines	bool HierarchicalPathfinder::Chunk::RegionNearestNavcellInGoal(u16 r, u16 i0, u16 j0, const PathGoal& goal, u16& iOut, u16& jOut, u32& dist2Best) const
switch(goal.type)		switch(goal.type)
{		{
case PathGoal::POINT:		case PathGoal::POINT:
{		{
// i and j can be equal to CHUNK_SIZE on the top and right borders of the map,		// 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		// specially when mapSize is a multiple of CHUNK_SIZE
int i = std::min((int)CHUNK_SIZE - 1, gi - m_ChunkI * 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);		int j = std::min((int)CHUNK_SIZE - 1, gj - m_ChunkJ * CHUNK_SIZE);
if (m_Regions[j][i] == r)		if (m_Regions[j][i] == r)
{		{
		templeUnsubmitted Done Inline Actions I think it's possible (but rare) to send in gi/gj on the exact top/right side of the chunk, and I'm not sure we want to automatically return false in those cases. See D926. temple: I think it's possible (but rare) to send in gi/gj on the exact top/right side of the chunk, and…
		wraitiiAuthorUnsubmitted Done Inline Actions mh, I thought I rebased on your patch but it seems not, so yeah that's a regression I suppose, I'll fix it. wraitii: mh, I thought I rebased on your patch but it seems not, so yeah that's a regression I suppose…
iOut = gi;		iOut = gi;
jOut = gj;		jOut = gj;
dist2Best = (gi - i0)*(gi - i0)		dist2Best = (gi - i0)*(gi - i0)
+ (gj - j0)*(gj - j0);		+ (gj - j0)*(gj - j0);
return true;		return true;
}		}
return false;		return false;
}		}
▲ Show 20 Lines • Show All 115 Lines • ▼ Show 20 Lines	void HierarchicalPathfinder::Recompute(Grid<NavcellData>* grid,
m_PassClassMasks = pathfindingPassClassMasks;		m_PassClassMasks = pathfindingPassClassMasks;

std::map<std::string, pass_class_t> allPassClasses = m_PassClassMasks;		std::map<std::string, pass_class_t> allPassClasses = m_PassClassMasks;
allPassClasses.insert(nonPathfindingPassClassMasks.begin(), nonPathfindingPassClassMasks.end());		allPassClasses.insert(nonPathfindingPassClassMasks.begin(), nonPathfindingPassClassMasks.end());

m_W = grid->m_W;		m_W = grid->m_W;
m_H = grid->m_H;		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		// Divide grid into chunks with round-to-positive-infinity
m_ChunksW = (grid->m_W + CHUNK_SIZE - 1) / CHUNK_SIZE;		m_ChunksW = (grid->m_W + CHUNK_SIZE - 1) / CHUNK_SIZE;
m_ChunksH = (grid->m_H + CHUNK_SIZE - 1) / CHUNK_SIZE;		m_ChunksH = (grid->m_H + CHUNK_SIZE - 1) / CHUNK_SIZE;

ENSURE(m_ChunksW < 256 && m_ChunksH < 256); // else the u8 Chunk::m_ChunkI will overflow

m_Chunks.clear();		m_Chunks.clear();
m_Edges.clear();		m_Edges.clear();

		// reset global regions
		m_NextGlobalRegionID = 1;

for (auto& passClassMask : allPassClasses)		for (auto& passClassMask : allPassClasses)
{		{
pass_class_t passClass = passClassMask.second;		pass_class_t passClass = passClassMask.second;

// Compute the regions within each chunk		// Compute the regions within each chunk
m_Chunks[passClass].resize(m_ChunksW*m_ChunksH);		m_Chunks[passClass].resize(m_ChunksW*m_ChunksH);
for (int cj = 0; cj < m_ChunksH; ++cj)		for (int cj = 0; cj < m_ChunksH; ++cj)
{		{
for (int ci = 0; ci < m_ChunksW; ++ci)		for (int ci = 0; ci < m_ChunksW; ++ci)
{		{
m_Chunks[passClass].at(cj*m_ChunksW + ci).InitRegions(ci, cj, grid, passClass);		m_Chunks[passClass].at(cj*m_ChunksW + ci).InitRegions(ci, cj, grid, passClass);
}		}
}		}

// Construct the search graph over the regions		// Construct the search graph over the regions

EdgesMap& edges = m_Edges[passClass];		EdgesMap& edges = m_Edges[passClass];
		RecomputeAllEdges(passClass, edges);

for (int cj = 0; cj < m_ChunksH; ++cj)		// Spread global regions.
{		std::map<RegionID, GlobalRegionID>& globalRegion = m_GlobalRegions[passClass];
for (int ci = 0; ci < m_ChunksW; ++ci)		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())
{		{
FindEdges(ci, cj, passClass, edges);		std::set<RegionID> reachable;
		FindReachableRegions(reg, reachable, passClass);
		for (const RegionID& region : reachable)
		globalRegion.insert({ region, ID });
		}
}		}
}		}
}		}

if (m_DebugOverlay)		if (m_DebugOverlay)
{		{
PROFILE("debug overlay");		PROFILE("debug overlay");
m_DebugOverlayLines.clear();		m_DebugOverlayLines.clear();
AddDebugEdges(GetPassabilityClass("default"));		AddDebugEdges(GetPassabilityClass("default"));
}		}
}		}

void HierarchicalPathfinder::Update(Grid<NavcellData>* grid, const Grid<u8>& dirtinessGrid)		void HierarchicalPathfinder::Update(Grid<NavcellData>* grid, const Grid<u8>& dirtinessGrid)
{		{
PROFILE3("Hierarchical Update");		PROFILE3("Hierarchical Update");

for (int cj = 0; cj < m_ChunksH; ++cj)		ASSERT(m_NextGlobalRegionID < std::numeric_limits<GlobalRegionID>::max());

		std::map<pass_class_t, std::vector<RegionID>> 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).
		templeUnsubmitted Done Inline Actions I profiled this with a few hundred wood choppers on Kerala, and `UpdateGlobalRegions` was about a tenth the size of `Update`, so I don't think it's a concern at the moment. Of course this depends on how big the chunks are. At 96x96 there's only 22x22 chunks on giant-size maps. temple: I profiled this with a few hundred wood choppers on Kerala, and `UpdateGlobalRegions` was about…
		wraitiiAuthorUnsubmitted Done Inline Actions Yes, it's just annoying from an intellectual perspective. wraitii: Yes, it's just annoying from an intellectual perspective.
		for (u8 cj = 0; cj < m_ChunksH; ++cj)
		templeUnsubmitted Done Inline Actions This is bad because you're mixing up regions in different pass classes. Instead first loop on pass classes like in `Recompute()`. temple: This is bad because you're mixing up regions in different pass classes. Instead first loop on…
		wraitiiAuthorUnsubmitted Done Inline Actions Correct. wraitii: Correct.
{		{
int j0 = cj * CHUNK_SIZE;		int j0 = cj * CHUNK_SIZE;
int j1 = std::min(j0 + CHUNK_SIZE, (int)dirtinessGrid.m_H);		int j1 = std::min(j0 + CHUNK_SIZE, (int)dirtinessGrid.m_H);
for (int ci = 0; ci < m_ChunksW; ++ci)		for (u8 ci = 0; ci < m_ChunksW; ++ci)
{		{
// Skip chunks where no navcells are dirty.		// Skip chunks where no navcells are dirty.
int i0 = ci * CHUNK_SIZE;		int i0 = ci * CHUNK_SIZE;
int i1 = std::min(i0 + CHUNK_SIZE, (int)dirtinessGrid.m_W);		int i1 = std::min(i0 + CHUNK_SIZE, (int)dirtinessGrid.m_W);
if (!dirtinessGrid.any_set_in_square(i0, j0, i1, j1))		if (!dirtinessGrid.any_set_in_square(i0, j0, i1, j1))
continue;		continue;

for (const std::pair<std::string, pass_class_t>& passClassMask : m_PassClassMasks)		for (const std::pair<std::string, pass_class_t>& passClassMask : m_PassClassMasks)
{		{
pass_class_t passClass = passClassMask.second;		pass_class_t passClass = passClassMask.second;
Chunk& a = m_Chunks[passClass].at(ci + cj*m_ChunksW);		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 local regions (not global regions)
a.InitRegions(ci, cj, grid, passClass);		a.InitRegions(ci, cj, grid, passClass);

		for (u16 i : a.m_RegionsID)
		needNewGlobalRegionMap[passClass].push_back(RegionID{ci, cj, i});
		templeUnsubmitted Done Inline Actions passClass temple: passClass

		UpdateEdges(ci, cj, passClass, edgeMap);
}		}
}		}
}		}
		templeUnsubmitted Done Inline Actions indent temple: indent

// TODO: Also be clever with edges		UpdateGlobalRegions(needNewGlobalRegionMap);
m_Edges.clear();
for (const std::pair<std::string, pass_class_t>& passClassMask : m_PassClassMasks)		if (m_DebugOverlay)
{		{
pass_class_t passClass = passClassMask.second;		PROFILE("debug overlay");
		templeUnsubmitted Done Inline Actions What if the update connects two previous disconnected global regions, is that handled? Instead we might need to keep a list of ID's that we encounter, and if more than one then do `FindReachableRegions()` to merge them. What if the update disconnects two large global regions? Unless all of the chunks were marked dirty, it will find one of them and use that ID, which is the same as the disconnected part. Instead we might have to do `FindReachableRegions()` with a new ID. temple: What if the update connects two previous disconnected global regions, is that handled? Instead…
		wraitiiAuthorUnsubmitted Done Inline Actions Yeah, you're correct. This was actually more buggy in other ways. wraitii: Yeah, you're correct. This was actually more buggy in other ways.
EdgesMap& edges = m_Edges[passClass];		m_DebugOverlayLines.clear();
		AddDebugEdges(GetPassabilityClass("default"));
		}
		}

for (int cj = 0; cj < m_ChunksH; ++cj)		/**
		* Connect a chunk's regions to their neighbors. Not optimised for global recomputing.
		* TODO: reduce code duplication with below
		*/
		void HierarchicalPathfinder::UpdateEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges)
{		{
for (int ci = 0; ci < m_ChunksW; ++ci)		std::vector<Chunk>& chunks = m_Chunks[passClass];

		Chunk& a = chunks.at(cj*m_ChunksW + ci);

		if (ci > 0)
		{
		Chunk& b = chunks.at(cj*m_ChunksW + (ci-1));
		RegionID raPrev(0,0,0);
		RegionID rbPrev(0,0,0);
		for (int j = 0; j < CHUNK_SIZE; ++j)
{		{
FindEdges(ci, cj, passClass, edges);		RegionID ra = a.Get(0, j);
		RegionID rb = b.Get(CHUNK_SIZE-1, j);
		if (ra.r && rb.r)
		{
		if (ra == raPrev && rb == rbPrev)
		continue;
		edges[ra].insert(rb);
		edges[rb].insert(ra);
		raPrev = ra;
		rbPrev = rb;
}		}
}		}
}		}

if (m_DebugOverlay)		if (ci < m_ChunksW-1)
{		{
PROFILE("debug overlay");		Chunk& b = chunks.at(cj*m_ChunksW + (ci+1));
m_DebugOverlayLines.clear();		RegionID raPrev(0,0,0);
AddDebugEdges(GetPassabilityClass("default"));		RegionID rbPrev(0,0,0);
		for (int j = 0; j < CHUNK_SIZE; ++j)
		{
		RegionID ra = a.Get(CHUNK_SIZE-1, j);
		RegionID rb = b.Get(0, j);
		if (ra.r && rb.r)
		{
		if (ra == raPrev && rb == rbPrev)
		continue;
		edges[ra].insert(rb);
		edges[rb].insert(ra);
		raPrev = ra;
		rbPrev = rb;
		}
		}
		}

		if (cj > 0)
		{
		Chunk& b = chunks.at((cj-1)*m_ChunksW + ci);
		RegionID raPrev(0,0,0);
		RegionID rbPrev(0,0,0);
		for (int i = 0; i < CHUNK_SIZE; ++i)
		{
		RegionID ra = a.Get(i, 0);
		RegionID rb = b.Get(i, CHUNK_SIZE-1);
		if (ra.r && rb.r)
		{
		if (ra == raPrev && rb == rbPrev)
		continue;
		edges[ra].insert(rb);
		edges[rb].insert(ra);
		raPrev = ra;
		rbPrev = rb;
		}
		}
		}

		if (cj < m_ChunksH - 1)
		{
		Chunk& b = chunks.at((cj+1)*m_ChunksW + ci);
		RegionID raPrev(0,0,0);
		RegionID rbPrev(0,0,0);
		for (int i = 0; i < CHUNK_SIZE; ++i)
		{
		RegionID ra = a.Get(i, CHUNK_SIZE-1);
		RegionID rb = b.Get(i, 0);
		if (ra.r && rb.r)
		{
		if (ra == raPrev && rb == rbPrev)
		continue;
		edges[ra].insert(rb);
		edges[rb].insert(ra);
		raPrev = ra;
		rbPrev = rb;
		}
		}
}		}
}		}

/**		/**
* Find edges between regions in this chunk and the adjacent below/left chunks.		* Find edges between regions in all chunks, in an optimised manner (only look at top/left)
*/		*/
void HierarchicalPathfinder::FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges)		void HierarchicalPathfinder::RecomputeAllEdges(pass_class_t passClass, EdgesMap& edges)
{		{
std::vector<Chunk>& chunks = m_Chunks[passClass];		std::vector<Chunk>& 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);		Chunk& a = chunks.at(cj*m_ChunksW + ci);

// For each edge between chunks, we loop over every adjacent pair of		// For each edge between chunks, we loop over every adjacent pair of
// navcells in the two chunks. If they are both in valid regions		// 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.		// (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		// (We don't need to test for duplicates since EdgesMap already uses a
// std::set which will drop duplicate entries.)		// std::set which will drop duplicate entries.)
// But as set.insert can be quite slow on large collection, and that we usually		// 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.		// try to insert the same values, we cache the previous one for a fast test.

if (ci > 0)		if (ci > 0)
{		{
Chunk& b = chunks.at(cj*m_ChunksW + (ci-1));		Chunk& b = chunks.at(cj*m_ChunksW + (ci-1));
RegionID raPrev(0,0,0);		RegionID raPrev(0,0,0);
RegionID rbPrev(0,0,0);		RegionID rbPrev(0,0,0);
for (int j = 0; j < CHUNK_SIZE; ++j)		for (int j = 0; j < CHUNK_SIZE; ++j)
{		{
RegionID ra = a.Get(0, j);		RegionID ra = a.Get(0, j);
RegionID rb = b.Get(CHUNK_SIZE-1, j);		RegionID rb = b.Get(CHUNK_SIZE-1, j);
if (ra.r && rb.r)		if (ra.r && rb.r)
{		{
if (ra == raPrev && rb == rbPrev)		if (ra == raPrev && rb == rbPrev)
continue;		continue;
edges[ra].insert(rb);		edges[ra].insert(rb);
edges[rb].insert(ra);		edges[rb].insert(ra);
raPrev = ra;		raPrev = ra;
rbPrev = rb;		rbPrev = rb;
}		}
}		}
}		}

if (cj > 0)		if (cj > 0)
{		{
Chunk& b = chunks.at((cj-1)*m_ChunksW + ci);		Chunk& b = chunks.at((cj-1)*m_ChunksW + ci);
RegionID raPrev(0,0,0);		RegionID raPrev(0,0,0);
RegionID rbPrev(0,0,0);		RegionID rbPrev(0,0,0);
for (int i = 0; i < CHUNK_SIZE; ++i)		for (int i = 0; i < CHUNK_SIZE; ++i)
{		{
RegionID ra = a.Get(i, 0);		RegionID ra = a.Get(i, 0);
RegionID rb = b.Get(i, CHUNK_SIZE-1);		RegionID rb = b.Get(i, CHUNK_SIZE-1);
if (ra.r && rb.r)		if (ra.r && rb.r)
{		{
if (ra == raPrev && rb == rbPrev)		if (ra == raPrev && rb == rbPrev)
continue;		continue;
edges[ra].insert(rb);		edges[ra].insert(rb);
edges[rb].insert(ra);		edges[rb].insert(ra);
raPrev = ra;		raPrev = ra;
rbPrev = rb;		rbPrev = rb;
}		}
}		}
}		}
		}
		}
}		}

/**		/**
* Debug visualisation of graph edges between regions.		* Debug visualisation of graph edges between regions.
*/		*/
void HierarchicalPathfinder::AddDebugEdges(pass_class_t passClass)		void HierarchicalPathfinder::AddDebugEdges(pass_class_t passClass)
{		{
const EdgesMap& edges = m_Edges[passClass];		const EdgesMap& edges = m_Edges[passClass];
Show All 21 Lines	for (const RegionID& region: edge.second)

std::vector<float> xz;		std::vector<float> xz;
xz.push_back(a.X.ToFloat());		xz.push_back(a.X.ToFloat());
xz.push_back(a.Y.ToFloat());		xz.push_back(a.Y.ToFloat());
xz.push_back(b.X.ToFloat());		xz.push_back(b.X.ToFloat());
xz.push_back(b.Y.ToFloat());		xz.push_back(b.Y.ToFloat());

m_DebugOverlayLines.emplace_back();		m_DebugOverlayLines.emplace_back();
m_DebugOverlayLines.back().m_Color = CColor(1.0, 1.0, 1.0, 1.0);		m_DebugOverlayLines.back().m_Color = CColor(1.0, 0.0, 0.0, 1.0);
SimRender::ConstructLineOnGround(*m_SimContext, xz, m_DebugOverlayLines.back(), true);		SimRender::ConstructLineOnGround(*m_SimContext, xz, m_DebugOverlayLines.back(), true);
}		}
}		}
}		}

		void HierarchicalPathfinder::UpdateGlobalRegions(const std::map<pass_class_t, std::vector<RegionID>>& 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<pass_class_t, std::vector<RegionID>>& regionsInNeed : needNewGlobalRegionMap)
		for (const RegionID& reg : regionsInNeed.second)
		{
		std::map<RegionID, GlobalRegionID>& globalRegions = m_GlobalRegions[regionsInNeed.first];
		// if we have already been given a region, skip us.
		if (globalRegions.find(reg) != globalRegions.end())
		continue;

		std::set<RegionID> reachable;
		FindReachableRegions(reg, reachable, regionsInNeed.first);

		GlobalRegionID ID = m_NextGlobalRegionID++;

		for (const RegionID& reg : reachable)
		globalRegions[reg] = ID;
		}
		templeUnsubmitted Done Inline Actions indent temple: indent
		}

HierarchicalPathfinder::RegionID HierarchicalPathfinder::Get(u16 i, u16 j, pass_class_t passClass)		HierarchicalPathfinder::RegionID HierarchicalPathfinder::Get(u16 i, u16 j, pass_class_t passClass)
{		{
int ci = i / CHUNK_SIZE;		int ci = i / CHUNK_SIZE;
int cj = j / CHUNK_SIZE;		int cj = j / CHUNK_SIZE;
ENSURE(ci < m_ChunksW && cj < m_ChunksH);		ENSURE(ci < m_ChunksW && cj < m_ChunksH);
return m_Chunks[passClass][cj*m_ChunksW + ci].Get(i % CHUNK_SIZE, j % CHUNK_SIZE);		return m_Chunks[passClass][cj*m_ChunksW + ci].Get(i % CHUNK_SIZE, j % CHUNK_SIZE);
}		}

void HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass)		HierarchicalPathfinder::GlobalRegionID HierarchicalPathfinder::GetGlobalRegion(u16 i, u16 j, pass_class_t passClass)
		{
		RegionID region = Get(i, j, passClass);
		if (region.r == 0)
		return (GlobalRegionID)0;
		return m_GlobalRegions[passClass][region];
		}

		void CreatePointGoalAt(u16 i, u16 j, PathGoal& goal)
		{
		// recreate the goal as a point-goal.
		PathGoal newGoal;
		newGoal.type = PathGoal::POINT;
		Pathfinding::NavcellCenter(i, j, newGoal.x, newGoal.z);
		// sanity check
		if(!goal.NavcellContainsGoal(i, j))
		{
		// try to get as close as possible to the goal inside this navcell.
		CFixedVector2D relativePos(goal.x - newGoal.x, goal.z - newGoal.z);
		CFixedVector2D u(fixed::FromInt(1), fixed::FromInt(0));
		CFixedVector2D v(fixed::FromInt(0), fixed::FromInt(1));
		// use a halfsize slightly below 1, otherwise we can end up on the edge and that confuses the pathfinder.
		CFixedVector2D halfsize(fixed::FromInt(2)/5, fixed::FromInt(2)/5);

		relativePos = Geometry::NearestPointOnSquare(relativePos, u, v, halfsize);
		newGoal.x += relativePos.X;
		newGoal.z += relativePos.Y;
		}
		goal = newGoal;
		}

		bool HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass)
{		{
PROFILE2("MakeGoalReachable");		PROFILE2("MakeGoalReachable");
RegionID source = Get(i0, j0, passClass);

// Find everywhere that's reachable		// Make the goal reachable.
std::set<RegionID> reachableRegions;		// In case the goal is a passable navcell, we'll just compare global regions and return immediately.
FindReachableRegions(source, reachableRegions, passClass);		// If the goal isn't a passable navcell, we'll try to make it one using the limited FindNearestPassableNavcell
		// This optimises for a common case where we want to go to a tree and similar things.
		// If the goal is decidedly not passable, we'll revert to a flood-fill.

// Check whether any reachable region contains the goal		////////////////////////////////
// And get the navcell that's the closest to the point		// determine if we will be able to reach the goal.
		u16 gi, gj;
		Pathfinding::NearestNavcell(goal.x, goal.z, gi, gj, m_W, m_H);

u16 bestI = 0;		std::set<RegionID> goalRegions;
u16 bestJ = 0;		FindGoalRegions(gi, gj, goal, goalRegions, passClass);
u32 dist2Best = std::numeric_limits<u32>::max();

for (const RegionID& region : reachableRegions)		std::vector<RegionID> targetRegions;

		GlobalRegionID startID = GetGlobalRegion(i0, j0, passClass);
		bool reachable = false;
		for (const RegionID& r : goalRegions)
		if (m_GlobalRegions[passClass][r] == startID)
{		{
// Skip region if its chunk doesn't contain the goal area		reachable = true;
entity_pos_t x0 = Pathfinding::NAVCELL_SIZE * (region.ci * CHUNK_SIZE);		targetRegions.push_back(r);
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 goal is a point-goal and reachable, just return early.
		if (reachable && goal.type == PathGoal::POINT)
		{
		CreatePointGoalAt(gi, gj, goal);
		return reachable;
		}

		templeUnsubmitted Done Inline Actions This seems to be a mistake. temple: This seems to be a mistake.
		wraitiiAuthorUnsubmitted Done Inline Actions (irrelevant since A* was removed) wraitii: (irrelevant since A* was removed)
// If the region contains the goal area, the goal is reachable		////////////////////////////////
// Remember the best point for optimization.		// If the goal is unreachable, we'd still like to get at least somewhere on the closest reachable region.
if (GetChunk(region.ci, region.cj, passClass).RegionNearestNavcellInGoal(region.r, i0, j0, goal, i, j, dist2))		// Use a limited FindNearestPassableNavcell search, for the 5 neighboring navcells (common case: a tree…)
		// if that returns a passable, reachable navcell, go there instead.
		if (!reachable)
		wraitiiAuthorUnsubmitted Not Done Inline Actions This bit might be removed with D1074. It's doing a little useless work, making the unreachable case perhaps slightly slower than svn (see profiling above) wraitii: This bit might be removed with D1074. It's doing a little useless work, making the unreachable…
{		{
// If it's a point, no need to move it, we're done		u16 ngi = gi, ngj = gj;
if (goal.type == PathGoal::POINT)		FindNearestPassableNavcell(ngi, ngj, passClass, true);
return;
if (dist2 < dist2Best)		if (GetGlobalRegion(ngi, ngj, passClass) == startID)
{		{
bestI = i;		gi = ngi;
bestJ = j;		gj = ngj;
dist2Best = dist2;		CreatePointGoalAt(gi, gj, goal);
		return reachable;
}		}
}		}
		templeUnsubmitted Done Inline Actions This is always 66, because `currScore` is zero since you erase at L918 and `DistBetween` is always 196 - 30 = 66. temple:* This is always 66, because `currScore` is zero since you erase at L918 and `DistBetween` is…
		wraitiiAuthorUnsubmitted Done Inline Actions (irrelevant since A* was removed) wraitii: (irrelevant since A* was removed)

		////////////////////////////////
		// Loop through regions and get the best navcell.
		u16 bestI = 0, bestJ = 0;
		u32 bestDist = std::numeric_limits<u32>::max();
		if (reachable)
		{
		// If we have target regions, we can just check them individually, which is reasonably fast.
		for (const RegionID& region : targetRegions)
		{
		u16 ri, rj;
		u32 dist;
		GetChunk(region.ci, region.cj, passClass).RegionNearestNavcellInGoal(region.r, i0, j0, goal, ri, rj, dist);
		if (dist < bestDist)
		{
		bestI = ri;
		bestJ = rj;
		bestDist = dist;
}		}
		}
		}
		else
		{
		// Otherwise we should try all reachable regions.
		templeUnsubmitted Done Inline Actions Why not use a priority queue, rather than looping through all open nodes? temple: Why not use a priority queue, rather than looping through all open nodes?
		wraitiiAuthorUnsubmitted Done Inline Actions Complicated the implementation at the time. It might or might not be a speedup tbh, A* is quite fast and cache-dependent from what I've found. wraitii: Complicated the implementation at the time. It might or might not be a speedup tbh, A* is quite…
		wraitiiAuthorUnsubmitted Done Inline Actions Irrelevant now. wraitii: Irrelevant now.
		// To avoid doing too much work, we'll sort them by distance from chunk center
		// If we've already found a point from a region and our current chunk center is at least √2*region_width farther
		// we can be sure any navcell in this region will not be better, so we can exit.

// If the goal area wasn't reachable,		struct RegionComparator
// find the navcell that's nearest to the goal's center
if (dist2Best == std::numeric_limits<u32>::max())
{		{
u16 iGoal, jGoal;		RegionComparator() = delete;
Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H);		RegionComparator(u16 i, u16 j): gi (i), gj(j) {};

FindNearestNavcellInRegions(reachableRegions, iGoal, jGoal, passClass);		u32 dist(const HierarchicalPathfinder::RegionID& region) const
		{
		return (region.ci * CHUNK_SIZE + CHUNK_SIZE/2 - gi) * (region.ci * CHUNK_SIZE + CHUNK_SIZE/2 - gi) +
		(region.cj * CHUNK_SIZE + CHUNK_SIZE/2 - gj) * (region.cj * CHUNK_SIZE + CHUNK_SIZE/2 - gj);
		}
		bool operator()(const HierarchicalPathfinder::RegionID& a, const HierarchicalPathfinder::RegionID& b) const
		{
		if (dist(a) < dist(b))
		return true;
		if (dist(a) > dist(b))
		return false;
		return a.r < b.r;
		}
		u16 gi, gj;
		};

// Construct a new point goal at the nearest reachable navcell		// TODO: if this becomes too slow, perhaps roll-out an optimised insert-sort here.
PathGoal newGoal;		RegionComparator comparator(gi, gj);
newGoal.type = PathGoal::POINT;		std::set<RegionID, RegionComparator> reachableRegions(comparator);
Pathfinding::NavcellCenter(iGoal, jGoal, newGoal.x, newGoal.z);		FindReachableRegions(Get(i0, j0, passClass), reachableRegions, passClass);
goal = newGoal;
		u32 bestChunkCenterDist = std::numeric_limits<u32>::max();
		u32 maxDistFromChunkCenter = (fixed::FromInt(3) / 2 * HierarchicalPathfinder::CHUNK_SIZE).ToInt_RoundToInfinity();
		ENSURE(maxDistFromChunkCenter < std::numeric_limits<u16>::max());
		maxDistFromChunkCenter *= maxDistFromChunkCenter;
		for (const RegionID& region : reachableRegions)
		{
		u32 chunkDist = comparator.dist(region);
		if (bestDist < std::numeric_limits<u32>::max() && (ssize_t)chunkDist > (ssize_t)bestChunkCenterDist + maxDistFromChunkCenter)
		templeUnsubmitted Done Inline Actions should rewrite as addition temple: should rewrite as addition
		wraitiiAuthorUnsubmitted Done Inline Actions Oh, yeah, that's way better. wraitii: Oh, yeah, that's way better.
		break; // break instead of continue as the set is ordered in increased distance.

		int ri, rj;
		u32 dist;
		GetChunk(region.ci, region.cj, passClass).RegionNavcellNearest(region.r, gi, gj, ri, rj, dist);
		if (dist < bestDist)
		{
		bestI = ri;
		bestJ = rj;
		bestDist = dist;
		bestChunkCenterDist = chunkDist;
		}
		}
		}
		CreatePointGoalAt(bestI, bestJ, goal);

		return reachable;
		}

		template <typename Ordering>
		void HierarchicalPathfinder::FindReachableRegions(RegionID from, std::set<RegionID, Ordering>& reachable, pass_class_t passClass)
		{
		// Flood-fill the region graph, starting at 'from',
		// collecting all the regions that are reachable via edges
		reachable.insert(from);

		EdgesMap& edgeMap = m_Edges[passClass];
		if (edgeMap.find(from) == edgeMap.end())
return;		return;
		templeUnsubmitted Done Inline Actions space temple: space
		wraitiiAuthorUnsubmitted Done Inline Actions This is why it works in D53 when const-correcting (cf D1830) wraitii: This is why it works in D53 when const-correcting (cf D1830)

		std::vector<RegionID> open;
		open.reserve(64);
		open.push_back(from);

		while (!open.empty())
		templeUnsubmitted Done Inline Actions I guess not important but this isn't a dist2 = distance squared. temple: I guess not important but this isn't a dist2 = distance squared.
		wraitiiAuthorUnsubmitted Done Inline Actions Can't recall why I went for manhattan tbh. wraitii: Can't recall why I went for manhattan tbh.
		wraitiiAuthorUnsubmitted Done Inline Actions Nothing useful in my non-squashed commit messages either, I'll just switch to square I guess. wraitii: Nothing useful in my non-squashed commit messages either, I'll just switch to square I guess.
		{
		RegionID curr = open.back();
		open.pop_back();
		for (const RegionID& region : edgeMap[curr])
		// Add to the reachable set; if this is the first time we added
		// it then also add it to the open list
		if (reachable.insert(region).second)
		open.push_back(region);
		}
}		}

ENSURE(dist2Best != std::numeric_limits<u32>::max());		void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass, bool limited)
PathGoal newGoal;		{
newGoal.type = PathGoal::POINT;		// If our current region is not 0, then this is passable, return immediately.
Pathfinding::NavcellCenter(bestI, bestJ, newGoal.x, newGoal.z);		if (Get(i, j, passClass).r != 0)
goal = newGoal;		return;
		templeUnsubmitted Done Inline Actions I suppose we could be next to a corner and then want to expand twice? temple: I suppose we could be next to a corner and then want to expand twice?
		wraitiiAuthorUnsubmitted Done Inline Actions I don't believe it makes a difference, we expand until we've found something or exhausted the search area. wraitii: I don't believe it makes a difference, we expand until we've found something or exhausted the…

		u32 dist2Best = std::numeric_limits<u32>::max();

		u16 oi = i, oj = j;
		// in most cases we're < 5 cells away so go for this first
		for (u16 tj = std::max(0, oj-5); tj <= std::min(m_H-1, oj+5); ++tj)
		for (u16 ti = std::max(0, oi-5); ti <= std::min(m_W-1, oi+5); ++ti)
		if (Get(ti, tj, passClass).r != 0)
		{
		u32 dist = (ti-oi)(ti-oi) + (tj-oj)(tj-oj);
		if (dist < dist2Best)
		{
		i = ti;
		j = tj;
		dist2Best = dist;
		}
}		}
		if (dist2Best < std::numeric_limits<u32>::max() \|\| limited)
		return;

		// We are on an impassable area, so we cannot rely on our accessibility.
		// This function does not necessarily need to return the absolute closest navcell,
		// since standing on impassable terrain is already a little wonky
		// We'll expand in a cross-wise way, returning the best passable cell after each expansion (if any)
		// this will ensure that we return an acceptable close navcell in general.
		// since the first time we could be close to an edge, we'll expand at least once.

void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass)		u16 iBest = i;
		u16 jBest = j;
		dist2Best = std::numeric_limits<u32>::max();

		u16 si = i/CHUNK_SIZE, sj = j/CHUNK_SIZE;
		std::set<std::pair<u8, u8>> visited = { { si, sj } };
		std::vector<std::pair<u8, u8>> openList = { { si, sj } }, openListTemp;
		templeUnsubmitted Done Inline Actions Could try inserting, see lines 1228-9. temple: Could try inserting, see lines 1228-9.
		wraitiiAuthorUnsubmitted Done Inline Actions Right. That container was probably a vector at some point which explains the code, but I'll switch it. wraitii: Right. That container was probably a vector at some point which explains the code, but I'll…
		static const int expander[4][2] = { { 1, 0 },{ -1, 0 },{ 0, 1 },{ 0, -1 } };
		for (size_t step = 0;;++step)
		{
		openListTemp.clear();
		for (std::pair<u8, u8> chunk : openList)
		{
		// go through all regions of a chunk
		int tempi, tempj;
		u32 dist2;
		GetChunk(chunk.first, chunk.second, passClass).NearestNavcell(i, j, tempi, tempj, dist2);
		if (dist2 < dist2Best)
		{
		iBest = tempi;
		jBest = tempj;
		dist2Best = dist2;
		}
		// expand cross
		for (const int* dir : expander)
{		{
std::set<RegionID> regions;		u8 x = (u8)std::min(std::max((int)si + dir[0], 0), (int)(m_ChunksW-1));
FindPassableRegions(regions, passClass);		u8 z = (u8)std::min(std::max((int)sj + dir[1], 0), (int)(m_ChunksH-1));
FindNearestNavcellInRegions(regions, i, j, passClass);		if (visited.insert({ x, z }).second)
		openListTemp.push_back({ x, z });
		}
		}
		if (openListTemp.empty() \|\| (step > 0 && dist2Best < std::numeric_limits<u32>::max()))
		break;
		openList.swap(openListTemp);
		}
		i = iBest;
		j = jBest;
}		}

void HierarchicalPathfinder::FindNearestNavcellInRegions(const std::set<RegionID>& regions, u16& iGoal, u16& jGoal, pass_class_t passClass)		void HierarchicalPathfinder::FindNearestNavcellInRegions(const std::set<RegionID>& regions, u16& iGoal, u16& jGoal, pass_class_t passClass)
{		{
// Find the navcell in the given regions that's nearest to the goal navcell:		// 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		// * For each region, record the (squared) minimal distance to the goal point
// * Sort regions by that underestimated distance		// * Sort regions by that underestimated distance
// * For each region, find the actual nearest navcell		// * For each region, find the actual nearest navcell
▲ Show 20 Lines • Show All 43 Lines • ▼ Show 20 Lines	if (dist2 < dist2Best)
dist2Best = dist2;		dist2Best = dist2;
}		}
}		}

iGoal = iBest;		iGoal = iBest;
jGoal = jBest;		jGoal = jBest;
}		}

void HierarchicalPathfinder::FindReachableRegions(RegionID from, std::set<RegionID>& reachable, pass_class_t passClass)		void HierarchicalPathfinder::FindGoalRegions(u16 gi, u16 gj, const PathGoal& goal, std::set<HierarchicalPathfinder::RegionID>& regions, pass_class_t passClass)
{		{
// Flood-fill the region graph, starting at 'from',		if (goal.type == PathGoal::POINT)
// collecting all the regions that are reachable via edges

std::vector<RegionID> open;
open.push_back(from);
reachable.insert(from);

while (!open.empty())
{		{
RegionID curr = open.back();		RegionID region = Get(gi, gj, passClass);
open.pop_back();		if (region.r > 0)
		regions.insert(region);
for (const RegionID& region : m_Edges[passClass][curr])		return;
// Add to the reachable set; if this is the first time we added
// it then also add it to the open list
if (reachable.insert(region).second)
open.push_back(region);
}
}		}

void HierarchicalPathfinder::FindPassableRegions(std::set<RegionID>& regions, pass_class_t passClass)		// 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.
// Construct a set of all regions of all chunks for this pass class		// Note that this also works in the Inverse-circle / Inverse-square case
for (const Chunk& chunk : m_Chunks[passClass])		// 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.
// region 0 is impassable tiles		// 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.
for (int r = 1; r <= chunk.m_NumRegions; ++r)		// It's not worth it to skip the "inner" regions since we'd need ranges above CHUNK_SIZE for that to start mattering
regions.insert(RegionID(chunk.m_ChunkI, chunk.m_ChunkJ, r));		// (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

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

void HierarchicalPathfinder::FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid<u16>& grid)		void HierarchicalPathfinder::FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid<u16>& grid)
		templeUnsubmitted Done Inline Actions unneeded braces temple: unneeded braces
{		{
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;

const Chunk& c = m_Chunks[passClass][region.cj * m_ChunksW + region.ci];		const Chunk& c = m_Chunks[passClass][region.cj * m_ChunksW + region.ci];

Show All 36 Lines