0ad/source/simulation2/helpers/LongPathfinder.cpp

/* 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 <http://www.gnu.org/licenses/>.
 */

#include "precompiled.h"

#include "LongPathfinder.h"

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

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

#include <mutex>

namespace
{
static std::mutex g_DebugMutex;
}

/**
 * Jump point cache.
 *
 * The JPS algorithm wants to efficiently either find the first jump point
 * in some direction from some cell (not counting the cell itself),
 * if it is reachable without crossing any impassable cells;
 * or know that there is no such reachable jump point.
 * The jump point is always on a passable cell.
 * We cache that data to allow fast lookups, which helps performance
 * significantly (especially on sparse maps).
 * Recalculation might be expensive but the underlying passability data
 * changes relatively rarely.
 *
 * To allow the algorithm to detect goal cells, we want to treat them as
 * jump points too. (That means the algorithm will push those cells onto
 * its open queue, and will eventually pop a goal cell and realise it's done.)
 * (Goals might be circles/squares/etc, not just a single cell.)
 * But the goal generally changes for every path request, so we can't cache
 * it like the normal jump points.
 * Instead, if there's no jump point from some cell then we'll cache the
 * first impassable cell as an 'obstruction jump point'
 * (with a flag to distinguish from a real jump point), and then the caller
 * can test whether the goal includes a cell that's closer than the first
 * (obstruction or real) jump point,
 * and treat the goal cell as a jump point in that case.
 *
 * We only ever need to find the jump point relative to a passable cell;
 * the cache is allowed to return bogus values for impassable cells.
 */
class JumpPointCache
{
	/**
	 * Simple space-inefficient row storage.
	 */
	struct RowRaw
	{
		std::vector<u16> data;

		size_t GetMemoryUsage() const
		{
			return data.capacity() * sizeof(u16);
		}

		RowRaw(int length)
		{
			data.resize(length);
		}

		/**
		 * Set cells x0 <= x < x1 to have jump point x1.
		 */
		void SetRange(int x0, int x1, bool obstruction)
		{
			ENSURE(0 <= x0 && x0 <= x1 && x1 < (int)data.size());
			for (int x = x0; x < x1; ++x)
				data[x] = (x1 << 1) | (obstruction ? 1 : 0);
		}

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

		void Finish() { }
	};

	struct RowTree
	{
		/**
		 * Represents an interval [u15 x0, u16 x1)
		 * with a boolean obstruction flag,
		 * packed into a single u32.
		 */
		struct Interval
		{
			Interval() : data(0) { }

			Interval(int x0, int x1, bool obstruction)
			{
				ENSURE(0 <= x0 && x0 < 0x8000);
				ENSURE(0 <= x1 && x1 < 0x10000);
				data = ((u32)x0 << 17) | (u32)(obstruction ? 0x10000 : 0) | (u32)x1;
			}

			int x0() { return data >> 17; }
			int x1() { return data & 0xFFFF; }
			bool obstruction() { return (data & 0x10000) != 0; }

			u32 data;
		};

		std::vector<Interval> data;

		size_t GetMemoryUsage() const
		{
			return data.capacity() * sizeof(Interval);
		}

		RowTree(int UNUSED(length))
		{
		}

		void SetRange(int x0, int x1, bool obstruction)
		{
			ENSURE(0 <= x0 && x0 <= x1);
			data.emplace_back(x0, x1, obstruction);
		}

