Navigation: Artificial Intelligence

Tutorial 1 - Pathfinding


Overview

There are some good books on Game AI. I've always been interested in pathfinding algorithms. Like getting a tank to navigate around obstacles and reach an enemy base. Here I want to describe the A* algorithm.

The A* Algorithm

It's fun. To do pathfinding using the A* algorithm you need to represent your game world as a grid. Each grid cell can be walkable or unwalkable. Then, if you have a unit like a tank or maybe a character, the pathfinding algorithm will find a path from the start node to the goal node. The path is a list of nodes from start to finish that the unit is allowed to walk on, if a path is found.

Graphics

To test out the algorithm, we need to draw the unit that will move along the path and we need to draw walls at the positions of the unwalkable nodes. You can use the "Sprite" class for this.
Look at the code if you need to.

Implementing The A* Algorithm

Basically we need to start with a grid. You can represent a path like this(See Figure 1):


Figure 1

Each grid cell is called a node. The grid itself is a vector, or list, of nodes. A node has a position, x,y and it is either walkable or not walkable. The A* algorithm attempts to find the shortest route from a start node to a goal node. The path itself is also a list of nodes, connected nodes. I have defined a class to represent a node as follows:
class Node
{
public:
	Node();
	~Node();

	D3DXVECTOR2 Pos;
	bool walkable;

	int index;
	float gcost;
	float hcost;
	float fcost;
	Node* NodeParent;

	void ResetStatus();
};
The other members, index, gcost, hcost, fcost and NodeParent are used to find a path. I will explain these in a moment.

Basically, the way it works is that each node has a cost that it takes to get to that node and the nodes with the least costs are the favoured ones. For example it costs less to travel horizontally than it does diagonally.

In the A* FindPath function, you declare an open list and a closed list. The open list is a list of possible candidate nodes and the closed list is a list of definite candidate nodes. Once the fcost has been calculated for each node in the open list, the fcost is the deciding factor that decides what node gets added to the closed list. The node with the lowest fcost in the open list gets added to the closed list.

If at any point the goal node is added to the closed list then the path has been found.

When a node is added to the closed list, it is a definite path node so the next logical step is to consider nodes adjacent to it (See Figure 2).


Figure 2

For each adjacent square, consider it as a candidate only if it is walkable(And not already in closed list or open list). Also, for each adjacent square, let horizontal and vertical squares cost 10 and diagonal cost 14, initially. Add each adjacent square to the open list if it is walkable(A possible candidate) and make the bright orange square(Figure 2) the parent of each of these squares. You will see why a parent is necessary later.

Ok, now for each adjacent square, the same ones(Figure 2) we set the gcost of these squares. The gcost is the total horizontal/vertical+diagonal movement cost to get to that square, which is the gcost of the parent + 10 or 14.

After setting the gcost, set the hcost of each of these squares too. The hcost is an estimated cost. It is the difference in horizontal squares + the difference in vertical squares from the adjacent square to the goal node.(See Figure 3).


Figure 3

Finally the fcost for each adjacent square is the sum of it's gcost + hcost.

One more check we need to do for the A*. If one of the adjecent squares is already in the open list and it is shorter to travel from the node we have just added to the closed list, currentNode, to the adjacent square, then we can make the current node the parent of that adjacent square and update it's gcost. For example:
	//adjSquare Already in open list.
						
	//Is it a shorter path to go from currentNode to
	//adjSquare or shorter to go from existing parent
	//of adjSquare to adjSquare?

	float tmpGCost = adjSquare->gcost;

	//Is adjSquare orthogonal or diagonal to 
	//currentNode?
	float horz_dist = abs(axi - ix);
	float vert_dist = abs(ayi - iy);

	if(horz_dist == 1 && vert_dist == 1)
	{
		//Diagonal
		tmpGCost = currentNode->gcost + 14.0f;
	}
	else if(horz_dist == 1 || vert_dist == 1){
		//Orthogonal
		tmpGCost = currentNode->gcost + 10.0f;
	}
                    
	//Is the new path shorter than the original?
	if(tmpGCost < adjSquare->gcost)
	{
		//Yes, change parent of adjacent node to currentNode
		adjSquare->gcost = tmpGCost;
		adjSquare->fcost = adjSquare->gcost + adjSquare->hcost;
		adjSquare->NodeParent = currentNode;
	}
