No longer updated: new and updated blog (including old content) at http://roy-t.nl

Archive for http://roy-t.nl, please update your bookmarks

Implementing A* path finding in C# (and XNA): source-code (can we cut the corner?)

Posted by Roy Triesscheijn on Tuesday 24 February, 2009

Note: since the 1st of December 2009, this blog was moved to www.roy-t.nl, all content here should be considered archived, new content, updates, comments, etc… will no longer be released here. A fresh copy of this article exists at the new website, here you can post comments and ask questions which I will try to answer asap. I also wrote a new version of my A* implementation here: http://roy-t.nl/index.php/2009/07/07/new-version-a-pathfinding-in-3d/ which is greatly superior and faster than this version.

Sincerely,

Roy Triesscheijn

Update: there is a new fully 2D and 3D version of this article now available that fixes many issues some of you where having, and having a much faster and more readable codebase, get it here!

Please excuse my grammar in my last post, I was quite tired and well, let’s not talk about it anymore 🙂 .

Anyway, as I’ve said I’ve been working on an A* path finding algorithm in C# for one of my XNA projects. I’ve cleaned up the garbage and refactored the algorithm into one nice .cs file (2classes)

Today I will give a short explanation of the A* path finding algorithm and my implementation of it, specifically the extra point about cutting corners. You can download the source code at the end of this article. The source-code can be used in any C# project, and doesn’t use specific XNA classes. (All it really uses are Point’s, generic Lists and a couple of ints and bools).

Note: For more in depth information check out the following links

http://www.policyalmanac.org/games/aStarTutorial.htm

http://en.wikipedia.org/wiki/A*_search_algorithm

http://en.wikipedia.org/wiki/Dijkstra’s_algorithm (A* is an extension on Dijkstra’s algorithm. (You could view Dijkstra’s algorithm as A* where H is always 0, more about H later))

Note 2: I will use the terms square and node interchangeable in this article because in my A* implementation my nodes are square, however you can use A* for any kind of shapes for the node, and my code is easily adjustable to accommodate that.

A* generally works the following way  (source: Patrick Lester from policyalmanac.org )

1) Add the starting square (or node) to the open list.

2) Repeat the following:

a) Look for the lowest F cost square on the open list. We refer to this as the current square.

b) Switch it to the closed list.

c) For each of the 8 squares adjacent to this current square …

– If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following.

– If it isn’t on the open list, add it to the open list. Make the current square the parent of this square. Record the F, G, and H costs of the square.

– If it is on the open list already, check to see if this path to that square is better, using G cost as the measure. A lower G cost means that this is a better path. If so, change the parent of the square to the current square, and recalculate the G and F scores of the square. If you are keeping your open list sorted by F score, you may need to resort the list to account for the change.

d) Stop when you:

Add the target square to the closed list, in which case the path has been found (see note below), or

Fail to find the target square, and the open list is empty. In this case, there is no path.

3) Save the path. Working backwards from the target square, go from each square to its parent square until you reach the starting square. That is your path.

The costs F which is  G+H might not be evident at first. But is calculated the following way:

G is the movement cost from the start point to that square, and H is the estimated cost from there to the end square.

G is calculated as  TargetSquare.G =  parent.G + 10  or + 14 if the square is diagonal from the parent. (That’s because the square root of 2 is 1.4 and we try to keep the numbers integers here)

H is calculated (in my implementation) as the Manhattan distance from the target to the end.  Which is something like (Math.Abs(G.x – H.x) + Math.Abs(G.y – H.y) ) * 10.

The algorithm keeps checking of squares that are on the open list can be reached cheaper from the current square.

Corner Cutting.

Now about the corner cutting: my implementation adds one new situation before the first point of 2.C.

-If the node is diagonal from the current node check if we can cut the corners of the 2 others nodes we will cross. If so this square is walkable, else it isn’t.

A picture might explain better why this is important:

Art

square A is our current square and we are considering if we can walk 2 square B. Square B’s walkable attribute is set to true, so we might think that we can continue to (now point 2) in c, adding it to the open list etc… However if the object that is going to walk the path is going to get to B, a part of it will be at with red indicated areas of squares C and D.  Imagine square D represents a house, that exactly fills the square, this way our object is going to traverse trough a house! However if squares C and D represents a well, centred in a square filled with grass, we can easily cut the corner to get to B.

The rest of my algorithm isn’t any different than general A*. The code is well commented and all the pitfalls are avoided as much as possible. However why the code works might not be evident if you haven’t studied A* first. I suggest you check the link to policyalmanac.org at the top of this article for a very detailed explanation of A*

Speed.

A* is a very fast algorithm, I’ve tested it on a grid with 64 nodes and the general search time was under 1ms.

Download.

