# Pathfinding II: A* and Heuristics

In our previous installment, we discussed the data structures that we will use to represent the graph which we will use for pathfinding on the terrain, as well as the initial pre-processing that was necessary to populate that graph with the information that our pathfinding algorithm will make use of. Now, we are ready to actually implement our pathfinding algorithm. We’ll be using A*, probably the most commonly used graph search algorithm for pathfinding.

A* is one of the most commonly used pathfinding algorithms in games because it is fast, flexible, and relatively simple to implement. A* was originally a refinement of Dijkstra’s graph search algorithm. Dijkstra’s algorithm is guaranteed to determine the shortest path between any two nodes in a directed graph, however, because Dijkstra’s algorithm only takes into account the cost of reaching an intermediate node from the start node, it tends to consider many nodes that are not on the optimal path. An alternative to Dijkstra’s algorithm is Greedy Best-First search. Best-First uses a heuristic function to estimate the cost of reaching the goal from a given intermediate node, without reference to the cost of reaching the current node from the start node. This means that Best-First tends to consider far fewer nodes than Dijkstra, but is not guaranteed to produce the shortest path in a graph which includes obstacles that are not predicted by the heuristic.

A* blends these two approaches, by using a cost function (**f(x)**) to evaluate
each node based on both the cost from the start node (**g(x)**) and the estimated
cost to the goal (**h(x)**). This allows A* to both find the optimum shortest
path, while considering fewer nodes than pure Dijkstra’s algorithm. The number of
intermediate nodes expanded by A* is somewhat dependent on the characteristics of the heuristic
function used. There are generally three cases of heuristics that can be used to control
A*, which result in different performance characteristics:

- When h(x) underestimates the true cost of reaching the goal from the current node, A* will expand more nodes, but is guaranteed to find the shortest path.
- When h(x) is exactly the true cost of reaching the goal, A* will only expand nodes along the shortest path, meaning that it runs very fast and produces the optimal path.
- When h(x) overestimates the true cost of reaching the goal from the current node, A* will expand fewer intermediate nodes. Depending on how much h(x) underestimates the true cost, this may result in paths that are not the true shortest path; however, this does allow the algorithm to complete more quickly.

For games, we will generally use heuristics of the third class. It is important that we generate good paths when doing pathfinding for our units, but it is generally not necessary that they be mathematically perfect; they just need to look good enough, and the speed savings are very important when we are trying to cram all of our rendering and update code into just a few tens of milliseconds, in order to hit 30-60 frames per second.

A* uses two sets to keep track of the nodes that it is operating on. The first set is
the closed set, which contains all of the nodes that A* has previously considered; this is
sometimes called the interior of the search. The other set is the open set, which contains
those nodes which are adjacent to nodes in the closed set, but which have not yet been processed
by the A* algorithm. The open set is generally sorted by the calculated cost of the node
(**f(x)**), so that the algorithm can easily select the most promising new node to
consider. Because of this, we usually consider the open list to be a priority queue.
The particular implementation of this priority queue has a large impact on the speed of A*; for
best performance, we need to have a data structure that supports fast membership checks (is a
node in the queue?), fast removal of the best element in the queue, and fast insertions into the
queue. Amit Patel provides a good overview of the pros and cons of different data
structures for the priority queue on his A*
page; I will be using a priority queue derived from Blue
Raja’s Priority Queue class, which is essentially a binary heap. For our closed
set, the primary operations that we will perform are insertions and membership tests, which makes
the .Net HashSet<T> class a good choice.

### A* Algorithm

We will implement the A* algorithm in the GetPath function of our Terrain class. This function takes the map coordinates of the start and goal map tiles, and returns the list of MapTiles that makes up the path between them. If no path is found, the function will return an empty list.

Our first step is to check that both tile coordinates are valid and that the two tiles are connected as part of the same tile set. If this is not the case, we can exit the function early and save the calculation costs; otherwise, the algorithm would have to expand all of the nodes in the tile set containing the start node before determining that there was no valid path.

