Differential D14 Diff 17087 source/simulation2/helpers/LongPathfinder.cpp

Changeset View

Standalone View

source/simulation2/helpers/LongPathfinder.cpp

/* Copyright (C) 2019 Wildfire Games.		/* Copyright (C) 2021 Wildfire Games.
* This file is part of 0 A.D.		* This file is part of 0 A.D.
*		*
* 0 A.D. is free software: you can redistribute it and/or modify		* 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		* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 2 of the License, or		* the Free Software Foundation, either version 2 of the License, or
* (at your option) any later version.		* (at your option) any later version.
*		*
* 0 A.D. is distributed in the hope that it will be useful,		* 0 A.D. is distributed in the hope that it will be useful,
Show All 10 Lines
#include "LongPathfinder.h"		#include "LongPathfinder.h"

#include "lib/bits.h"		#include "lib/bits.h"
#include "ps/Profile.h"		#include "ps/Profile.h"

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

		#include <mutex>

		namespace
		{
		StanUnsubmitted Not Done Inline Actions Is it used anywhere ? Stan: Is it used anywhere ?
		wraitiiAuthorUnsubmitted Done Inline Actions Seems unused in the current implementation, but won't make unrelated changes here. wraitii: Seems unused in the current implementation, but won't make unrelated changes here.
		static std::mutex g_DebugMutex;
		}

/**		/**
* Jump point cache.		* Jump point cache.
*		*
* The JPS algorithm wants to efficiently either find the first jump point		* The JPS algorithm wants to efficiently either find the first jump point
* in some direction from some cell (not counting the cell itself),		* in some direction from some cell (not counting the cell itself),
* if it is reachable without crossing any impassable cells;		* if it is reachable without crossing any impassable cells;
* or know that there is no such reachable jump point.		* or know that there is no such reachable jump point.
* The jump point is always on a passable cell.		* The jump point is always on a passable cell.
▲ Show 20 Lines • Show All 46 Lines • ▼ Show 20 Lines	void SetRange(int x0, int x1, bool obstruction)
for (int x = x0; x < x1; ++x)		for (int x = x0; x < x1; ++x)
data[x] = (x1 << 1) \| (obstruction ? 1 : 0);		data[x] = (x1 << 1) \| (obstruction ? 1 : 0);
}		}

/**		/**
* Returns the coordinate of the next jump point xp (where x < xp),		* Returns the coordinate of the next jump point xp (where x < xp),
* and whether it's an obstruction point or jump point.		* and whether it's an obstruction point or jump point.
*/		*/
void Get(int x, int& xp, bool& obstruction)		void Get(int x, int& xp, bool& obstruction) const
{		{
ENSURE(0 <= x && x < (int)data.size());		ENSURE(0 <= x && x < (int)data.size());
xp = data[x] >> 1;		xp = data[x] >> 1;
obstruction = data[x] & 1;		obstruction = data[x] & 1;
}		}

void Finish() { }		void Finish() { }
};		};
▲ Show 20 Lines • Show All 74 Lines • ▼ Show 20 Lines	void Finish()
size_t depth = ceil_log2(data.size() + 1);		size_t depth = ceil_log2(data.size() + 1);
tree.resize((1 << depth) - 1);		tree.resize((1 << depth) - 1);
ConstructTree(tree, 0, data.size() / 2, data.size(), 0);		ConstructTree(tree, 0, data.size() / 2, data.size(), 0);
}		}

data.swap(tree);		data.swap(tree);
}		}

