Saturday 10 December 2011

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!

22 comments:

  1. So maybe im wrong, however this part:

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

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

    shouldnt the:

    private List openList = new List();
    private List closedList = new List();

    be:
    private List openList = new List();
    private List closedList = new List();

    ReplyDelete
  2. Oops >.> I meant shouldnt it be:

    private List openList = new List();
    private List closedList = new List();

    ReplyDelete
  3. 0.o now im just looking stupid >.> (how come when I take the spaces out it completely omits the triangle brackets and everything inside them?)

    new List< SearchNode >();

    ive typed it out this time lets see if this works.

    ReplyDelete
  4. Lol Im commenting again. So it turns out that my issues in the end was not my grasp of the A* algorithm, or my implementation of it. It was in the code making my object move. I feel so stupid now.

    But thanks you are awesome. And have really helped me out.

    ReplyDelete
  5. You are right, thanks for pointing that out!

    ReplyDelete
  6. Thank you for these tutorials and please keep them coming.

    ReplyDelete
  7. So, whats your next tutorial going to be 0.o?

    ReplyDelete
  8. Haha who says there will be a next... ;)

    ReplyDelete
  9. Thank you for the toturials
    I what to add the new type of enemy
    How can I do that
    Thank you very much ^^

    ReplyDelete
  10. Maybe for another game :D
    These tutorials are great and help me a lot
    Thank you

    ReplyDelete
  11. Hey, I want to say thanks so much. I found that beginners A* tutorial a week ago or so and managed to get something going but it had some bugs I couldn't track down. Then I came across your tutorial here and was able to perfect it. Thanks a lot for putting this up, you did a really great job. I hope you do more tutorials.

    ReplyDelete
  12. Hey, I'm getting a System.NullReferenceException at the startNode.InOpenList = true, in the FindPath method... have I done something wrong along the line?

    ReplyDelete
    Replies
    1. Were you ever able to fix this? I'm getting the same problem as well.

      Delete
    2. Nevermind just figured it out. When you first initialize your starting and end points, they need to be valid walkable points i.e. tile = 0 where you place the points. Otherwise, the code will return null since you start off on an invalid tile i.e. tile = 1.

      Delete
  13. Hey I'm wondering if there is any way to import a map that would put less burden on the processor.

    http://peeba.gfxile.net/wp-content/uploads/2008/07/ruins06.jpg

    Is the link the image I am using but when i create the layout array be 32 by 21 it slows the game down heavily. Are all A* algorithms going to suffer with that many tiles?

    Your tutorial was amazing and I very much enjoyed reading it, I am hoping that it will ultimately work perfect for me. Thank you

    ReplyDelete
  14. Due to this being a tutorial, I tried to make the algorithm as simple an easy to follow as possible, this means it isn't very optimized!

    You may want to look into a few of these things to speed up your path finding:

    http://roy-t.nl/index.php/2009/07/07/new-version-a-pathfinding-in-3d/

    http://harablog.wordpress.com/ (In particular the hierarchical stuff)

    ReplyDelete
  15. Instead of returning a List of points, you could return a Stack of points, that would save you having to do any kind of reversing

    ReplyDelete
  16. If I wanted to assign the pathfinding code to the enemy in the tower defence tutorial you created how would I go about doing this?

    ReplyDelete
  17. Wow you did an awesome job on the pathfinding. I just had one question for you, if I wanted to update the path during runtime, what would be the most efficient way to do this?

    ReplyDelete
  18. Cheers, this worked perfectly for me and helped me so much.
    Thankyou

    ReplyDelete
  19. This comment has been removed by the author.

    ReplyDelete
  20. Thanks you very much!
    I searched a long time a tutorial like yours!

    ReplyDelete