You can download the source code via this link: (My Skydrive)

Don’t be afraid to post your optimizations, notes, questions or other comments here!

kick it on GameDevKicks.com

Advertisements

32 Responses to “Implementing A* path finding in C# (and XNA): source-code (can we cut the corner?)”

  1. […] Implementing A* path finding in C# (and XNA): source-code (can we cut the corner?) […]

  2. Rod said

    Hi, that’s a good example. But how do you find out, when the target node is not reachable (blocked by walls or something).

  3. Rod said

    Hi Royalexander.
    And what if I want to prevent diagonally paths in general? No matter if they are possible or not.

    Thanks.

  4. Rod said

    Ok never mind, I didn’t see the canCutCorners property in the Node :). Works fine!

  5. Rod said

    Hi again. I’ve got a problem with having multiple objects who wish to find their own path.
    When one of them calculates a new path, the paths of all the others are getting “broken”. I think it’s because the SearchPath method only returns references of the nodes.
    How would you resolve this problem? I want to avoid having the complete node list and a new PathFinder instance for each of the objects who want to search for the way on the map.
    Thanks!

  6. Hey again Rod,

    I’ll look into it as soon as possible, it’s true that SearchPath returns a reference to the nodes, but it returns a new list so it shouldn’t be a problem as long as the only thing you do with the nodes is reading them and deleting them from the list.

    However there must be something going-on. I’ll post here asap with a sollution.

  7. Rod said

    As soon as you run SearchPath a second time, all the references on existing waypoint lists will be changed too. So the lists still have the correct length but the values aren’t correct anymore and maybe some nodes will be just empty (HScore, GScore are 0 and Parent is -1 and so on).
    I’ve thought about a possible solution and tried to deep copy the waypoint list as soon as it was returned by the SearchPath method. So the references aren’t set anymore.
    I let the Node class inherit from the ICloneable interface and added this method to it:

    #region ICloneable Member
    public object Clone()
    {
    Node node = new Node();
    node.CanCutCorners = CanCutCorners;
    node.GScore = GScore;
    node.HScore = HScore;
    node.IsWalkable = IsWalkable;
    node.Parent = Parent;
    return node;
    }
    #endregion

    And at the point where I want the waypoint list to be used I added the following:

    List waypointList = new List();
    List waypointListCloned = new List();

    waypointList = Pathfinder.SearchPath(startPoint, startPoint); // thats the referenced list
    foreach (Node node in waypointList)
    {
    waypointListCloned.Add((Node)node.Clone());
    }
    myObject.WaypointList = waypointListCloned; // thats the cloned list

    That works but maybe there is a better solution. Cloneing isn’t very fast I think, the performance depends on how long the waypoint list will be. But maybe the performance is good enough, haven’t tested it.

    I also thought about implementing the cloneing directly into the Pathfinder class but maybe someone likes to search only for one path at great speed. Perhaps it’s a option to implement a overloaded SearchPath method where you can choose if you want to have a list with oder without deep copied nodes.

    Greetings,
    Rod

    • Hey Rod, arg this is going to be my 3rd attempt to answer your question *having some small browser or wp issues somehow*

      I’m afraid I dont have any better sollution that the one you came up with at the moment :). Thanks for your sollution and input!

  8. Rod said

    I think this solution fits for my purposes and it’s still quite fast. Thanks for implementing in c# at this point, it’s way better than the other versions I’ve tested.

    But I think I’ve found a little bug. I’ve tried to locate it but I wasn’t successful until now.
    When you build a “row” of unwalkable nodes between the start point and the end point, with one node shifted by one sqare the path is cutting the corners even if all nodes on the map are CanCutCorners = false.
    Here an code example for a console application, the pathfinder class is untouched:

    private static PathFinder _pathFinder;

    static void Main(string[] args)
    {
    int mapSize = 10;
    _pathFinder = new PathFinder(new Node[mapSize, mapSize]);

    // Initialize nodes
    for (int i = 0; i < mapSize; i++)
    {
    for (int j = 0; j < mapSize; j++)
    {
    _pathFinder.Nodes[i, j] = new Node(true, false);
    }
    }

    // Set “walls”
    _pathFinder.Nodes[3, 0].IsWalkable = false;
    _pathFinder.Nodes[3, 1].IsWalkable = false;
    _pathFinder.Nodes[3, 2].IsWalkable = false;
    _pathFinder.Nodes[2, 3].IsWalkable = false;
    _pathFinder.Nodes[3, 4].IsWalkable = false;
    _pathFinder.Nodes[3, 5].IsWalkable = false;
    _pathFinder.Nodes[3, 6].IsWalkable = false;
    _pathFinder.Nodes[3, 7].IsWalkable = false;
    _pathFinder.Nodes[3, 8].IsWalkable = false;
    _pathFinder.Nodes[3, 9].IsWalkable = false;

    List nodeList = _pathFinder.SearchPath(0, 2, 7, 2);
    Console.ReadLine();
    }
    }

    You will see, that the path is cutting the corner when “walking” from 2,4 to 3,3.
    Maybe you have a idea?

    Greetings, Rod.

  9. Hey Rod,

    Damn seems a little bug is still in there somewhere. I will look into it, and maybe update the code if I find a better solution, but I can’t say how fast I’ll get around to it.

  10. Rod said

    Hi again 😉
    I’ve got it. Sorry for spamming 😀

    The bug was to set the GScore in SetScore method of an node where CanCutCorners is false to a value of 10000. This makes the node cost very much, but it is still walkable. And when every other possible node is not reachable it takes this way, even if it costs very much.

    One possible solution is to replace your if block in your SetScore method with this:

    if (Nodes[sX, tY].CanCutCorners == false || Nodes[tX, sY].CanCutCorners == false)
    {
    gScore = 10000; // can’t cut corners so make that way very costly
    Nodes[tX, tY].Parent = new Point(-1, -1); // set the parent to -1 because this node isn’t reachable
    }

    and edit this line in your Handle Neighbours method:
    openList.Add(newPoint);

    to this:
    if (Nodes[newPoint.X, newPoint.Y].Parent != new Point(-1, -1))
    {
    openList.Add(newPoint);
    }

    I think to set the gscore to 10000 isn’t necessary anymore, parent to -1 has to be enough.

    Greets, Rod.

  11. Artimus said

    First of all thanks for doing this.

    I was just wondering if someone could do a quick implementation of this. I’m having a hard time figuring out how to actually use it. I’m just playing around with a simple XNA app to learn about FSM’s and some basic AI concepts. It has an entity that I want to navigate through a 2d tile map with path and wall tiles. I first tried to do some basic collision detection with some even more basic steering behavior but I found it’s not accurate enough for what I want, so I started looking for Path finding to add to my steering and came across this which looks perfect for what I want to do.

    Basically I am wondering how it works, not really the algorithm or a*, but how I initialize the nodes or get it started. Do I need to make a Node[,] array first by adding the coords from all my tiles.center point? I guess in simpler terms I am wondering how I get my map tile info into the pathfinder to find the path. and in the Node[,] array what two values are you storing ([value, value]). Sorry for my noobish nature I am still quite new to programming in general so reading and understanding other peoples code isn’t the easiest thing for me yet :). Thank you in advance for any help as well as thank you for sharing your knowledge.

    Thanks,

    Artimus

    • Hi Artimus, sorry for the late reply but I’ve got a snippet here on how to use the class:


      Node[,] nodes = new Node[8,8];
      for(int x = 0; x <= nodes.GetUpperBound(0); x++)
      {
      for(int y = 0; y <= nodes.GetUpperBound(1); y++)
      {
      nodes[x, y] = new Node(true, true);
      }
      }
      finder = new PathFinder(nodes);
      finder.SearchPath(0, 0, 4, 4);

      Basically you just create a node array, and when you create the individual nodes you set them walkable/cancutcorners or not (you can also change them later via node.isWalkable etc..) you then construct a new Pathfinder(nodes) and call finder.SearchPath(startx, starty, endX, endY), this will return a nice list with the best path from start to finish.

      Good luck

  12. darthuvius said

    id like to see a simple implementation also, maybe just have a sprite move across a grid? maybe also have a wall of unwalkable nodes in the middle of grid.

  13. GeorgeL said

    How do I set the target and starting point?


    public void UpdateEnemy()
    {
    Node[,] nodes = new Node[80,45];
    for (int x = 0; x < 80; x++)
    {
    for (int y = 0; y < 45; y++)
    {
    int valA = array[x, y];
    if (valA == 1)
    {
    nodes[x, y] = new Node(true, false);
    }
    else
    {
    nodes[x, y] = new Node(false, false);
    }
    }
    }

    nodes[expos, eypos] = new Node(true, false)

    }

    • Hey GeorgeL and Darthuvius, look at the comment above your comments, the implementation is fairly easy, but keep in mind you don’t set a node as target/finish but a position in the array. Here is a simple implementation/code snippet once again. (In a game you would execute this code, store the list and gradually over gametime make the pawn get closer to the next node in the list (either lineair or with some spiffy code), once it’s at the node you remove that node and get to the next one).

      Node[,] nodes = new Node[8,8];
      for(int x = 0; x <= nodes.GetUpperBound(0); x++)
      {
      for(int y = 0; y <= nodes.GetUpperBound(1); y++)
      {
      nodes[x, y] = new Node(true, true);
      }
      }
      finder = new PathFinder(nodes);
      finder.SearchPath(0, 0, 4, 4);

      Goodluck!

  14. GeorgeL said

    This example wont really help me then I’m guessing. I am making a Digger (www.digger.org) clone and have one enemy chase the player, but can only go throug cut tunnels. I have everything except the AI done.

    • Hey GeorgeL, I dont understand what you want from this example, ofcourse it’s not a full AI implementation but it’s just pathfinding and you can set a start and end point as I’ve shown in my last comment.

  15. jbaiguade said

    thanks for the algorithm implementation! great work!

    I’ve done a full sample in XNA based in your class:

    ht_tp://desenvolupa.wordpress.com/

  16. Roy,

    I think I’ve found some kind of bug in the algorithm implementation. Please check this code:

    http://www.verticaling.com/archivos/AStarPathfindingLoadedMap.rar

    It is a simple call to your A*Algorithm implementation from a XNA 3.0 project. See that you can click on the map with the mouse: left button moves the destiny of the “route”, and the right button moves the “origin”.

    Moving the destiny does not cause any problem, but curiously moving the origin causes a SystemOutOfMemory exception. This exception is caused by the private List CreatePathList(Node lastNode) method, when de path.count gets up to 134217728 elements.

    • Hey man,

      I’ll check it once I have time however this code was not intended to be used on such large paths, it’s to be used ‘realtime’ for small paths about 1000 elements or something. Anyway I will check it once I find some time :), thanks for your reply.

  17. jbosch said

    Hi,

    I think the problem is not the number of elements, because when you move the “destiny” the algorithm finds the path well and fast. The problem appears only when you “move” the origin.

    Thanks anyway 🙂

  18. Ryan said

    Hi Roy,

    I’m a beginner at game development in general. I’ve been using XNA for about the past month, exclusively learning how to write 3D games.

    I’m working on a 3D tower defense game and have my terrain drawn, enemies drawn and able to move around. I’m ready to implement A* for pathing. Your example is the cleanest and most understandable that I’ve found – thank you.

    I’ve been told that 2D pathing is the same for a 3D game. I’m having a little bit of difficulty wrapping my brain around how to use this algorithm for a 3D terrain, though. How would I correlate the 2D grid to my 3D terrain, so that my enemy units work with 3D coordinates, yet are able to check the 2D grid for if they can move there or not?

    Thanks in advance

    • Hey Ryan,

      It depends on what style of 3D you are using, if you are just using 3D as a beautification it’s best of thinking as your terrain viewed from directly above it’s not hard to imagine how the A* algorithm would work in that way. About the 3rd coordinate (height) try to let your bots follow the terain by collision with the mesh or by a heightmap. A* will tell them to go left for 100feet but your collision algorithm should tell them that there is a bump so they should move up a bit.

      If there is a mountain that has a very steep side, best make that side “non travelable” so that bots will realistically go around it.

      If your terrain is extremely complex with tunnels and stuff it might become very hard to use standard tiles for this algorithm. The best thing for these complex terrains in my opinion is the ‘breadcrumbs’ system as found in games like Unreal. Instead of tiles you drop breadcrumps in your map (Just a struct with a list and a vector3). Place breadcrumps in your map where you want paths. In the list of each breadcrump list the adjacent breadcrumps reachable from that destination. A bot will look for its closest breadcrumb, and a breadcrumb close to the target. And will then via A* just calculate the root, but because of following the breadcrumbs he can actually follow a path trough a tunnel without becomming confused.

      But I think the first way is all you need for a TD game. Goodluck and be sure to show the game once you’ve finished it!

      If your 3D terrain is really complex

      • Ryan said

        Thanks a lot for the response! I’ll see what I can come up with this week. Yea, I’m just using 3D for beautification, basically. It’s still a top-down view.

  19. Ryan said

    As was mentioned earlier in these comments – the A* algorithm doesn’t quite like it when you move the player, thus changing the origin point. Have you figured out what’s causing this?

    Currently, I’m just calculating this backwards – changing the “destination” values, keeping the “origin” the same. I don’t know if this will produce bad results in the long run or anything, but the other way around doesn’t work – it hangs.

  20. […] Implementing A* path finding in C# (and XNA): source-code (can we cut the corner?) […]

  21. Vos said

    Hi,
    is it possible to find the “nearest” square ?
    Imagine a big circle with blocking squares and you set the target into it.
    If it is not reachable it will return an empty list, but can you
    just “try” to reach it and return the list of nodes up to the one that
    is nearest ? So the unit will stuck in front of the wall with the lowest
    distance to its target.

  22. liccowee said

    hi, Royalexander, i cant find any source code at My Skydrive. so how can i get it anymore?

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

 
%d bloggers like this: