A* Pathfinding Tutorial : Part 3

Welcome to the final part of the A* pathfinding tutorial! In this tutorial we will be implementing the algorithm I described in part 2 in code!

At the start of this tutorial we will be adding some useful properties and methods to the SearchNode and Pathfinder classes that will make writing the FindPath method more simple.

Then to finish off I will post the full code for FindPath method indicating which piece of code relates to which step of the algorithm posted in the last tutorial!

Edited on the 10/12/2011 at 8:00pm

To start with we are going to add some of the properties that I described in the last tutorial to the SearchNode class :

/// <summary>
/// A reference to the node that transfered this node to
/// the open list. This will be used to trace our path back
/// from the goal node to the start node.
/// </summary>
public SearchNode Parent;

/// <summary>
/// Provides an easy way to check if this node
/// is in the open list.
/// </summary>
public bool InOpenList;
/// <summary>
/// Provides an easy way to check if this node
/// is in the closed list.
/// </summary>
public bool InClosedList;

/// <summary>
/// The approximate distance from the start node to the
/// goal node if the path goes through this node. (F)
/// </summary>
public float DistanceToGoal;
/// <summary>
/// Distance traveled from the spawn point. (G)
/// </summary>
public float DistanceTraveled;

All of these properties are described in the last tutorial in detail so if you do not understand the role of one of the fields, I would suggest that you start by having a read of that!

Next we are going to add the open and closed lists to the Pathfinder class as well as the Heuristic function :

// Holds search nodes that are avaliable to search.

private List<SearchNode> openList = new List<SearchNode>();

// Holds the nodes that have already been searched.
private List<SearchNode> closedList = new List<SearchNode>();

/// <summary>
/// Returns an estimate of the distance between two points. (H)
/// </summary>
private float Heuristic(Point point1, Point point2)
{
    return Math.Abs(point1.X - point2.X) +
           Math.Abs(point1.Y - point2.Y);
}

As mentioned in the last tutorial, the heuristic is a function that will give an estimate of the distance between two points, for this tutorial I am using what is called the Manhattan Distance which I beleive gives the best trade off between speed and accuracy. However there are many different heuristic functions out there that will give you a more accurate “shortest path” but will be a slower function!

The next method we are going to add is the ResetSearchNodes method, this method will be responsible for “resetting” the pathfinder so that we can start a new search :

/// <summary>
/// Resets the state of the search nodes.
/// </summary>
private void ResetSearchNodes()
{
    openList.Clear();
    closedList.Clear();

    for (int x = 0; x < levelWidth; x++)
    {
        for (int y = 0; y < levelHeight; y++)
        {
            SearchNode node = searchNodes[x, y];

            if (node == null)
            {
                continue;
            }

            node.InOpenList = false;
            node.InClosedList = false;

            node.DistanceTraveled = float.MaxValue;
            node.DistanceToGoal = float.MaxValue;
        }
    }
}

This method resets the key properties used in the algorithm so that we can use this method again and again and still get valid paths.

Another key method that we are going to add is the FindBestNode method. This method will search through the open list and finds the one with the smallest “Distance to Goal” :

/// <summary>
/// Returns the node with the smallest distance to goal.
/// </summary>
private SearchNode FindBestNode()
{
    SearchNode currentTile = openList[0];

    float smallestDistanceToGoal = float.MaxValue;

    // Find the closest node to the goal.
    for (int i = 0; i < openList.Count; i++)
    {
        if (openList[i].DistanceToGoal < smallestDistanceToGoal)
        {
            currentTile = openList[i];
            smallestDistanceToGoal = currentTile.DistanceToGoal;
        }
    }
    return currentTile;
}

The first thing this method does create a new field called smallestDistanceToGoal and set it to be some massive number. We will use this field to keep a track of the smallest distance to the goal that we have found when looping over all the nodes.

The next thing we do is loop through all of the nodes and compare each nodes DistanceToGoal to the smallestDistanceToGoal, and if the nodes DistanceToGoal is smaller, than this node becomes the best node we have found so far. This type of method is very common in programming.

The last helper method that we are going to add is one that will find the final path; we use the parent pointer of the goal node to trace a path back to the start node:

/// <summary>
/// Use the parent field of the search nodes to trace
/// a path from the end node to the start node.
/// </summary>
private List<Vector2> FindFinalPath(SearchNode startNode, SearchNode endNode)
{
    closedList.Add(endNode);

    SearchNode parentTile = endNode.Parent;

    // Trace back through the nodes using the parent fields
    // to find the best path.
    while (parentTile != startNode)
    {
        closedList.Add(parentTile);
        parentTile = parentTile.Parent;
    }

    List<Vector2> finalPath = new List<Vector2>();

    // Reverse the path and transform into world space.
    for (int i = closedList.Count - 1; i >= 0; i--)
    {
        finalPath.Add(new Vector2(closedList[i].Position.X * 32,
                                  closedList[i].Position.Y * 32));
    }

    return finalPath;
}

And now to add the method that you no doubt have been waiting for! The FindPath method follows the exact same logic as the algorithm posted in the last tutorial:

