astar.cpp

#include "astar.hpp"
#include "vector.hpp"

#include <algorithm>
#include <iterator>
#include <vector>
#include <queue>
#include <cassert>

using Index = int;

struct Cell
{
	enum class State
	{
		Obstructed,
		Reached,
		NotReached
	};

	State state;
	/// Which Cell is the previous one on the shortest path to this one? Filled in once reached.
	Index predecessor;
};

struct Node
{
	/// number of steps required to reach this position
	int numSteps;
	Vector2i position;
};

int FindPath( const int nStartX, const int nStartY,
	const int nTargetX, const int nTargetY,
	const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
	int* pOutBuffer, const int nOutBufferSize )
{
	const Vector2i start{ nStartX, nStartY };
	const Vector2i target{ nTargetX, nTargetY };

	// Create mutable copy of map with additional info on shortest path to any given point (for fast (O(1)) indexing)
	std::vector< Cell > cells;
	cells.reserve( nMapWidth * nMapHeight );
	std::transform(
		pMap, pMap + nMapWidth * nMapHeight,
		std::back_inserter( cells ),
		[]( const unsigned char traversable )
	{
		return Cell{ traversable ? Cell::State::NotReached : Cell::State::Obstructed };
	}
	);

	/// Convert a 2D Position to an Index
	auto toIndex = [ nMapWidth ]( const Vector2i& position ) -> Index
	{
		return position[ 1 ] * nMapWidth + position[ 0 ];
	};
	/// Retreive the cell at a given position
	auto getCell = [ &cells, &toIndex ]( const Vector2i& position ) -> Cell&
	{
		return cells[ toIndex( position ) ];
	};
	/// Whether a given coordinate is inside the map
	auto isValid = [ nMapWidth, nMapHeight ]( const Vector2i& position ) -> bool
	{
		return
			position[ 0 ] >= 0 && position[ 0 ] < nMapWidth &&
			position[ 1 ] >= 0 && position[ 1 ] < nMapHeight;
	};
	const Index startIndex = toIndex( start );


	/// (A*) Heuristic for estimating the total number of steps to the target - uses euclidean distance.
	auto estimatedSquaredSteps = [ &target ]( const Node& node ) -> int
	{
		Vector2i difference = target - node.position;
		int squaredDistance = difference * difference;
		return node.numSteps * node.numSteps + squaredDistance;
	};

	/// Compare two Nodes based on their estimated steps to the target; larger means shorter estimated path.
	auto compareNodes = [ &estimatedSquaredSteps ]( const Node& lhs, const Node& rhs ) -> bool
	{
		return estimatedSquaredSteps( lhs ) > estimatedSquaredSteps( rhs );
	};

	using Queue = std::priority_queue< Node, std::vector< Node >, decltype( compareNodes ) >;
	Queue queue( compareNodes );

	/// Add the neighbours of a given cell to the queue, if they have not been reached yet
	auto pushNeighbours = [ & ]( const Node& node )
	{
		// Offsets of neighbouring cells
		static constexpr Vector2i neighbours[]{
			{ 1, 0 },
			{ -1, 0 },
			{ 0, 1 },
			{ 0, -1 }
		};
		for( const auto& offset : neighbours )
		{
			const auto neighbour = node.position + offset;
			if( isValid( neighbour ) )
			{
				auto& cell = getCell( neighbour );
				if( cell.state == Cell::State::NotReached )
				{
					cell.state = Cell::State::Reached;
					cell.predecessor = toIndex( node.position );
					queue.push( Node{ node.numSteps + 1, neighbour } );
				}
			}
		}
	};

	queue.push( Node{ 0, start } );
	getCell( start ).state = Cell::State::Reached;

	while( !queue.empty() )
	{
		Node node = queue.top();
		queue.pop();
		if( node.position == target )
		{
			if( 0 < node.numSteps && node.numSteps <= nOutBufferSize )
			{
				Index location = toIndex( node.position );
				int* outIt = pOutBuffer + node.numSteps - 1;
				while( location != startIndex )
				{
					assert( outIt >= pOutBuffer );
					assert( outIt < pOutBuffer + nOutBufferSize );
					*outIt = location;
					--outIt;
					location = cells[ location ].predecessor;
				}
			}
			return node.numSteps;
		}
		else
		{
			pushNeighbours( node );
		}
	}
	
	return -1;
}