void Get(int x, int& xp, bool& obstruction)		void Get(int x, int& xp, bool& obstruction) const
{		{
// Search the binary tree for an interval which contains x		// Search the binary tree for an interval which contains x
int i = 0;		int i = 0;
while (true)		while (true)
{		{
ENSURE(i < (int)data.size());		ENSURE(i < (int)data.size());
Interval interval = data[i];		Interval interval = data[i];
if (x < interval.x0())		if (x < interval.x0())
▲ Show 20 Lines • Show All 128 Lines • ▼ Show 20 Lines	size_t GetMemoryUsage() const
return bytes;		return bytes;
}		}

/**		/**
* Returns the next jump point (or goal point) to explore,		* Returns the next jump point (or goal point) to explore,
* at (ip, j) where i < ip.		* at (ip, j) where i < ip.
* Returns i if there is no such point.		* Returns i if there is no such point.
*/		*/
int GetJumpPointRight(int i, int j, const PathGoal& goal)		int GetJumpPointRight(int i, int j, const PathGoal& goal) const
{		{
int ip;		int ip;
bool obstruction;		bool obstruction;
m_JumpPointsRight[j].Get(i, ip, obstruction);		m_JumpPointsRight[j].Get(i, ip, obstruction);
// Adjust ip to be a goal cell, if there is one closer than the jump point;		// Adjust ip to be a goal cell, if there is one closer than the jump point;
// and then return the new ip if there is a goal,		// and then return the new ip if there is a goal,
// or the old ip if there is a (non-obstruction) jump point		// or the old ip if there is a (non-obstruction) jump point
if (goal.NavcellRectContainsGoal(i + 1, j, ip - 1, j, &ip, NULL) \|\| !obstruction)		if (goal.NavcellRectContainsGoal(i + 1, j, ip - 1, j, &ip, NULL) \|\| !obstruction)
return ip;		return ip;
return i;		return i;
}		}

int GetJumpPointLeft(int i, int j, const PathGoal& goal)		int GetJumpPointLeft(int i, int j, const PathGoal& goal) const
{		{
int mip; // mirrored value, because m_JumpPointsLeft is generated from a mirrored map		int mip; // mirrored value, because m_JumpPointsLeft is generated from a mirrored map
bool obstruction;		bool obstruction;
m_JumpPointsLeft[j].Get(m_Width - 1 - i, mip, obstruction);		m_JumpPointsLeft[j].Get(m_Width - 1 - i, mip, obstruction);
int ip = m_Width - 1 - mip;		int ip = m_Width - 1 - mip;
if (goal.NavcellRectContainsGoal(i - 1, j, ip + 1, j, &ip, NULL) \|\| !obstruction)		if (goal.NavcellRectContainsGoal(i - 1, j, ip + 1, j, &ip, NULL) \|\| !obstruction)
return ip;		return ip;
return i;		return i;
}		}

int GetJumpPointUp(int i, int j, const PathGoal& goal)		int GetJumpPointUp(int i, int j, const PathGoal& goal) const
{		{
int jp;		int jp;
bool obstruction;		bool obstruction;
m_JumpPointsUp[i].Get(j, jp, obstruction);		m_JumpPointsUp[i].Get(j, jp, obstruction);
if (goal.NavcellRectContainsGoal(i, j + 1, i, jp - 1, NULL, &jp) \|\| !obstruction)		if (goal.NavcellRectContainsGoal(i, j + 1, i, jp - 1, NULL, &jp) \|\| !obstruction)
return jp;		return jp;
return j;		return j;
}		}

int GetJumpPointDown(int i, int j, const PathGoal& goal)		int GetJumpPointDown(int i, int j, const PathGoal& goal) const
{		{
int mjp; // mirrored value		int mjp; // mirrored value
bool obstruction;		bool obstruction;
m_JumpPointsDown[i].Get(m_Height - 1 - j, mjp, obstruction);		m_JumpPointsDown[i].Get(m_Height - 1 - j, mjp, obstruction);
int jp = m_Height - 1 - mjp;		int jp = m_Height - 1 - mjp;
if (goal.NavcellRectContainsGoal(i, j - 1, i, jp + 1, NULL, &jp) \|\| !obstruction)		if (goal.NavcellRectContainsGoal(i, j - 1, i, jp + 1, NULL, &jp) \|\| !obstruction)
return jp;		return jp;
return j;		return j;
}		}
};		};

//////////////////////////////////////////////////////////		//////////////////////////////////////////////////////////

LongPathfinder::LongPathfinder() :		LongPathfinder::LongPathfinder() :
m_UseJPSCache(false),		m_UseJPSCache(false),
m_Grid(NULL), m_GridSize(0),		m_Grid(NULL), m_GridSize(0)
m_DebugOverlay(NULL), m_DebugGrid(NULL), m_DebugPath(NULL)
{		{
		StanUnsubmitted Not Done Inline Actions Why the change ? Why not add m_Grid as well ? Stan: Why the change ? Why not add m_Grid as well ?
		wraitiiAuthorUnsubmitted Done Inline Actions The items are now thread_local global variables, instead of members of LongPathfinder, unlike M_Grid. wraitii: The items are now thread_local global variables, instead of members of LongPathfinder, unlike…
		StanUnsubmitted Not Done Inline Actions Ah I see, could nullptr the last while at it :) Stan: Ah I see, could nullptr the last while at it :)
}		}

