Differential D14 Diff 7121 source/simulation2/helpers/HierarchicalPathfinder.cpp

Changeset View

Standalone View

source/simulation2/helpers/HierarchicalPathfinder.cpp

Show First 20 Lines • Show All 345 Lines • ▼ Show 20 Lines	else if (!enabled && m_DebugOverlay)
m_SimContext = NULL;		m_SimContext = NULL;
}		}
}		}

void HierarchicalPathfinder::Recompute(Grid<NavcellData>* grid,		void HierarchicalPathfinder::Recompute(Grid<NavcellData>* grid,
const std::map<std::string, pass_class_t>& nonPathfindingPassClassMasks,		const std::map<std::string, pass_class_t>& nonPathfindingPassClassMasks,
const std::map<std::string, pass_class_t>& pathfindingPassClassMasks)		const std::map<std::string, pass_class_t>& pathfindingPassClassMasks)
{		{
PROFILE3("Hierarchical Recompute");		//PROFILE3("Hierarchical Recompute");

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;

// 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		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();

for (auto& passClassMask : allPassClasses)		for (auto& passClassMask : allPassClasses)
{		{
pass_class_t passClass = passClassMask.second;		pass_class_t passClass = passClassMask.second;
		wraitiiAuthorUnsubmitted Done Inline Actions forgot to remove this before uploading it seems :p wraitii: forgot to remove this before uploading it seems :p
		ItmsUnsubmitted Done Inline Actions For some reason Jenkins wasn't able to parse this: ? LANCELOT 1 LANCELOT 2 LANCELOT 4 LANCELOT 1 LANCELOT 2 LANCELOT 4 LANCELOT 1 LANCELOT 2 LANCELOT 4 <?xml version="1.0" encoding="UTF-8" ?> <testsuite name="cxxtest" timestamp="Sun Apr 28 18:20:29 2019" tests="318" errors="0" failures="0" time="0" > ... Itms: For some reason Jenkins wasn't able to parse this: ? ``` LANCELOT 1 LANCELOT 2 LANCELOT 4…

// 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);
Show All 10 Lines	for (int cj = 0; cj < m_ChunksH; ++cj)
{		{
FindEdges(ci, cj, passClass, edges);		FindEdges(ci, cj, passClass, edges);
}		}
}		}
}		}

if (m_DebugOverlay)		if (m_DebugOverlay)
{		{
PROFILE("debug overlay");		//PROFILE("debug overlay");
StanUnsubmitted Done Inline Actions Shouldn't that have been added back ? Stan: Shouldn't that have been added back ?
wraitiiAuthorUnsubmitted Done Inline Actions nah, who cares about the time profiling debug overlay honestly. wraitii: nah, who cares about the time profiling debug overlay honestly.
		StanUnsubmitted Not Done Inline Actions Check for mainthread maybe there too. Stan: Check for mainthread maybe there too.
		Kuba386Unsubmitted Done Inline Actions All of these are fixed in next diff. I've commented some because I was getting Profiler assertions and after changing profiler to avoid these assertions I haven't uncommented all of the profiler calls. Kuba386: All of these are fixed in next diff. I've commented some because I was getting Profiler…
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)
{		{
		StanUnsubmitted Not Done Inline Actions Check for mainthread maybe there too. Stan: Check for mainthread maybe there too.
PROFILE3("Hierarchical Update");		//PROFILE3("Hierarchical Update");

for (int cj = 0; cj < m_ChunksH; ++cj)		for (int cj = 0; cj < m_ChunksH; ++cj)
{		{
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 (int ci = 0; ci < m_ChunksW; ++ci)
{		{
// Skip chunks where no navcells are dirty.		// Skip chunks where no navcells are dirty.
Show All 24 Lines	for (int cj = 0; cj < m_ChunksH; ++cj)
{		{
FindEdges(ci, cj, passClass, edges);		FindEdges(ci, cj, passClass, edges);
}		}
}		}
}		}

if (m_DebugOverlay)		if (m_DebugOverlay)
{		{
PROFILE("debug overlay");		//PROFILE("debug overlay");
StanUnsubmitted Done Inline Actions Shouldn't that have been added back ? Stan: Shouldn't that have been added back ?
		StanUnsubmitted Not Done Inline Actions Check for mainthread maybe there too. Stan: Check for mainthread maybe there too.
m_DebugOverlayLines.clear();		m_DebugOverlayLines.clear();
AddDebugEdges(GetPassabilityClass("default"));		AddDebugEdges(GetPassabilityClass("default"));
}		}
}		}

/**		/**
* Find edges between regions in this chunk and the adjacent below/left chunks.		* Find edges between regions in this chunk and the adjacent below/left chunks.
*/		*/
void HierarchicalPathfinder::FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges)		void HierarchicalPathfinder::FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges) const
{		{
std::vector<Chunk>& chunks = m_Chunks[passClass];		const std::vector<Chunk>& chunks = m_Chunks.at(passClass);

Chunk& a = chunks.at(cj*m_ChunksW + ci);		const 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));		const 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);		const 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)
{		{
▲ Show 20 Lines • Show All 45 Lines • ▼ Show 20 Lines	for (const RegionID& region: edge.second)

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, 1.0, 1.0, 1.0);
SimRender::ConstructLineOnGround(*m_SimContext, xz, m_DebugOverlayLines.back(), true);		SimRender::ConstructLineOnGround(*m_SimContext, xz, m_DebugOverlayLines.back(), true);
}		}
}		}
}		}

HierarchicalPathfinder::RegionID HierarchicalPathfinder::Get(u16 i, u16 j, pass_class_t passClass)		HierarchicalPathfinder::RegionID HierarchicalPathfinder::Get(u16 i, u16 j, pass_class_t passClass) const
{		{
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.at(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)		void HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const
{		{
PROFILE2("MakeGoalReachable");		//PROFILE2("MakeGoalReachable");
		StanUnsubmitted Not Done Inline Actions Check for mainthread maybe there too. Stan: Check for mainthread maybe there too.
RegionID source = Get(i0, j0, passClass);		RegionID source = Get(i0, j0, passClass);

// Find everywhere that's reachable		// Find everywhere that's reachable
std::set<RegionID> reachableRegions;		std::set<RegionID> reachableRegions;
FindReachableRegions(source, reachableRegions, passClass);		FindReachableRegions(source, reachableRegions, passClass);

// Check whether any reachable region contains the goal		// Check whether any reachable region contains the goal
// And get the navcell that's the closest to the point		// And get the navcell that's the closest to the point
▲ Show 20 Lines • Show All 50 Lines • ▼ Show 20 Lines	void HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const

ENSURE(dist2Best != std::numeric_limits<u32>::max());		ENSURE(dist2Best != std::numeric_limits<u32>::max());
PathGoal newGoal;		PathGoal newGoal;
newGoal.type = PathGoal::POINT;		newGoal.type = PathGoal::POINT;
Pathfinding::NavcellCenter(bestI, bestJ, newGoal.x, newGoal.z);		Pathfinding::NavcellCenter(bestI, bestJ, newGoal.x, newGoal.z);
goal = newGoal;		goal = newGoal;
}		}

void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass)		void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) const
{		{
std::set<RegionID> regions;		std::set<RegionID> regions;
FindPassableRegions(regions, passClass);		FindPassableRegions(regions, passClass);
FindNearestNavcellInRegions(regions, i, j, passClass);		FindNearestNavcellInRegions(regions, i, j, passClass);
}		}

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) const
{		{
// 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
// * Stop when the underestimated distances are worse than the best real distance		// * Stop when the underestimated distances are worse than the best real distance

std::vector<std::pair<u32, RegionID> > regionDistEsts; // pair of (distance^2, region)		std::vector<std::pair<u32, RegionID> > regionDistEsts; // pair of (distance^2, region)
Show All 40 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::FindReachableRegions(RegionID from, std::set<RegionID>& 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

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

while (!open.empty())		while (!open.empty())
{		{
RegionID curr = open.back();		RegionID curr = open.back();
open.pop_back();		open.pop_back();

for (const RegionID& region : m_Edges[passClass][curr])		// apparently the region we're from has no edges, so we'll return the empty set
		// TODO: maybe here we should pretend we are on any other region?
		// indeed because of rasterization issues sometimes we can end up on "unconnected" regions
		if (m_Edges.at(passClass).find(curr) == m_Edges.at(passClass).end())
		continue;

		for (const RegionID& region : m_Edges.at(passClass).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)		void HierarchicalPathfinder::FindPassableRegions(std::set<RegionID>& 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[passClass])		for (const Chunk& chunk : m_Chunks.at(passClass))
{		{
// region 0 is impassable tiles		// region 0 is impassable tiles
for (int r = 1; r <= chunk.m_NumRegions; ++r)		for (int r = 1; r <= chunk.m_NumRegions; ++r)
regions.insert(RegionID(chunk.m_ChunkI, chunk.m_ChunkJ, r));		regions.insert(RegionID(chunk.m_ChunkI, chunk.m_ChunkJ, r));
}		}
}		}

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

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

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 (c.m_Regions[j][i] == region.r)		if (c.m_Regions[j][i] == region.r)
grid.set(i0 + i, j0 + j, value);		grid.set(i0 + i, j0 + j, value);
}		}

Grid<u16> HierarchicalPathfinder::GetConnectivityGrid(pass_class_t passClass)		Grid<u16> HierarchicalPathfinder::GetConnectivityGrid(pass_class_t passClass) const
{		{
Grid<u16> connectivityGrid(m_W, m_H);		Grid<u16> connectivityGrid(m_W, m_H);
connectivityGrid.reset();		connectivityGrid.reset();

u16 idx = 1;		u16 idx = 1;

for (size_t i = 0; i < m_W; ++i)		for (size_t i = 0; i < m_W; ++i)
{		{
Show All 21 Lines