		/**
		 * Recursive helper function for Finish().
		 * Given two ranges [x0, pivot) and [pivot, x1) in the sorted array 'data',
		 * the pivot element is added onto the binary tree (stored flattened in an
		 * array), and then each range is split into two sub-ranges with a pivot in
		 * the middle (to ensure the tree remains balanced) and ConstructTree recurses.
		 */
		void ConstructTree(std::vector<Interval>& tree, size_t x0, size_t pivot, size_t x1, size_t idx_tree)
		{
			ENSURE(x0 < data.size());
			ENSURE(x1 <= data.size());
			ENSURE(x0 <= pivot);
			ENSURE(pivot < x1);
			ENSURE(idx_tree < tree.size());

			tree[idx_tree] = data[pivot];

			if (x0 < pivot)
				ConstructTree(tree, x0, (x0 + pivot) / 2, pivot, (idx_tree << 1) + 1);
			if (pivot + 1 < x1)
				ConstructTree(tree, pivot + 1, (pivot + x1) / 2, x1, (idx_tree << 1) + 2);
		}

		void Finish()
		{
			// Convert the sorted interval list into a balanced binary tree

			std::vector<Interval> tree;

			if (!data.empty())
			{
				size_t depth = ceil_log2(data.size() + 1);
				tree.resize((1 << depth) - 1);
				ConstructTree(tree, 0, data.size() / 2, data.size(), 0);
			}

			data.swap(tree);
		}

		void Get(int x, int& xp, bool& obstruction) const
		{
			// Search the binary tree for an interval which contains x
			int i = 0;
			while (true)
			{
				ENSURE(i < (int)data.size());
				Interval interval = data[i];
				if (x < interval.x0())
					i = (i << 1) + 1;
				else if (x >= interval.x1())
					i = (i << 1) + 2;
				else
				{
					ENSURE(interval.x0() <= x && x < interval.x1());
					xp = interval.x1();
					obstruction = interval.obstruction();
					return;
				}
			}
		}
	};

	// Pick one of the row implementations
	typedef RowRaw Row;

public:
	int m_Width;
	int m_Height;
	std::vector<Row> m_JumpPointsRight;
	std::vector<Row> m_JumpPointsLeft;
	std::vector<Row> m_JumpPointsUp;
	std::vector<Row> m_JumpPointsDown;

	/**
	 * Compute the cached obstruction/jump points for each cell,
	 * in a single direction. By default the code assumes the rightwards
	 * (+i) direction; set 'transpose' to switch to upwards (+j),
	 * and/or set 'mirror' to reverse the direction.
	 */
	void ComputeRows(std::vector<Row>& rows,
		const Grid<NavcellData>& terrain, pass_class_t passClass,
		bool transpose, bool mirror)
	{
		int w = terrain.m_W;
		int h = terrain.m_H;

		if (transpose)
			std::swap(w, h);

		// Check the terrain passability, adjusted for transpose/mirror
#define TERRAIN_IS_PASSABLE(i, j) \
	IS_PASSABLE( \
		mirror \
		? (transpose ? terrain.get((j), w-1-(i)) : terrain.get(w-1-(i), (j))) \
		: (transpose ? terrain.get((j), (i)) : terrain.get((i), (j))) \
	, passClass)

		rows.reserve(h);
		for (int j = 0; j < h; ++j)
			rows.emplace_back(w);

		for (int j = 1; j < h - 1; ++j)
		{
			// Find the first passable cell.
			// Then, find the next jump/obstruction point after that cell,
			// and store that point for the passable range up to that cell,
			// then repeat.

			int i = 0;
			while (i < w)
			{
				// Restart the 'while' loop until we reach a passable cell
				if (!TERRAIN_IS_PASSABLE(i, j))
				{
					++i;
					continue;
				}

				// i is now a passable cell; find the next jump/obstruction point.
				// (We assume the map is surrounded by impassable cells, so we don't
				// need to explicitly check for world bounds here.)

				int i0 = i;
				while (true)
				{
					++i;

					// Check if we hit an obstructed tile
					if (!TERRAIN_IS_PASSABLE(i, j))
					{
						rows[j].SetRange(i0, i, true);
						break;
					}

					// Check if we reached a jump point
					if ((!TERRAIN_IS_PASSABLE(i - 1, j - 1) && TERRAIN_IS_PASSABLE(i, j - 1)) ||
						(!TERRAIN_IS_PASSABLE(i - 1, j + 1) && TERRAIN_IS_PASSABLE(i, j + 1)))
					{
						rows[j].SetRange(i0, i, false);
						break;
					}
				}
			}

			rows[j].Finish();
		}
#undef TERRAIN_IS_PASSABLE
	}

