Pathfinding is a fundamental tool commonly used to move characters around a map intelligently. Here I will be going over the A* algorithm written in C++.
It’s important to visualize how the pathfinding algorithms search for the most efficient path. Here is a neat demo using a JavaScript library called Pathfinding.js. By playing with Pathfinder.js, you should notice that the algorithm first tries to go straight for the end point and only when it hits a blocked node will it look for nodes that follow a less direct path.
The Algorithm
In a nutshell, the A* algorithm uses a graph of nodes that have a weight associated to them used to weigh paths from the start node to the end node.
What weight represents is arbitrary, since A* can be applied to different scenarios. For navigating through a map of squares for example, this value could be based on distance and steps taken. The algorithm processes neighboring nodes, adding all neighbors to an “open list” and then moving on to the lowest cost node in that list.
Given the open list begins with the start node (opposed to be empty), the main loop of the algorithm looks like this:
- Get next node from open list (now known as current node)
- Skip node if in closed list or out of bounds
- Add current node to closed list
- Remove current node from open list
- Process neighboring nodes (North, West, South, East)
- Set current node as parent of neighbor if weight is less
In all, the algorithm uses three lists:
- Open List: Keeps track of which nodes to search next and is ordered by weight
- Closed List: Al nodes that have already been searched
- Path: Linked list that connects the end node to the start node
To clarify on the path, there are actually multiple paths that exist because less efficient paths may be found before the most efficient path. The end node will have the final path, and that is all we care about.
Implementation
Next we’ll go over the structures and functions involved with this algorithm. One core structure is the node.
Node Structure
As mentioned before, this is implemented in C++ and the graph is comprised of nodes.
Here is the Node structure with only its members:
struct Node {
int x;
int y;
int index = 0;
int distance;
bool blocked = 0;
int weight = 0;
Node *parent = NULL;
};I use indices to keep track of nodes. This includes determining which node is the start and end, as well as which nodes have already been processed (closed list) and which nodes should be processed next (open list).
Note: The index struct member is there for convenience, so a coordinate to index lookup doesn’t need to be run every time.
The path list mentioned before is a linked list created through the parent struct member, which points to another node.
Grid and List Structures
Nodes are stored as pointers in a vector. The open list is implemented as a multiset with the comparison function, NodeComp, so sorting is automatically done. Lastly, the closed list is implemented as a vector of ints that stores the indexes.
typedef std::multiset<Node*, NodeComp> NodeList; // So we don't need to type out the full definition everywhere NodeList open_list; // Node pointers to process std::vector<int> closed_list; // Node indices already processed std::vector<Node*> grid; // All nodes
Main Loop
To recap with pseudo-code, there is an outer loop that does some workflow handling and maintenance of the open and closed lists. Contained by the outer loop is a second (inner) loop that steps through each neighboring node and updates the weight if it does less as well as well as add the neighbor to the open list conditionally.
Outer Loop
I’m going to cover each loop separately, so we’ll start off with the outer loop.
// 1. Add start node (pointer) to open list
open_list.insert(grid.at(start_node)); // Begin with start node
while(!open_list.empty()) { // Loop through open_list until exhausted
current_it = open_list.begin();
current = *current_it; // current_node
// 2. Bail out of loop if destination is reached
if(end_node == current->index) {
break;
}
// 3. Add index of current node to closed list
closed_list.push_back(current->index);
// 4. Remove pointer entry of current node to open list
open_list.erase(current_it);
// 5. For each neighboring node that exists in grid, set parent and weight conditionally
...
}Step 2 is so we don’t continue looping unnecessarily. Since open_list is a multiset that orders by weight, once the destination is reached, we know the most efficient path has been found.
Step 3 and step 4 move the node from the open list to the closed list. This is to prevent an infinite loop from within the inner loop when neighbors are added to the open list. Basically, a node is a neighbor to many nodes, and we only want to add it to the open list once. So to prevent it from being added multiple times, the closed list is used.
Inner Loop
...
while(!open_list.empty()) { // Loop through open_list until exhausted
...
for(std::vector<Vec2i>::iterator dit = directions.begin(); dit != directions.end(); ++dit) {
// 1. Move onto next neighbor if current neighbor is outside of gid dimensions
if(current->x + (*dit).x >= MAP_WIDTH || current->x + (*dit).x < 0) {
continue;
}
if(current->y + (*dit).y >= MAP_HEIGHT || current->y + (*dit).y < 0) {
continue;
}
int index = coords_to_index(current->x + (*dit).x, current->y + (*dit).y);
// 2. Move to next neighbor if node is in closed list
std::vector<int>::iterator cit = std::find(closed_list.begin(), closed_list.end(), index);
if(cit != closed_list.end()) {
continue;
}
Node *node = grid.at(index);
// 3. Move on to next neighbor if node blocked (unwalkable)
if(node->blocked) {
closed_list.push_back(node->index);
continue;
}
NodeList::iterator it = open_list.find(node);
int total_weight = current->weight + 10; // Used to weed out most efficient path
// 4. Process neighbor, set weight/parent
if(it == open_list.end()) {
node->parent = current;
node->weight = total_weight;
open_list.insert(node);
} else if(total_weight < node->weight) {
node->parent = current;
node->weight = total_weight;
}
}
}
...Step 1 – 3 weed out invalid neighbors.
Step 4 is where the neighbor actually gets processed. Here there are a few different things that are happening.
The first time a neighbor node is processed here, the weight is set and the parent is set to the current_node assigned from the outer loop. If the neighbor node is processed again (from another node), the weight and parent will only be set of the weight is less then what is already set
Lastly, if the neighbor node has not already been added to the open list, it is added.
The Final Path
Once the main algorithm loop is complete, we will have all the data we need to identify the final path. This is done through the parent class member of the nodes that act as a linked list.
In this implementation, the final path is stored in a vector of ints (indexes) by traversing the linked list.
std::vector<int> path_indices;
while(current != NULL) {
path_indices.push_back(current->index);
current = current->parent;
}current is the last node pulled from the open_list . This will be the destination if it could be reached, otherwise it will be the closest node to the destination.
The linked list goes backwards, from destination node to start node and was all made possible by the algorithm comparing weights and setting the parent property to create the linked list.
Proof of Concept Example
Here is a working example of the algorithm covered in this post.
Conclusion
Welp, there you have it, a C++ implementation of the A* algorithm. Moving forward I’m going to try to apply this to a JavaScript-based mini-project I’ve been working on and use it for both avoiding obstacles and plotting the correct sequence of actions.
Eventually I will look into Jump Point Search (JPS) which is an extension of the A* algorithm.