LongPathfinder::~LongPathfinder()
{
SAFE_DELETE(m_DebugOverlay);
SAFE_DELETE(m_DebugGrid);
SAFE_DELETE(m_DebugPath);
}

#define PASSABLE(i, j) IS_PASSABLE(state.terrain->get(i, j), state.passClass)		#define PASSABLE(i, j) IS_PASSABLE(state.terrain->get(i, j), state.passClass)

// Calculate heuristic cost from tile i,j to goal		// Calculate heuristic cost from tile i,j to goal
// (This ought to be an underestimate for correctness)		// (This ought to be an underestimate for correctness)
PathCost LongPathfinder::CalculateHeuristic(int i, int j, int iGoal, int jGoal) const		PathCost LongPathfinder::CalculateHeuristic(int i, int j, int iGoal, int jGoal) const
{		{
int di = abs(i - iGoal);		int di = abs(i - iGoal);
int dj = abs(j - jGoal);		int dj = abs(j - jGoal);
▲ Show 20 Lines • Show All 314 Lines • ▼ Show 20 Lines	while (true)

ni += di;		ni += di;
nj += dj;		nj += dj;
}		}
}		}

void LongPathfinder::ComputeJPSPath(const HierarchicalPathfinder& hierPath, entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, WaypointPath& path) const		void LongPathfinder::ComputeJPSPath(const HierarchicalPathfinder& hierPath, entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, WaypointPath& path) const
{		{
PROFILE("ComputePathJPS");		PROFILE2("ComputePathJPS");
PROFILE2_IFSPIKE("ComputePathJPS", 0.0002);
StanUnsubmitted Not Done Inline Actions ?? Stan: ??
wraitiiAuthorUnsubmitted Done Inline Actions Tis' something I did in the past and was a horrible idea. wraitii: Tis' something I did in the past and was a horrible idea.
PathfinderState state = { 0 };		PathfinderState state = { 0 };

		if (m_UseJPSCache)
		{
		// Needs to lock for construction, or several threads might try doing that at the same time.
		static std::mutex JPCMutex;
		std::unique_lock<std::mutex> lock(JPCMutex);
std::map<pass_class_t, shared_ptr<JumpPointCache> >::const_iterator it = m_JumpPointCache.find(passClass);		std::map<pass_class_t, shared_ptr<JumpPointCache> >::const_iterator it = m_JumpPointCache.find(passClass);
if (it != m_JumpPointCache.end())		if (it != m_JumpPointCache.end())
state.jpc = it->second.get();		state.jpc = it->second.get();

if (m_UseJPSCache && !state.jpc)		if (!state.jpc)
{		{
state.jpc = new JumpPointCache;		m_JumpPointCache[passClass] = std::make_shared<JumpPointCache>();
state.jpc->reset(m_Grid, passClass);		m_JumpPointCache[passClass]->reset(m_Grid, passClass);
debug_printf("PATHFINDER: JPC memory: %d kB\n", (int)state.jpc->GetMemoryUsage() / 1024);		debug_printf("PATHFINDER: JPC memory: %d kB\n", (int)state.jpc->GetMemoryUsage() / 1024);
m_JumpPointCache[passClass] = shared_ptr<JumpPointCache>(state.jpc);		state.jpc = m_JumpPointCache[passClass].get();
		}
}		}

// Convert the start coordinates to tile indexes		// Convert the start coordinates to tile indexes
u16 i0, j0;		u16 i0, j0;
Pathfinding::NearestNavcell(x0, z0, i0, j0, m_GridSize, m_GridSize);		Pathfinding::NearestNavcell(x0, z0, i0, j0, m_GridSize, m_GridSize);

if (!IS_PASSABLE(m_Grid->get(i0, j0), passClass))		if (!IS_PASSABLE(m_Grid->get(i0, j0), passClass))
{		{
▲ Show 20 Lines • Show All 156 Lines • ▼ Show 20 Lines	void LongPathfinder::ComputeJPSPath(const HierarchicalPathfinder& hierPath, entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, WaypointPath& path) const
// The last waypoint is slightly incorrect (it's not the goal but the center		// The last waypoint is slightly incorrect (it's not the goal but the center
// of the navcell of the goal), so replace it		// of the navcell of the goal), so replace it
if (!path.m_Waypoints.empty())		if (!path.m_Waypoints.empty())
path.m_Waypoints.front() = { state.goal.x, state.goal.z };		path.m_Waypoints.front() = { state.goal.x, state.goal.z };

ImprovePathWaypoints(path, passClass, origGoal.maxdist, x0, z0);		ImprovePathWaypoints(path, passClass, origGoal.maxdist, x0, z0);

// Save this grid for debug display		// Save this grid for debug display
delete m_DebugGrid;		if (m_Debug.Overlay)
m_DebugGrid = state.tiles;		{
m_DebugSteps = state.steps;		std::lock_guard<std::mutex> lock(g_DebugMutex);
m_DebugGoal = state.goal;		delete m_Debug.Grid;
		m_Debug.Grid = state.tiles;
		m_Debug.Steps = state.steps;
		m_Debug.Goal = state.goal;
		}
		else
		SAFE_DELETE(state.tiles);
}		}

#undef PASSABLE		#undef PASSABLE

void LongPathfinder::ImprovePathWaypoints(WaypointPath& path, pass_class_t passClass, entity_pos_t maxDist, entity_pos_t x0, entity_pos_t z0) const		void LongPathfinder::ImprovePathWaypoints(WaypointPath& path, pass_class_t passClass, entity_pos_t maxDist, entity_pos_t x0, entity_pos_t z0) const
{		{
if (path.m_Waypoints.empty())		if (path.m_Waypoints.empty())
return;		return;
▲ Show 20 Lines • Show All 42 Lines • ▼ Show 20 Lines	for (size_t k = 1; k < waypoints.size() - 1; ++k)
}		}
}		}
newWaypoints.push_back(waypoints.back());		newWaypoints.push_back(waypoints.back());
path.m_Waypoints.swap(newWaypoints);		path.m_Waypoints.swap(newWaypoints);
}		}

void LongPathfinder::GetDebugDataJPS(u32& steps, double& time, Grid<u8>& grid) const		void LongPathfinder::GetDebugDataJPS(u32& steps, double& time, Grid<u8>& grid) const
{		{
steps = m_DebugSteps;		steps = m_Debug.Steps;
time = m_DebugTime;		time = m_Debug.Time;

if (!m_DebugGrid)		if (!m_Debug.Grid)
return;		return;

		std::lock_guard<std::mutex> lock(g_DebugMutex);

u16 iGoal, jGoal;		u16 iGoal, jGoal;
Pathfinding::NearestNavcell(m_DebugGoal.x, m_DebugGoal.z, iGoal, jGoal, m_GridSize, m_GridSize);		Pathfinding::NearestNavcell(m_Debug.Goal.x, m_Debug.Goal.z, iGoal, jGoal, m_GridSize, m_GridSize);

grid = Grid<u8>(m_DebugGrid->m_W, m_DebugGrid->m_H);		grid = Grid<u8>(m_Debug.Grid->m_W, m_Debug.Grid->m_H);
for (u16 j = 0; j < grid.m_H; ++j)		for (u16 j = 0; j < grid.m_H; ++j)
{		{
for (u16 i = 0; i < grid.m_W; ++i)		for (u16 i = 0; i < grid.m_W; ++i)
{		{
if (i == iGoal && j == jGoal)		if (i == iGoal && j == jGoal)
continue;		continue;
PathfindTile t = m_DebugGrid->get(i, j);		PathfindTile t = m_Debug.Grid->get(i, j);
grid.set(i, j, (t.IsOpen() ? 1 : 0) \| (t.IsClosed() ? 2 : 0));		grid.set(i, j, (t.IsOpen() ? 1 : 0) \| (t.IsClosed() ? 2 : 0));
}		}
}		}
}		}

void LongPathfinder::SetDebugOverlay(bool enabled)
{
if (enabled && !m_DebugOverlay)
m_DebugOverlay = new LongOverlay(*this);
else if (!enabled && m_DebugOverlay)
SAFE_DELETE(m_DebugOverlay);
}