/// <summary>
/// Finds the optimal path from one point to another.
/// </summary>
public List<Vector2> FindPath(Point startPoint, Point endPoint)
{
    // Only try to find a path if the start and end points are different.
    if (startPoint == endPoint)
    {
        return new List<Vector2>();
    }

    /////////////////////////////////////////////////////////////////////
    // Step 1 : Clear the Open and Closed Lists and reset each node’s F 
    //          and G values in case they are still set from the last 
    //          time we tried to find a path. 
    /////////////////////////////////////////////////////////////////////
    ResetSearchNodes();

    // Store references to the start and end nodes for convenience.
    SearchNode startNode = searchNodes[startPoint.X, startPoint.Y];
    SearchNode endNode = searchNodes[endPoint.X, endPoint.Y];

    /////////////////////////////////////////////////////////////////////
    // Step 2 : Set the start node’s G value to 0 and its F value to the 
    //          estimated distance between the start node and goal node 
    //          (this is where our H function comes in) and add it to the 
    //          Open List. 
    /////////////////////////////////////////////////////////////////////
    startNode.InOpenList = true;

    startNode.DistanceToGoal = Heuristic(startPoint, endPoint);
    startNode.DistanceTraveled = 0;

    openList.Add(startNode);

    /////////////////////////////////////////////////////////////////////
    // Setp 3 : While there are still nodes to look at in the Open list : 
    /////////////////////////////////////////////////////////////////////
    while (openList.Count > 0)
    {
        /////////////////////////////////////////////////////////////////
        // a) : Loop through the Open List and find the node that 
        //      has the smallest F value.
        /////////////////////////////////////////////////////////////////
        SearchNode currentNode = FindBestNode();

        /////////////////////////////////////////////////////////////////
        // b) : If the Open List empty or no node can be found, 
        //      no path can be found so the algorithm terminates.
        /////////////////////////////////////////////////////////////////
        if (currentNode == null)
        {
            break;
        }

        /////////////////////////////////////////////////////////////////
        // c) : If the Active Node is the goal node, we will 
        //      find and return the final path.
        /////////////////////////////////////////////////////////////////
        if (currentNode == endNode)
        {
            // Trace our path back to the start.
            return FindFinalPath(startNode, endNode);
        }

        /////////////////////////////////////////////////////////////////
        // d) : Else, for each of the Active Node’s neighbours :
        /////////////////////////////////////////////////////////////////
        for (int i = 0; i < currentNode.Neighbors.Length; i++)
        {
            SearchNode neighbor = currentNode.Neighbors[i];

            //////////////////////////////////////////////////
            // i) : Make sure that the neighbouring node can 
            //      be walked across. 
            //////////////////////////////////////////////////
            if (neighbor == null || neighbor.Walkable == false)
            {
                continue;
            }

            //////////////////////////////////////////////////
            // ii) Calculate a new G value for the neighbouring node.
            //////////////////////////////////////////////////
            float distanceTraveled = currentNode.DistanceTraveled + 1;

            // An estimate of the distance from this node to the end node.
            float heuristic = Heuristic(neighbor.Position, endPoint);

            //////////////////////////////////////////////////
            // iii) If the neighbouring node is not in either the Open 
            //      List or the Closed List : 
            //////////////////////////////////////////////////
            if (neighbor.InOpenList == false && neighbor.InClosedList == false)
            {
                // (1) Set the neighbouring node’s G value to the G value 
                //     we just calculated.
                neighbor.DistanceTraveled = distanceTraveled;
                // (2) Set the neighbouring node’s F value to the new G value + 
                //     the estimated distance between the neighbouring node and
                //     goal node.
                neighbor.DistanceToGoal = distanceTraveled + heuristic;
                // (3) Set the neighbouring node’s Parent property to point at the Active 
                //     Node.
                neighbor.Parent = currentNode;
                // (4) Add the neighbouring node to the Open List.
                neighbor.InOpenList = true;
                openList.Add(neighbor);
            }
            //////////////////////////////////////////////////
            // iv) Else if the neighbouring node is in either the Open 
            //     List or the Closed List :
            //////////////////////////////////////////////////
            else if (neighbor.InOpenList || neighbor.InClosedList)
            {
                // (1) If our new G value is less than the neighbouring 
                //     node’s G value, we basically do exactly the same 
                //     steps as if the nodes are not in the Open and 
                //     Closed Lists except we do not need to add this node 
                //     the Open List again.
                if (neighbor.DistanceTraveled > distanceTraveled)
                {
                    neighbor.DistanceTraveled = distanceTraveled;
                    neighbor.DistanceToGoal = distanceTraveled + heuristic;

                    neighbor.Parent = currentNode;
                }
            }
        }

        /////////////////////////////////////////////////////////////////
        // e) Remove the Active Node from the Open List and add it to the 
        //    Closed List
        /////////////////////////////////////////////////////////////////
        openList.Remove(currentNode);
        currentNode.InClosedList = true;
    }

    // No path could be found.
    return new List<Vector2>();
}

And that is all there is to A-Star! The last thing we are going to do in this tutorial is a quick example of how to use the FindPath method!

The first thing you need to do is go to Game1.cs and at the top of the class add a field for the pathfinder :

Pathfinder pathfinder;

Next, we go to the Initialize method and add the following:

pathfinder = new Pathfinder(map);
List<Vector2> path = pathfinder.FindPath(new Point(0, 0), new Point(9, 9));

foreach (Vector2 point in path)
{
    System.Diagnostics.Debug.WriteLine(point);
}

After we initialize the pathfinder, we use it for the first time! We find a path from the top left corner to the bottom right corner and we output the points that make up the path to the output window!

And there you have, you now have the means to make your enemies that little bit smatter!

Thank you for reading and I hope you have enjoyed this tutorial series!

Here if the code for this tutorial.