	void reset(const Grid<NavcellData>* terrain, pass_class_t passClass)
	{
		PROFILE2("JumpPointCache reset");
		TIMER(L"JumpPointCache reset");

		m_Width = terrain->m_W;
		m_Height = terrain->m_H;

		ComputeRows(m_JumpPointsRight, *terrain, passClass, false, false);
		ComputeRows(m_JumpPointsLeft, *terrain, passClass, false, true);
		ComputeRows(m_JumpPointsUp, *terrain, passClass, true, false);
		ComputeRows(m_JumpPointsDown, *terrain, passClass, true, true);
	}

	size_t GetMemoryUsage() const
	{
		size_t bytes = 0;
		for (int i = 0; i < m_Width; ++i)
		{
			bytes += m_JumpPointsUp[i].GetMemoryUsage();
			bytes += m_JumpPointsDown[i].GetMemoryUsage();
		}
		for (int j = 0; j < m_Height; ++j)
		{
			bytes += m_JumpPointsRight[j].GetMemoryUsage();
			bytes += m_JumpPointsLeft[j].GetMemoryUsage();
		}
		return bytes;
	}

	/**
	 * Returns the next jump point (or goal point) to explore,
	 * at (ip, j) where i < ip.
	 * Returns i if there is no such point.
	 */
	int GetJumpPointRight(int i, int j, const PathGoal& goal) const
	{
		int ip;
		bool obstruction;
		m_JumpPointsRight[j].Get(i, ip, obstruction);
		// 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,
		// or the old ip if there is a (non-obstruction) jump point
		if (goal.NavcellRectContainsGoal(i + 1, j, ip - 1, j, &ip, NULL) || !obstruction)
			return ip;
		return i;
	}

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

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

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

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

LongPathfinder::LongPathfinder() :
	m_UseJPSCache(false),
	m_Grid(NULL), m_GridSize(0)
{
}

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

// Calculate heuristic cost from tile i,j to goal
// (This ought to be an underestimate for correctness)
PathCost LongPathfinder::CalculateHeuristic(int i, int j, int iGoal, int jGoal) const
{
	int di = abs(i - iGoal);
	int dj = abs(j - jGoal);
	int diag = std::min(di, dj);
	return PathCost(di - diag + dj - diag, diag);
}

// Do the A* processing for a neighbour tile i,j.
void LongPathfinder::ProcessNeighbour(int pi, int pj, int i, int j, PathCost pg, PathfinderState& state) const
{
	// Reject impassable tiles
	if (!PASSABLE(i, j))
		return;

	PathfindTile& n = state.tiles->get(i, j);

	if (n.IsClosed())
		return;

	PathCost dg;
	if (pi == i)
		dg = PathCost::horizvert(abs(pj - j));
	else if (pj == j)
		dg = PathCost::horizvert(abs(pi - i));
	else
	{
		ASSERT(abs((int)pi - (int)i) == abs((int)pj - (int)j)); // must be 45 degrees
		dg = PathCost::diag(abs((int)pi - (int)i));
	}

	PathCost g = pg + dg; // cost to this tile = cost to predecessor + delta from predecessor

	PathCost h = CalculateHeuristic(i, j, state.iGoal, state.jGoal);

	// If this is a new tile, compute the heuristic distance
	if (n.IsUnexplored())
	{
		// Remember the best tile we've seen so far, in case we never actually reach the target
		if (h < state.hBest)
		{
			state.hBest = h;
			state.iBest = i;
			state.jBest = j;
		}
	}
	else
	{
		// If we've already seen this tile, and the new path to this tile does not have a
		// better cost, then stop now
		if (g >= n.GetCost())
			return;

		// Otherwise, we have a better path.

		// If we've already added this tile to the open list:
		if (n.IsOpen())
		{
			// This is a better path, so replace the old one with the new cost/parent
			PathCost gprev = n.GetCost();
			n.SetCost(g);
			n.SetPred(pi, pj, i, j);
			state.open.promote(TileID(i, j), gprev + h, g + h, h);
			return;
		}
	}

	// Add it to the open list:
	n.SetStatusOpen();
	n.SetCost(g);
	n.SetPred(pi, pj, i, j);
	PriorityQueue::Item t = { TileID(i, j), g + h, h };
	state.open.push(t);
}

/*
 * In the JPS algorithm, after a tile is taken off the open queue,
 * we don't process every adjacent neighbour (as in standard A*).
 * Instead we only move in a subset of directions (depending on the
 * direction from the predecessor); and instead of moving by a single
 * cell, we move up to the next jump point in that direction.
 * The AddJumped... functions do this by calling ProcessNeighbour
 * on the jump point (if any) in a certain direction.
 * The HasJumped... functions return whether there is any jump point
 * in that direction.
 */

// JPS functions scan navcells towards one direction
// OnTheWay tests whether we are scanning towards the right direction, to avoid useless scans
inline bool OnTheWay(int i, int j, int di, int dj, int iGoal, int jGoal)
{
	if (dj != 0)
	{
		// We're not moving towards the goal
		if ((jGoal - j) * dj < 0)
			return false;
	}
	else if (j != jGoal)
		return false;

	if (di != 0)
	{
		// We're not moving towards the goal
		if ((iGoal - i) * di < 0)
			return false;
	}
	else if (i != iGoal)
		return false;

	return true;
}


void LongPathfinder::AddJumpedHoriz(int i, int j, int di, PathCost g, PathfinderState& state, bool detectGoal) const
{
	if (m_UseJPSCache)
	{
		int jump;
		if (di > 0)
			jump = state.jpc->GetJumpPointRight(i, j, state.goal);
		else
			jump = state.jpc->GetJumpPointLeft(i, j, state.goal);

		if (jump != i)
			ProcessNeighbour(i, j, jump, j, g, state);
	}
	else
	{
		ASSERT(di == 1 || di == -1);
		int ni = i + di;
		while (true)
		{
			if (!PASSABLE(ni, j))
				break;

			if (detectGoal && state.goal.NavcellContainsGoal(ni, j))
			{
				state.open.clear();
				ProcessNeighbour(i, j, ni, j, g, state);
				break;
			}

			if	((!PASSABLE(ni - di, j - 1) && PASSABLE(ni, j - 1)) ||
				(!PASSABLE(ni - di, j + 1) && PASSABLE(ni, j + 1)))
			{
				ProcessNeighbour(i, j, ni, j, g, state);
				break;
			}

			ni += di;
		}
	}
}

// Returns the i-coordinate of the jump point if it exists, else returns i
int LongPathfinder::HasJumpedHoriz(int i, int j, int di, PathfinderState& state, bool detectGoal) const
{
	if (m_UseJPSCache)
	{
		int jump;
		if (di > 0)
			jump = state.jpc->GetJumpPointRight(i, j, state.goal);
		else
			jump = state.jpc->GetJumpPointLeft(i, j, state.goal);

		return jump;
	}
	else
	{
		ASSERT(di == 1 || di == -1);
		int ni = i + di;
		while (true)
		{
			if (!PASSABLE(ni, j))
				return i;

			if (detectGoal && state.goal.NavcellContainsGoal(ni, j))
			{
				state.open.clear();
				return ni;
			}

			if	((!PASSABLE(ni - di, j - 1) && PASSABLE(ni, j - 1)) ||
				(!PASSABLE(ni - di, j + 1) && PASSABLE(ni, j + 1)))
				return ni;

			ni += di;
		}
	}
}

void LongPathfinder::AddJumpedVert(int i, int j, int dj, PathCost g, PathfinderState& state, bool detectGoal) const
{
	if (m_UseJPSCache)
	{
		int jump;
		if (dj > 0)
			jump = state.jpc->GetJumpPointUp(i, j, state.goal);
		else
			jump = state.jpc->GetJumpPointDown(i, j, state.goal);

		if (jump != j)
			ProcessNeighbour(i, j, i, jump, g, state);
	}
	else
	{
		ASSERT(dj == 1 || dj == -1);
		int nj = j + dj;
		while (true)
		{
			if (!PASSABLE(i, nj))
				break;

			if (detectGoal && state.goal.NavcellContainsGoal(i, nj))
			{
				state.open.clear();
				ProcessNeighbour(i, j, i, nj, g, state);
				break;
			}

			if	((!PASSABLE(i - 1, nj - dj) && PASSABLE(i - 1, nj)) ||
				(!PASSABLE(i + 1, nj - dj) && PASSABLE(i + 1, nj)))
			{
				ProcessNeighbour(i, j, i, nj, g, state);
				break;
			}

			nj += dj;
		}
	}
}

// Returns the j-coordinate of the jump point if it exists, else returns j
int LongPathfinder::HasJumpedVert(int i, int j, int dj, PathfinderState& state, bool detectGoal) const
{
	if (m_UseJPSCache)
	{
		int jump;
		if (dj > 0)
			jump = state.jpc->GetJumpPointUp(i, j, state.goal);
		else
			jump = state.jpc->GetJumpPointDown(i, j, state.goal);

		return jump;
	}
	else
	{
		ASSERT(dj == 1 || dj == -1);
		int nj = j + dj;
		while (true)
		{
			if (!PASSABLE(i, nj))
				return j;

			if (detectGoal && state.goal.NavcellContainsGoal(i, nj))
			{
				state.open.clear();
				return nj;
			}

			if	((!PASSABLE(i - 1, nj - dj) && PASSABLE(i - 1, nj)) ||
				(!PASSABLE(i + 1, nj - dj) && PASSABLE(i + 1, nj)))
				return nj;

			nj += dj;
		}
	}
}

/*
 * We never cache diagonal jump points - they're usually so frequent that
 * a linear search is about as cheap and avoids the setup cost and memory cost.
 */
void LongPathfinder::AddJumpedDiag(int i, int j, int di, int dj, PathCost g, PathfinderState& state) const
{
	// 	ProcessNeighbour(i, j, i + di, j + dj, g, state);
	// 	return;

	ASSERT(di == 1 || di == -1);
	ASSERT(dj == 1 || dj == -1);

	int ni = i + di;
	int nj = j + dj;
	bool detectGoal = OnTheWay(i, j, di, dj, state.iGoal, state.jGoal);
	while (true)
	{
		// Stop if we hit an obstructed cell
		if (!PASSABLE(ni, nj))
			return;

		// Stop if moving onto this cell caused us to
		// touch the corner of an obstructed cell
		if (!PASSABLE(ni - di, nj) || !PASSABLE(ni, nj - dj))
			return;

		// Process this cell if it's at the goal
		if (detectGoal && state.goal.NavcellContainsGoal(ni, nj))
		{
			state.open.clear();
			ProcessNeighbour(i, j, ni, nj, g, state);
			return;
		}

		int fi = HasJumpedHoriz(ni, nj, di, state, detectGoal && OnTheWay(ni, nj, di, 0, state.iGoal, state.jGoal));
		int fj = HasJumpedVert(ni, nj, dj, state, detectGoal && OnTheWay(ni, nj, 0, dj, state.iGoal, state.jGoal));
		if (fi != ni || fj != nj)
		{
			ProcessNeighbour(i, j, ni, nj, g, state);
			g += PathCost::diag(abs(ni - i));

			if (fi != ni)
				ProcessNeighbour(ni, nj, fi, nj, g, state);

			if (fj != nj)
				ProcessNeighbour(ni, nj, ni, fj, g, state);

			return;
		}

		ni += di;
		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
{
	PROFILE2("ComputePathJPS");
	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, std::shared_ptr<JumpPointCache>>::const_iterator it = m_JumpPointCache.find(passClass);
		if (it != m_JumpPointCache.end())
			state.jpc = it->second.get();

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

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

	if (!IS_PASSABLE(m_Grid->get(i0, j0), passClass))
	{
		// The JPS pathfinder requires units to be on passable tiles
		// (otherwise it might crash), so handle the supposedly-invalid
		// state specially
		hierPath.FindNearestPassableNavcell(i0, j0, passClass);
	}

	state.goal = origGoal;

	// Make the goal reachable. This includes shortening the path if the goal is in a non-passable
	// region, transforming non-point goals to reachable point goals, etc.
	hierPath.MakeGoalReachable(i0, j0, state.goal, passClass);

	ENSURE(state.goal.type == PathGoal::POINT);

	// If we're already at the goal tile, then move directly to the exact goal coordinates
	if (state.goal.NavcellContainsGoal(i0, j0))
	{
		path.m_Waypoints.emplace_back(Waypoint{ state.goal.x, state.goal.z });
		return;
	}

	Pathfinding::NearestNavcell(state.goal.x, state.goal.z, state.iGoal, state.jGoal, m_GridSize, m_GridSize);

	ENSURE((state.goal.x / Pathfinding::NAVCELL_SIZE).ToInt_RoundToNegInfinity() == state.iGoal);
	ENSURE((state.goal.z / Pathfinding::NAVCELL_SIZE).ToInt_RoundToNegInfinity() == state.jGoal);

	state.passClass = passClass;

	state.steps = 0;

	state.tiles = new PathfindTileGrid(m_Grid->m_W, m_Grid->m_H);
	state.terrain = m_Grid;

	state.iBest = i0;
	state.jBest = j0;
	state.hBest = CalculateHeuristic(i0, j0, state.iGoal, state.jGoal);

	PriorityQueue::Item start = { TileID(i0, j0), PathCost() };
	state.open.push(start);
	state.tiles->get(i0, j0).SetStatusOpen();
	state.tiles->get(i0, j0).SetPred(i0, j0, i0, j0);
	state.tiles->get(i0, j0).SetCost(PathCost());

	while (true)
	{
		++state.steps;

		// If we ran out of tiles to examine, give up
		if (state.open.empty())
			break;

		// Move best tile from open to closed
		PriorityQueue::Item curr = state.open.pop();
		u16 i = curr.id.i();
		u16 j = curr.id.j();
		state.tiles->get(i, j).SetStatusClosed();

		// If we've reached the destination, stop
		if (state.goal.NavcellContainsGoal(i, j))
		{
			state.iBest = i;
			state.jBest = j;
			state.hBest = PathCost();
			break;
		}

		PathfindTile tile = state.tiles->get(i, j);
		PathCost g = tile.GetCost();

		// Get the direction of the predecessor tile from this tile
		int dpi = tile.GetPredDI();
		int dpj = tile.GetPredDJ();
		dpi = (dpi < 0 ? -1 : dpi > 0 ? 1 : 0);
		dpj = (dpj < 0 ? -1 : dpj > 0 ? 1 : 0);

		if (dpi != 0 && dpj == 0)
		{
			// Moving horizontally from predecessor
			if (!PASSABLE(i + dpi, j - 1))
			{
				AddJumpedDiag(i, j, -dpi, -1, g, state);
				AddJumpedVert(i, j, -1, g, state, OnTheWay(i, j, 0, -1, state.iGoal, state.jGoal));
			}
			if (!PASSABLE(i + dpi, j + 1))
			{
				AddJumpedDiag(i, j, -dpi, +1, g, state);
				AddJumpedVert(i, j, +1, g, state, OnTheWay(i, j, 0, +1, state.iGoal, state.jGoal));
			}
			AddJumpedHoriz(i, j, -dpi, g, state, OnTheWay(i, j, -dpi, 0, state.iGoal, state.jGoal));
		}
		else if (dpi == 0 && dpj != 0)
		{
			// Moving vertically from predecessor
			if (!PASSABLE(i - 1, j + dpj))
			{
				AddJumpedDiag(i, j, -1, -dpj, g, state);
				AddJumpedHoriz(i, j, -1, g, state,OnTheWay(i, j, -1, 0, state.iGoal, state.jGoal));
			}
			if (!PASSABLE(i + 1, j + dpj))
			{
				AddJumpedDiag(i, j, +1, -dpj, g, state);
				AddJumpedHoriz(i, j, +1, g, state,OnTheWay(i, j, +1, 0, state.iGoal, state.jGoal));
			}
			AddJumpedVert(i, j, -dpj, g, state, OnTheWay(i, j, 0, -dpj, state.iGoal, state.jGoal));
		}
		else if (dpi != 0 && dpj != 0)
		{
			// Moving diagonally from predecessor
			AddJumpedHoriz(i, j, -dpi, g, state, OnTheWay(i, j, -dpi, 0, state.iGoal, state.jGoal));
			AddJumpedVert(i, j, -dpj, g, state, OnTheWay(i, j, 0, -dpj, state.iGoal, state.jGoal));
			AddJumpedDiag(i, j, -dpi, -dpj, g, state);
		}
		else
		{
			// No predecessor, i.e. the start tile
			// Start searching in every direction

			// XXX - check passability?

			bool passl = PASSABLE(i - 1, j);
			bool passr = PASSABLE(i + 1, j);
			bool passd = PASSABLE(i, j - 1);
			bool passu = PASSABLE(i, j + 1);

			if (passl && passd)
				ProcessNeighbour(i, j, i-1, j-1, g, state);
			if (passr && passd)
				ProcessNeighbour(i, j, i+1, j-1, g, state);
			if (passl && passu)
				ProcessNeighbour(i, j, i-1, j+1, g, state);
			if (passr && passu)
				ProcessNeighbour(i, j, i+1, j+1, g, state);
			if (passl)
				ProcessNeighbour(i, j, i-1, j, g, state);
			if (passr)
				ProcessNeighbour(i, j, i+1, j, g, state);
			if (passd)
				ProcessNeighbour(i, j, i, j-1, g, state);
			if (passu)
				ProcessNeighbour(i, j, i, j+1, g, state);
		}
	}

	// Reconstruct the path (in reverse)
	u16 ip = state.iBest, jp = state.jBest;
	while (ip != i0 || jp != j0)
	{
		PathfindTile& n = state.tiles->get(ip, jp);
		entity_pos_t x, z;
		Pathfinding::NavcellCenter(ip, jp, x, z);
		path.m_Waypoints.emplace_back(Waypoint{ x, z });

		// Follow the predecessor link
		ip = n.GetPredI(ip);
		jp = n.GetPredJ(jp);
	}
	// The last waypoint is slightly incorrect (it's not the goal but the center
	// of the navcell of the goal), so replace it
	if (!path.m_Waypoints.empty())
		path.m_Waypoints.front() = { state.goal.x, state.goal.z };

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

	// Save this grid for debug display
	if (m_Debug.Overlay)
	{
		std::lock_guard<std::mutex> lock(g_DebugMutex);
		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

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

	if (maxDist > fixed::Zero())
	{
		CFixedVector2D start(x0, z0);
		CFixedVector2D first(path.m_Waypoints.back().x, path.m_Waypoints.back().z);
		CFixedVector2D offset = first - start;
		if (offset.CompareLength(maxDist) > 0)
		{
			offset.Normalize(maxDist);
			path.m_Waypoints.emplace_back(Waypoint{ (start + offset).X, (start + offset).Y });
		}
	}

	if (path.m_Waypoints.size() < 2)
		return;

	std::vector<Waypoint>& waypoints = path.m_Waypoints;
	std::vector<Waypoint> newWaypoints;

	CFixedVector2D prev(waypoints.front().x, waypoints.front().z);
	newWaypoints.push_back(waypoints.front());
	for (size_t k = 1; k < waypoints.size() - 1; ++k)
	{
		CFixedVector2D ahead(waypoints[k + 1].x, waypoints[k + 1].z);
		CFixedVector2D curr(waypoints[k].x, waypoints[k].z);

		if (maxDist > fixed::Zero() && (curr - prev).CompareLength(maxDist) > 0)
		{
			// We are too far away from the previous waypoint, so create one in
			// between and continue with the improvement of the path
			prev = prev + (curr - prev) / 2;
			newWaypoints.emplace_back(Waypoint{ prev.X, prev.Y });
		}

		// If we're mostly straight, don't even bother.
		if ((ahead - curr).Perpendicular().Dot(curr - prev).Absolute() <= fixed::Epsilon() * 100)
			continue;

		if (!Pathfinding::CheckLineMovement(prev.X, prev.Y, ahead.X, ahead.Y, passClass, *m_Grid))
		{
			prev = CFixedVector2D(waypoints[k].x, waypoints[k].z);
			newWaypoints.push_back(waypoints[k]);
		}
	}
	newWaypoints.push_back(waypoints.back());
	path.m_Waypoints.swap(newWaypoints);
}

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

	if (!m_Debug.Grid)
		return;

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

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

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

void LongPathfinder::ComputePath(const HierarchicalPathfinder& hierPath, entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal,
    pass_class_t passClass, WaypointPath& path) const
{
	if (!m_Grid)
	{
		LOGERROR("The pathfinder grid hasn't been setup yet, aborting ComputeJPSPath");
		return;
	}

	ComputeJPSPath(hierPath, x0, z0, origGoal, passClass, path);
}
void LongPathfinder::ComputePath(const HierarchicalPathfinder& hierPath, entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal,
	pass_class_t passClass, std::vector<CircularRegion> excludedRegions, WaypointPath& path)
{
	GenerateSpecialMap(passClass, excludedRegions);
	ComputeJPSPath(hierPath, x0, z0, origGoal, SPECIAL_PASS_CLASS, path);
}

inline bool InRegion(u16 i, u16 j, CircularRegion region)
{
	fixed cellX = Pathfinding::NAVCELL_SIZE * i;
	fixed cellZ = Pathfinding::NAVCELL_SIZE * j;

	return CFixedVector2D(cellX - region.x, cellZ - region.z).CompareLength(region.r) <= 0;
}

void LongPathfinder::GenerateSpecialMap(pass_class_t passClass, std::vector<CircularRegion> excludedRegions)
{
	for (u16 j = 0; j < m_Grid->m_H; ++j)
	{
		for (u16 i = 0; i < m_Grid->m_W; ++i)
		{
			NavcellData n = m_Grid->get(i, j);
			if (!IS_PASSABLE(n, passClass))
			{
				n |= SPECIAL_PASS_CLASS;
				m_Grid->set(i, j, n);
				continue;
			}

			for (CircularRegion& region : excludedRegions)
			{
				if (!InRegion(i, j, region))
					continue;

				n |= SPECIAL_PASS_CLASS;
				break;
			}
			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_TERRAIN_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[(jp*w + ip)*4+3] == 0)
						{
							data[(jp*w + ip)*4+0] = 0xFF;
							data[(jp*w + ip)*4+1] = 0xFF;
							data[(jp*w + ip)*4+2] = 0xFF;
							data[(jp*w + 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);
}