void LongPathfinder::ComputePath(const HierarchicalPathfinder& hierPath, entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal,		void LongPathfinder::ComputePath(const HierarchicalPathfinder& hierPath, entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal,
pass_class_t passClass, WaypointPath& path) const		pass_class_t passClass, WaypointPath& path) const
{		{
if (!m_Grid)		if (!m_Grid)
{		{
LOGERROR("The pathfinder grid hasn't been setup yet, aborting ComputeJPSPath");		LOGERROR("The pathfinder grid hasn't been setup yet, aborting ComputeJPSPath");
return;		return;
}		}
Show All 36 Lines	for (u16 i = 0; i < m_Grid->m_W; ++i)

n \|= SPECIAL_PASS_CLASS;		n \|= SPECIAL_PASS_CLASS;
break;		break;
}		}
m_Grid->set(i, j, n);		m_Grid->set(i, j, n);
}		}
}		}
}		}


		/**
		* Terrain overlay for pathfinder debugging.
		* Renders a representation of the most recent pathfinding operation.
		*/
		class LongOverlay : public TerrainTextureOverlay
		{
		public:
		LongPathfinder& m_Pathfinder;

		LongOverlay(LongPathfinder& pathfinder) :
		TerrainTextureOverlay(Pathfinding::NAVCELLS_PER_TILE), m_Pathfinder(pathfinder)
		{
		}

		virtual void BuildTextureRGBA(u8* data, size_t w, size_t h)
		{
		// Grab the debug data for the most recently generated path
		u32 steps;
		double time;
		Grid<u8> debugGrid;
		m_Pathfinder.GetDebugData(steps, time, debugGrid);

		// Render navcell passability
		u8* p = data;
		for (size_t j = 0; j < h; ++j)
		{
		for (size_t i = 0; i < w; ++i)
		{
		SColor4ub color(0, 0, 0, 0);
		if (!IS_PASSABLE(m_Pathfinder.m_Grid->get((int)i, (int)j), m_Pathfinder.m_Debug.PassClass))
		color = SColor4ub(255, 0, 0, 127);

		if (debugGrid.m_W && debugGrid.m_H)
		{
		u8 n = debugGrid.get((int)i, (int)j);

		if (n == 1)
		color = SColor4ub(255, 255, 0, 127);
		else if (n == 2)
		color = SColor4ub(0, 255, 0, 127);

		if (m_Pathfinder.m_Debug.Goal.NavcellContainsGoal(i, j))
		color = SColor4ub(0, 0, 255, 127);
		}

		*p++ = color.R;
		*p++ = color.G;
		*p++ = color.B;
		*p++ = color.A;
		}
		}

		// Render the most recently generated path
		if (m_Pathfinder.m_Debug.Path && !m_Pathfinder.m_Debug.Path->m_Waypoints.empty())
		{
		std::vector<Waypoint>& waypoints = m_Pathfinder.m_Debug.Path->m_Waypoints;
		u16 ip = 0, jp = 0;
		for (size_t k = 0; k < waypoints.size(); ++k)
		{
		u16 i, j;
		Pathfinding::NearestNavcell(waypoints[k].x, waypoints[k].z, i, j, m_Pathfinder.m_GridSize, m_Pathfinder.m_GridSize);
		if (k == 0)
		{
		ip = i;
		jp = j;
		}
		else
		{
		bool firstCell = true;
		do
		{
		if (data[(jpw + ip)4+3] == 0)
		{
		data[(jpw + ip)4+0] = 0xFF;
		data[(jpw + ip)4+1] = 0xFF;
		data[(jpw + ip)4+2] = 0xFF;
		data[(jpw + ip)4+3] = firstCell ? 0xA0 : 0x60;
		}
		ip = ip < i ? ip+1 : ip > i ? ip-1 : ip;
		jp = jp < j ? jp+1 : jp > j ? jp-1 : jp;
		firstCell = false;
		}
		while (ip != i \|\| jp != j);
		}
		}
		}
		}
		};

		// These two functions must come below LongOverlay's definition.
		void LongPathfinder::SetDebugOverlay(bool enabled)
		{
		if (enabled && !m_Debug.Overlay)
		m_Debug.Overlay = new LongOverlay(*this);
		else if (!enabled && m_Debug.Overlay)
		SAFE_DELETE(m_Debug.Overlay);
		}

		LongPathfinder::~LongPathfinder()
		{
		SAFE_DELETE(m_Debug.Overlay);
		SAFE_DELETE(m_Debug.Grid);
		SAFE_DELETE(m_Debug.Path);
		}