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

Next Article

More VS2005 enhancements