At this point, if a path has been found, the path is determined by the parent node of each parent starting from the goal node. For example:
	//Back track from goal
	//Create path.

	if(GoalNode->NodeParent == NULL){
		//No path available to goal.
		return path;
	}

	Node* path_node = GoalNode;
	path.insert(path.begin() + 0, *path_node);

	while(true)
	{
		if(path_node->NodeParent == NULL){
			//No Parent.
			break;
		}

		path_node = path_node->NodeParent;
		path.insert(path.begin() + 0, *path_node);
	}

	//Reset Node Values for future function calls
	for(UINT i=0; i<GridNodes.size(); i++)
	{
		GridNodes[i].ResetStatus();
	}

	return path;

Modifications

You can modify the algrorithm to prevent characters from walking diagonally through blocks or walls with the following code:
//New Code Start
				
//Is adjacent square to the left of it's parent?
if(axi == xi - 1)
{
	//Is adjacent square above it's parent?
	if(ayi == yi - 1)
	{
		//Square is not walkable if there is a block to the left of it's parent(currentNode)
		Node* node = GetNode(xi-1, yi);
						
		if(node != NULL)
		{
			if(node->walkable == false){continue;}
		}
	                    
		//Square is not walkable if there is a block above of it's parent(currentNode)
		node = GetNode(xi, yi-1);

		if(node != NULL)
		{
			if(node->walkable == false){continue;}
		}
	}
	//Is adjacent square below it's parent?
	else if(ayi == yi + 1)
	{
		//Square is not walkable if there is a block to the left of it's parent(currentNode)
		Node* node = GetNode(xi-1, yi);
						
		if(node != NULL)
		{
			if(node->walkable == false){continue;}
		}
	                    
		//Square is not walkable if there is a block below of it's parent(currentNode)
		node = GetNode(xi, yi+1);
						
		if(node != NULL)
		{
			if(node->walkable == false){continue;}
		}
	}
}
else //Is adjacent square to the right of it's parent?
if(axi == xi + 1)
{
	//Is adjacent square above it's parent?
	if(ayi == yi - 1)
	{
		//Square is not walkable if there is a block to the right of it's parent(currentNode)
		Node* node = GetNode(xi+1, yi);
						
		if(node != NULL)
		{
			if(node->walkable == false){continue;}
		}
	                    
		//Square is not walkable if there is a block above of it's parent(currentNode)
		node = GetNode(xi, yi-1);

		if(node != NULL)
		{
			if(node->walkable == false){continue;}
		}
	}
	//Is adjacent square below it's parent?
	else if(ayi == yi + 1)
	{
		//Square is not walkable if there is a block to the right of it's parent(currentNode)
		Node* node = GetNode(xi+1, yi);
						
		if(node != NULL)
		{
			if(node->walkable == false){continue;}
		}
	                    
		//Square is not walkable if there is a block below of it's parent(currentNode)
		node = GetNode(xi, yi+1);
						
		if(node != NULL)
		{
			if(node->walkable == false){continue;}
		}
	}
}
				
//New Code End

The Full Code

Click to move the barbarian.



The Application files for this tutorial:
  • Macros.h
  • Mouse.h
  • Sprite.h
  • Object.h
  • Pathfinding.h
  • main.cpp
  • Mouse.cpp
  • Sprite.cpp
  • Object.cpp
  • Pathfinding.cpp
Download Source

Download Application

The Pathfinding Function

