Game Development

Pathfinding I

Tim-Oliver bothered me today with a question about A* pathfinding. It has been some time since I wrote our A* implementation in DVW, so I had to dig into this topic again. I used this tutorial and wrote some code. Note that this is just a straightforward implementation of the pseudo code presented in this article. I wouldn’t write productive code that way. Anyway, this might come handy for someone, so I’ll present it here. Next time, I will replace the open list with a binary heap.
Click here to see the source:

#include <queue>
#include <limits>
#include <algorithm>
#include <iostream>
const int MAP_HEIGHT = 13;
const int MAP_WIDTH = 27;
static char MAP[] =
	"XXXXXXXXXXXXXXXXXXXXXXXXXXX"
	"X.........................X"
	"X........XXXXXXXX.........X"
	"X........X......X.........X"
	"X...............X.........X"
	"XXXXXX.XXXXXXXXXX.........X"
	"X....X.X..................X"
	"X....X.XXXXXXXXXXXXXXXXXXXX"
	"X....X.........X..........X"
	"X....XXXXXXXX..X..XXXXXX..X"
	"X....X......X..X..X.......X"
	"X....X......X.....X.......X"
	"XXXXXXXXXXXXXXXXXXXXXXXXXXX";
struct Point
{
	int x, y;
	bool operator== ( const Point& p )
	{
		return ( x == p.x && y == p.y );
	}
};
typedef std::deque< Point > QueuePoint;
Point NEIGHBOURS[ 8 ] = { { 0, -1 }, { 1, -1 }, { 1, 0 }, { 1, 1 }, { 0, 1 }, { -1, 1 }, { -1, 0 }, { -1, -1 } };
int COST[ 8 ] = { 10, 14, 10, 14, 10, 14, 10, 14 };
class Pathfinder
{
public:
	Pathfinder( char* theMap, int width, int height )
	:	theMap( theMap ),
		width( width ),
		height( height )
	{
		predecessors = new Point[ width * height ];
		cost = new int[ width * height ];
		memset( cost, 0, width * height * sizeof( int ) );
	}
	~Pathfinder()
	{
		cleanUp();
	}
	void cleanUp()
	{
		open.clear();
		closed.clear();
		delete[] predecessors;
		delete[] cost;
	}
	QueuePoint findPath( Point start, Point end )
	{
		QueuePoint path;
		Point current;
		// Add the starting square (or node) to the open list.
		open.push_back( start );
		// Repeat the following:
		// Stop when you: Fail to find the target square, and the open list is empty.
		while( open.size() )
		{
			// Look for the lowest F cost square on the open list. We refer to this as the current square.
			int minCost = std::numeric_limits<int>::max();
			QueuePoint::iterator min = open.end();
			for( QueuePoint::iterator it = open.begin(); it != open.end(); ++it )
			{
				Point p = *it;
				int localCost = cost[ p.y * width + p.x ];
				if( localCost < minCost )
				{
					min = it;
					minCost = localCost;
				}
			}
			current = *min;
			// Stop when you Add the target square to the closed list, in which case the path has been found
			if( current == end )
			{
				break;
			}
			// Switch it to the closed list.
			closed.push_back( current );
			open.erase( min );
			int lastCost = cost[ current.y * width + current.x ];
			// For each of the 8 squares adjacent to this current square …
			for( int i = 0; i < 8; ++i )
			{
				Point newPoint;
				newPoint.x = current.x + NEIGHBOURS[ i ].x;
				newPoint.y = current.y + NEIGHBOURS[ i ].y;
				int idx = newPoint.y * width + newPoint.x;
				bool positionOk = newPoint.x >= 0 && newPoint.x < width && newPoint.y >= 0 && newPoint.y < height;
				bool mapFree = positionOk && theMap[ idx ] != 'X';
				bool notInClosedList = std::find( closed.begin(), closed.end(), newPoint ) == closed.end();
				// If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following.
				if( positionOk // out of bounds: ignore
					&& mapFree // blocked: ignore
					&& notInClosedList // on closed list: ignore
					)
				{
					// If it isn't on the open list, add it to the open list.
					// Make the current square the parent of this square.
					// Record the F, G, and H costs of the square.
					if( std::find( open.begin(), open.end(), newPoint ) == open.end() )
					{
						// not yet in open list
						open.push_back( newPoint );
						predecessors[ idx ] = current;
						cost[ idx ] = lastCost + COST[ i ];
					}
					else
					{
						/*
						If it is on the open list already, check to see if this path to that square is better,
						using G cost as the measure. A lower G cost means that this is a better path. If so,
						change the parent of the square to the current square, and recalculate the G and F scores
						of the square. If you are keeping your open list sorted by F score, you may need to resort
						the list to account for the change.
						*/
						if( lastCost + COST[ i ] < cost[ idx ] )
						{
							predecessors[ idx ] = current;
							cost[ idx ] = lastCost + COST[ i ];
						}
					}
				}
			}
		}
		if( open.size() )
		{
			// Save the path.
			// Working backwards from the target square, go from each square to its parent square until you reach the starting square.
			// That is your path.
			while( current.x != start.x || current.y != start.y )
			{
				path.push_front( current );
				current = predecessors[ current.y * width + current.x ];
			}
		}
		return path;
	}
private:
	char* theMap;
	int width, height;
	Point* predecessors;
	int* cost;
	QueuePoint open; // open nodes
	QueuePoint closed; // closed nodes
};
int main( int argc, char* argv[] )
{
	Pathfinder pf( MAP, MAP_WIDTH, MAP_HEIGHT );
	Point start = { 8, 6 };
	Point end = { 25, 11 };
	QueuePoint path = pf.findPath( start, end );
	for( QueuePoint::iterator it = path.begin(); it != path.end(); ++it )
	{
		const Point& p = *it;
		MAP[ p.y * MAP_WIDTH + p.x ] = '*';
	}
	MAP[ start.y * MAP_WIDTH + start.x ] = 'S';
	MAP[ end.y * MAP_WIDTH + end.x ] = 'E';
	for( int y = 0; y < MAP_HEIGHT; ++y )
	{
		std::string row( &MAP[ MAP_WIDTH * y ], MAP_WIDTH );
		std::cout << row.c_str() << std::endl;
	}
	return 0;
}