public List<MapTile> GetPath(Point start, Point goal) { var startTile = GetTile(start); var goalTile = GetTile(goal); // check that the start and goal positions are valid, and are not the same if (!Within(start) || !Within(goal) || start == goal || startTile == null || goalTile == null) { return new List<MapTile>(); } // Check that start and goal are walkable and that a path can exist between them if (startTile.Walkable && goalTile.Walkable && startTile.Set != goalTile.Set) { return new List<MapTile>(); }

Next, we need to reset the F and G costs of each tile in the map and create our open and closed sets. Then, we calculate the F and G costs for the start tile, and add the start tile to the open list. We calculate the H cost of the tile using the h(start, goal) heuristic function, which is a delegate function that we will cover more fully later when we discuss heuristic functions.

// reset costs foreach (var t in _tiles) { t.F = t.G = float.MaxValue; } var open = new PriorityQueue<MapTile>(_tiles.Length); var closed = new HashSet<MapTile>(); startTile.G = 0; startTile.F = h(start, goal); open.Enqueue(startTile, startTile.F);

Next, we enter the main loop of the A* algorithm. While the open set is not empty, and we have not reached the goal, we remove the best tile from the open set and add it to the closed set. Then we consider each valid connection to a neighboring tile for the current tile. We calculate the G cost of the neighbor by adding the cost of traversing the edge to the neighbor to the current node’s G cost. If the neighbor is already contained in the open set, but the G cost we have calculated is lower than its current G cost, we remove the neighbor from the open set, so that we can re-add it with the lower cost. Likewise, if the closed set contains the neighbor, and the calculated G cost is lower than its current cost, we remove the neighbor from the closed set, which will allow us to reconsider it. Now, if the neighbor is not included in the open or closed sets, we assign its newly calculated G cost, then calculate the neighbor’s F cost, using the sum of the G cost and the heuristic cost to reach the goal from this neighbor. We then add the neighbor to the open set, using this new F cost as the key for the priority queue. We also set the parent pointer for the neighbor to the current node, to allow us to reconstruct the path when we are finished.

MapTile current = null; while (open.Any() && current != goalTile) { current = open.Dequeue(); closed.Add(current); for (var i = 0; i < 8; i++) { var edge = current.Edges[i]; if (edge == null) { continue; } var neighbor = edge.Node2; var cost = current.G + edge.Cost; if (open.Contains(neighbor) && cost < neighbor.G) { open.Remove(neighbor); } if (closed.Contains(neighbor) && cost < neighbor.G) { closed.Remove(neighbor); } if (!open.Contains(neighbor) && !closed.Contains(neighbor)) { neighbor.G = cost; var f = cost + h(neighbor.MapPosition, goal); open.Enqueue(neighbor, f); neighbor.Parent = current; } } }

After we have calculated the path from the start to the goal, we construct the list of MapTiles on the path by following the train of Parent pointers from the goal tile back to the start tile. This gives us a path from the goal to the start, so we reverse the list, in order to obtain the path from the start to the goal, and then return the path.

System.Diagnostics.Debug.Assert(current == goalTile); var path = new List<MapTile>(); while (current != startTile) { path.Add(current); current = current.Parent; } path.Reverse(); return path; }

### Heuristics

As mentioned previously, the selection of a heuristic to use in A* can make a big difference in the performance of the algorithm. One important consideration in selecting a heuristic is to match the heuristic to the to the degrees of freedom of movement in your game map. For pathfinding, we’ll often be using some sort of 2D grid for our map representation, so I have created a class containing some of the more common distance heuristics used in these kinds of grids. This class, Heuristics.cs, also contains a definition for a delegate function, to enable us to easily switch between any of these heuristics, or even an arbitrary custom heuristic easily in our Terrain class.

public class Heuristics { public delegate float Distance(Point start, Point goal); public static float ManhattanDistance(Point start, Point goal) { var dx = Math.Abs(start.X - goal.X); var dy = Math.Abs(start.Y - goal.Y); var h = dx + dy; return h; } public static float DiagonalDistance(Point start, Point goal) { var dx = Math.Abs(start.X - goal.X); var dy = Math.Abs(start.Y - goal.Y); var h = Math.Max(dx, dy); return h; } public static float DiagonalDistance2(Point start, Point goal) { var dx = Math.Abs(start.X - goal.X); var dy = Math.Abs(start.Y - goal.Y); var h = (dx + dy) + (MathF.Sqrt2 - 2) * Math.Min(dx, dy); return h; } public static float EuclideanDistance(Point start, Point goal) { var dx = (goal.X - start.X); var dy = (goal.Y - start.Y); return MathF.Sqrt(dx * dx + dy * dy); } public static float HexDistance(Point start, Point goal) { var dx = start.X - goal.X; var dy = start.Y - goal.Y; var dz = dx - dy; var h = Math.Max(Math.Abs(dx), Math.Max(Math.Abs(dy), Math.Abs(dz))); return h; } }

These heuristics are tailored to different kinds of connectivity in our terrain map grid:

**ManhattanDistance**– This heuristic is suitable for maps or units where it is only legal to move along the ranks or files of the map, like a rook in chess, or if you were navigating city blocks.**DiagonalDistance**– This heuristic is suitable for maps or units where it is legal to also move along the diagonals, like the king in chess. This heuristic considers the cost of moving along a diagonal as the same as moving along a rank or file.**DiagonalDistance2**– This is similar to the previous heuristic, however, it computes the cost of traversing a diagonal as slightly more expensive than a rank or file, which is more appropriate if the actual distance covered is included in the cost of traversing an edge, as it is in real life.**EuclideanDistance**– This is the simple straigh-line, as the crow flies distance, computed using the Pythagorean theorem. Suitable for maps and units in which any degree of movement is valid.**HexDistance**– This is an adaptation of Manhattan distance tailored to maps in which the tiles are hexagonal in shape, and there are thus six equal directions of movement. This relies on a particular choice of coordinate system for the hexagonal tiles, in which one of the main 2D axes is aligned with a diagonal of the hex (i.e. not valid for the often-seen offset coordinate system). An example of a coordinate system for a hex grid using this heuristic is below.

All of these heuristics calculate the cost of traversing an edge using a uniform cost of 1. In our example, where the edge costs are determined by the vertical slope between tiles, this will always be an overestimate.

Next time, we’ll put these pieces together and develop a full example that puts our map representation and pathfinding algorithm into use to move a very simple unit around on our terrain. To do this, we’ll need to revisit our picking quadtree, so that we can determine which tile on the map we are selecting, as well as its world position.