The FindPath function:
std::vector<Node> PathFindingGrid::FindPath(Node* StartNode, Node* GoalNode)
{	
	vector<Node> path;

	//Return immediately if Start Node or Goal Node is invalid.
	if(StartNode == NULL || GoalNode == NULL)
	{
		return path;
	}
	//Otherwise, nodes exist; check if they are walkable.
	if(GoalNode->walkable == false || StartNode->walkable == false)
	{
		return path;
	}

	//Otherwise, Find path

	vector<Node*> open_list;
	vector<Node*> closed_list;

	//Add the starting node to the open list.
	open_list.push_back(StartNode);

	while(open_list.size() > 0)
	{
		//Look for the lowest F cost square in the open list.
		float fscore = 0;
	    
		Node* currentNode = open_list[0];

		//get the fcost of the first item in the open list.
		fscore = currentNode->fcost;
	    
		int pos = -1;
		//For each element in the open list.
		for(UINT j=0; j<open_list.size(); j++)
		{
			if(open_list[j]->fcost <= fscore)
			{
				currentNode = open_list[j];
				fscore = currentNode->fcost;
				pos = j;
			}
		}

		//(Drop it from the open list)
		//Add it to the closed list
		open_list.erase(open_list.begin()+pos);
	    
		//If it doesn't exist in the closed list already, add it.
		vector<Node*>::iterator it = find (closed_list.begin(), closed_list.end(), currentNode);
		
		if(it == closed_list.end())
		{
			closed_list.push_back(currentNode);
		}

		//Has the goal node been added to the closed list?
		if(currentNode == GoalNode){
			//Yes.
			break;
		}

		//Current node is square with least F cost added to the closed list
	    
		//For each of the 8 squares adjacent to this square...
		Node* adjLeft = NULL;
		Node* adjTop = NULL;
		Node* adjRight = NULL;
		Node* adjBottom = NULL;
		Node* adjNW = NULL;
		Node* adjNE = NULL;
		Node* adjSE = NULL;
		Node* adjSW = NULL;
	    
		//Node*, adjValue(10 or 14)
		map<Node*, float> AdjacentSquares;

		//Get the ix and iy value of currentSquare in the GridNodes vector
		int iy = (int)floor((float)currentNode->index / HorzNodeCount);
		int ix = (int)((float)currentNode->index - ((float)iy*HorzNodeCount));

		int index = 0;
		
		int adj_ix = -1;
		int adj_iy = -1;
		
		//adjLeft
		adj_ix = (ix-1);
		adj_iy = (iy);
		
		if(adj_ix >=0 && adj_ix < HorzNodeCount && adj_iy >=0 && adj_iy < VertNodeCount)
		{
			index = (adj_iy) * (int)HorzNodeCount + adj_ix;
			adjLeft = &GridNodes[index];
		}
		//adjTop
		adj_ix = (ix);
		adj_iy = (iy-1);
		
		if(adj_ix >=0 && adj_ix < HorzNodeCount && adj_iy >=0 && adj_iy < VertNodeCount)
		{
			index = (adj_iy) * (int)HorzNodeCount + adj_ix;
			adjTop = &GridNodes[index];
		}
		//adjRight
		adj_ix = (ix+1);
		adj_iy = (iy);
		
		if(adj_ix >=0 && adj_ix < HorzNodeCount && adj_iy >=0 && adj_iy < VertNodeCount)
		{
			index = (adj_iy) * (int)HorzNodeCount + adj_ix;
			adjRight = &GridNodes[index];
		}
		//adjBottom
		adj_ix = (ix);
		adj_iy = (iy+1);
		
		if(adj_ix >=0 && adj_ix < HorzNodeCount && adj_iy >=0 && adj_iy < VertNodeCount)
		{
			index = (adj_iy) * (int)HorzNodeCount + adj_ix;
			adjBottom = &GridNodes[index];
		}
		//adjNW
		adj_ix = (ix-1);
		adj_iy = (iy+1);
		
		if(adj_ix >=0 && adj_ix < HorzNodeCount && adj_iy >=0 && adj_iy < VertNodeCount)
		{
			index = (adj_iy) * (int)HorzNodeCount + adj_ix;
			adjNW = &GridNodes[index];
		}
		//adjNE
		adj_ix = (ix+1);
		adj_iy = (iy+1);
		
		if(adj_ix >=0 && adj_ix < HorzNodeCount && adj_iy >=0 && adj_iy < VertNodeCount)
		{
			index = (adj_iy) * (int)HorzNodeCount + adj_ix;
			adjNE = &GridNodes[index];
		}
		//adjSE
		adj_ix = (ix+1);
		adj_iy = (iy-1);
		
		if(adj_ix >=0 && adj_ix < HorzNodeCount && adj_iy >=0 && adj_iy < VertNodeCount)
		{
			index = (adj_iy) * (int)HorzNodeCount + adj_ix;
			adjSE = &GridNodes[index];
		}
		//adjSW
		adj_ix = (ix-1);
		adj_iy = (iy-1);
		
		if(adj_ix >=0 && adj_ix < HorzNodeCount && adj_iy >=0 && adj_iy < VertNodeCount)
		{
			index = (adj_iy) * (int)HorzNodeCount + adj_ix;
			adjSW = &GridNodes[index];
		}

		//Add adjacent squares to the open list.
		AdjacentSquares[adjLeft] = 10.0f;
		AdjacentSquares[adjTop] = 10.0f;
		AdjacentSquares[adjRight] = 10.0f;
		AdjacentSquares[adjBottom] = 10.0f;
	    
		AdjacentSquares[adjNW] = 14.0f;
		AdjacentSquares[adjNE] = 14.0f;
		AdjacentSquares[adjSE] = 14.0f;
		AdjacentSquares[adjSW] = 14.0f;

		//Iterate over the adjacent squares
		Node* adjSquare = NULL;
		float adjValue = 0;

		for(map<Node*, float>::iterator it=AdjacentSquares.begin(); it != AdjacentSquares.end(); it++)
		{
			//set adjSquare to key entry.
			adjSquare = it->first;

			if(adjSquare == NULL)
			{
				//NULL entry into map might have been added.
				continue;
			}

			if(adjSquare->walkable == false)
			{
				continue;
			}

			//Determine if adjSquare is diagonal or orthogonal...

			//get cost.
			adjValue = it->second;
			
			//Now we have an adjacent square and an adjacency value
			//If it is in the closed list, ignore it.

			vector<Node*>::iterator it2 = find (closed_list.begin(), closed_list.end(), adjSquare);
		
			if(it2 == closed_list.end())
			{
				//doesn't exist in closed list.

				//determine whether this square is walkable in terms of nearby blocks
				Node* parent = currentNode;
	            
				//Get parent xi,yi
				int yi = (int)floor((float)parent->index / HorzNodeCount);
				int xi = parent->index - (yi*(int)HorzNodeCount);

				//Get adjacent square xi,yi
				int ayi = (int)floor((float)adjSquare->index / HorzNodeCount);
				int axi = adjSquare->index - (ayi*(int)HorzNodeCount);
	            
				//New Code Start
				
				//Is adjacent square to the left of it's parent?
				if(axi == xi - 1)
				{
					//Is adjacent square above it's parent?
					if(ayi == yi - 1)
					{
						//Square is not walkable if there is a block to the left of it's parent(currentNode)
						Node* node = GetNode(xi-1, yi);
						
						if(node != NULL)
						{
							if(node->walkable == false){continue;}
						}
	                    
						//Square is not walkable if there is a block above of it's parent(currentNode)
						node = GetNode(xi, yi-1);

						if(node != NULL)
						{
							if(node->walkable == false){continue;}
						}
					}
					//Is adjacent square below it's parent?
					else if(ayi == yi + 1)
					{
						//Square is not walkable if there is a block to the left of it's parent(currentNode)
						Node* node = GetNode(xi-1, yi);
						
						if(node != NULL)
						{
							if(node->walkable == false){continue;}
						}
	                    
						//Square is not walkable if there is a block below of it's parent(currentNode)
						node = GetNode(xi, yi+1);
						
						if(node != NULL)
						{
							if(node->walkable == false){continue;}
						}
					}
				}
				else //Is adjacent square to the right of it's parent?
				if(axi == xi + 1)
				{
					//Is adjacent square above it's parent?
					if(ayi == yi - 1)
					{
						//Square is not walkable if there is a block to the right of it's parent(currentNode)
						Node* node = GetNode(xi+1, yi);
						
						if(node != NULL)
						{
							if(node->walkable == false){continue;}
						}
	                    
						//Square is not walkable if there is a block above of it's parent(currentNode)
						node = GetNode(xi, yi-1);

						if(node != NULL)
						{
							if(node->walkable == false){continue;}
						}
					}
					//Is adjacent square below it's parent?
					else if(ayi == yi + 1)
					{
						//Square is not walkable if there is a block to the right of it's parent(currentNode)
						Node* node = GetNode(xi+1, yi);
						
						if(node != NULL)
						{
							if(node->walkable == false){continue;}
						}
	                    
						//Square is not walkable if there is a block below of it's parent(currentNode)
						node = GetNode(xi, yi+1);
						
						if(node != NULL)
						{
							if(node->walkable == false){continue;}
						}
					}
				}
				
				//New Code End
	            
				//If it isn't in the open list, add it to the open list;
				vector<Node*>::iterator it3 = find (open_list.begin(), open_list.end(), adjSquare);

				if(it3 == open_list.end())
				{
					//Not already in list.
					//Add to open list
					open_list.push_back(adjSquare);
	                
					//Make the current square the parent of this square.
					//Record F, G and H costs for this square.
					adjSquare->NodeParent = currentNode;

					adjSquare->gcost = currentNode->gcost + adjValue;
					//hcost: distance of horizontal and vertical cells to goal.

					//Get Goal square xi,yi
					int gyi = (int)floor((float)GoalNode->index / HorzNodeCount);
					int gxi = GoalNode->index - (gyi*(int)HorzNodeCount);

					adjSquare->hcost = (float)(10*(abs(axi - gxi) + abs(ayi - gyi)));
					adjSquare->fcost = adjSquare->gcost + adjSquare->hcost;
				}
				else
				{
					//adjSquare Already in open list.
						
					//Is it a shorter path to go from currentNode to
					//adjSquare or shorter to go from existing parent
					//of adjSquare to adjSquare?

					float tmpGCost = adjSquare->gcost;

					//Is adjSquare orthogonal or diagonal to 
					//currentNode?
					int horz_dist = abs(axi - ix);
					int vert_dist = abs(ayi - iy);

					if(horz_dist == 1 && vert_dist == 1)
					{
						//Diagonal
						tmpGCost = currentNode->gcost + 14.0f;
					}
					else if(horz_dist == 1 || vert_dist == 1){
						//Orthogonal
						tmpGCost = currentNode->gcost + 10.0f;
					}
                    
					//Is the new path shorter than the original?
					if(tmpGCost < adjSquare->gcost)
					{
						//Yes, change parent of adjacent node to currentNode
						adjSquare->gcost = tmpGCost;
						adjSquare->fcost = adjSquare->gcost + adjSquare->hcost;
						adjSquare->NodeParent = currentNode;
					}
				}
			}
		}

		//End of for loop(For Adjacent Squares)
		AdjacentSquares.clear();
    
		//Is the open list empty? no path?
		if(open_list.size() == 0)
		{
			//No path available to goal
			break;
		}
	}

	//Generate the path by using node parents
	open_list.clear();
	closed_list.clear();

	//Back track from goal
	//Create path.

	if(GoalNode->NodeParent == NULL){
		//No path available to goal.
		return path;
	}

	Node* path_node = GoalNode;
	path.insert(path.begin() + 0, *path_node);

	while(true)
	{
		if(path_node->NodeParent == NULL){
			//No Parent.
			break;
		}

		path_node = path_node->NodeParent;
		path.insert(path.begin() + 0, *path_node);
	}

	//Reset Node Values for future function calls
	for(UINT i=0; i<GridNodes.size(); i++)
	{
		GridNodes[i].ResetStatus();
	}

	return path;
}