No longer updated: new and updated blog (including old content) at

Archive for, please update your bookmarks

New Version A* pathfinding in 3D

Posted by Roy Triesscheijn on Tuesday 7 July, 2009

Note: since the 1st of December 2009, this blog was moved to, 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.


Roy Triesscheijn

Intro and thanks.

Ok so let’s quickly forget the debacle around the first release of this code sample, I wan’t to delete the entire post about that one but I would’nt  have know that I made an error (until much later on) if it wasn’t for some people spotting it instantly! So I would first like to thank some of my visitors who spotted the errors:

-Oscar for spotting possible performance problems.

-Ryan for suggesting possible HashSet’s (altough I didn’t use them in the end, see below why).

I would also like to thank posters from  who helped me get this version of A* so fast, especially (on post order):

-Phyxion & Mammon for their one line version of Equals()

-Nick The Heazk for spotting that I totally forgot the Heuristic part for scoring (doh, I swear I had it in my draft)

-pedorus &  jip_86 for suggesting to use a Binary(Min)Heap

-Marcj & NotPingu for pointing out a 3D version of Pythagoras

-Soultaker for letting me think more about efficiency of containers (O(?) of add, extract and contains))

-.oisyn & pedorus for suggesting to combine a binary heap and a HashSet (altough I didn’t use it in the end, see why below)

-PrisonerOfPain for suggesting to use binary flags instead of extra HashSet’s (that’s why I didn’t use them).

Why is this version faster?

A lot was changed from the previous version, let’s start with the openList, checking if an item is in the openList costs O(n) iterations and checking which item has the lowest score costs O(n) and removing an item costs O(log(n)). So I’ve replaced the openList with a BinaryHeap for checking the lowest score and removing/adding. These operations cost O(1), O(log(n)). That’s allot faster already.

As for checking if an item is in an openList (or closedList) all I did was add a boolean flag to the BreadCrumb class (onOpenList and onClosedList).  Checking this flagg is O(1) so that really makes checks allot faster.

Also all I needed the closedList for was checking if items where already in there, so with the boolean flag I could completely remove the closedList.

Another new feature is that we immediatly return from the FindPath function when we find the finish, this also saves some operations.

I also made sure that I don’t  create to much new objects but re-use them after they where first indexed (this was  also the cause for a small ‘score’ bug) and I’ve re-aranged some ifs.

For the algorithm itself I now use DistanceSquared for the (forgotten) Heuristic part of the scoring which is allot better than ManHattan distance which is slightly biased. I’ve also changed the cost (G) to move from the current node to the next node to incorporate these squared cost  (so instead of 1,  1.4 and 1.7 I can now use 1,2,3 for these scores. I also don’t have to multiply all these numbers by 10 since 1, 2 and 3 are integers.

A final additon is the class Point3D which allows us to use only Integers instead of floats (which are slower for a cpu).

All in all this made this code about 200x faster (yes really!). But that is not completely fair since the first code was broken. If you count from after I fixed the heuristic part of the code the code is ‘only’ about 30x faster.

Benchmark results.

And before we get to the exciting part (the code!).  Let’s first show some benchmarks.


World: 10x10x10

Start: 0,0,0

End: 9,5,8

Nodes blocked: 300


Total (100 iterations) : 134ms

Average:  1.34ms

Lowest:   < 1ms

Highest:  17ms

Note: as you can see because we did this test 100 times there is quite allot off difference between the lowest and highest time we needed to calculate this route, that’s because sometimes the cpu stops executing this program, and starts handleing an irq or doing something else, that’s why it’s important to always take a big number of benches to be representative. We can see from the average that this code is extremely fast.


A* is an extremely efficient algorithm, consider we would’ve liked to brute-force this problem instead of using A*. That would’ve cost us 10^10^10 = 1,e+100 iterations. However with an (this) A* implementation we only needed 119 iterations (in which we checked 3094 nodes).

So, on to the code!

The code.

Well since I’ve divided every thing in much neater classes it’s a bit of a problem adding showing off all these as code-listings as I usually do. So I will only show the most important method here, the rest of the classes, and an example  you can download at the bottom of this article or by clicking here

        /// <summary>
        /// Method that switfly finds the best path from start to end. Doesn't reverse outcome
        /// </summary>
        /// <returns>The end breadcrump where each .next is a step back)</returns>
        private static BreadCrumb FindPathReversed(World world, Point3D start, Point3D end)
            MinHeap<BreadCrumb> openList = new MinHeap<BreadCrumb>(256);
            BreadCrumb[, ,] brWorld = new BreadCrumb[world.Right, world.Top, world.Back];
            BreadCrumb node;
            Point3D tmp;
            int cost;
            int diff;

            BreadCrumb current = new BreadCrumb(start);
            current.cost = 0;

            BreadCrumb finish = new BreadCrumb(end);
            brWorld[current.position.X, current.position.Y, current.position.Z] = current;

            while (openList.Count > 0)
                //Find best item and switch it to the 'closedList'
                current = openList.ExtractFirst();
                current.onClosedList = true;

                //Find neighbours
                for (int i = 0; i < surrounding.Length; i++)
                    tmp = current.position + surrounding[i];
                    if (world.PositionIsFree(tmp))
                        //Check if we've already examined a neighbour, if not create a new node for it.
                        if (brWorld[tmp.X, tmp.Y, tmp.Z] == null)
                            node = new BreadCrumb(tmp);
                            brWorld[tmp.X, tmp.Y, tmp.Z] = node;
                            node = brWorld[tmp.X, tmp.Y, tmp.Z];

                        //If the node is not on the 'closedList' check it's new score, keep the best
                        if (!node.onClosedList)
                            diff = 0;
                            if (current.position.X != node.position.X)
                                diff += 1;
                            if (current.position.Y != node.position.Y)
                                diff += 1;
                            if (current.position.Z != node.position.Z)
                                diff += 1;
                            cost = current.cost + diff + node.position.GetDistanceSquared(end);

                            if (cost < node.cost)
                                node.cost = cost;
                       = current;

                            //If the node wasn't on the openList yet, add it
                            if (!node.onOpenList)
                                //Check to see if we're done
                                if (node.Equals(finish))
                           = current;
                                    return node;
                                node.onOpenList = true;
            return null; //no path found

(note the class also has a normal FindPath() method that switches start and end for you).


In case you missed the download link in the middle of the text, download the entire example:  here

kick it on


23 Responses to “New Version A* pathfinding in 3D”

  1. […] New Version A* pathfinding in 3D […]

  2. SamIAm said

    I’m sure some folk will hate me for this but…

    “Allot” means to assign a portion.
    “A lot” means many.
    Choosing the improper word here changes the entire meaning of a sentence.

    “All and all” is not an idiom.
    “All in all” means altogether.
    This one doesn’t matter as much, it’s just improper.

    There are other grammatical errors, but…

    Aside from that, good job on the pathfindng!

    • Hey SamIAm,

      Don’t worry I won’t hate you for it, I’m not a native English speaker, so yeah I do tend to make mistakes like that, but now way to learn from my mistakes when no-one points them out. I’ll correct them asap.

  3. Ric said

    thanks for your code.
    Can you share a very simple xna 2d project example instead of a console app?
    I encountered some problems with update method.

    • Hey Ric,

      I’m afraid I don’t have a 2D project with A* in it available yet, might try to put one online but don’t count on it for this week. I’m curious to here though what problems you are having with the ‘update’ method (I assume you mean update in the Game1 class?).

      • Ric said

        I read that you aren’t native english speaker, I am worse..:)
        My problems refers to position of a little tank and how I can move it from a starting point to an end point. I put an instance of a world class on Game1. I create this world with 10,10,10.
        I use a simple tank class derived from DrawableGameComponent with a queue collection that I can use to set the tank path.
        After world initialization, I try to set an arbitrary start and end point to obtain a path from your pathfinder class. Here a code snippet from loadcontent method:
        crumb2 = PathFinder.FindPath(world, Point3D.Zero, new Point3D(5, 8, 9));
        while ( != null)
        Vector2 wayPoint = new Vector2((float)crumb2.position.X, (float)crumb2.position.Y);
        crumb2 =;

        I don’t know if this is good or wrong. Maybe, position of breadcrumb are to be modified with the screen position of the object to move?.
        Now, I think that I need to put in the update method of the tank object (it’s a drawablecomponent class and have an update method like the Game class) a piece of code that can move it from start to end of the path but I’m not shure.
        To build a simple test project of your pathfinder classes I used an example from with a simple tank class and a “waypoint system”

        • Hey Ric,

          Don’t worry your English is fine :). And well the world and the positions you get back are not real world co-ordinates indeed. You should think of the 10x10x10 world as a map, everything on it is really small. So if you wan’t to move from California to Arizona, on the map its 3 points away. In the real world this would mean (for example) 300miles. You have to let the world class reflect how your real world looks. But it is not the real world. (Sorry if this is a bit confusing). The world class just divides the real world into chunks.

          So you have to have code figuring out where World(1,1) really is in your real world. (It could for example be (150,150,0). And enqueue that position in your waypoints system. (I’ve never used the waypoint tutorial though, so I’m not sure what other steps are needed).

  4. Ric said

    The world class just divides the real world into chunks

    Ok, I understand now. It work fine, thanks.

  5. Martin said

    “Don’t worry I won’t hate you for it, I’m not a native English speaker, so yeah I do tend to make mistakes like that, but now way to learn from my mistakes when no-one points them out. I’ll correct them asap.”

    now -> now

    The most obvious errors:

    – to much new objects -> too many new objects
    – would’nt -> wouldn’t
    – have know -> have known
    – allot off difference -> a lot of difference (much difference)
    – allot faster -> a lot faster

    And some sentences are way too verbose:
    – Check to see if we’re done -> Check if we’re done

    Your blog posts are interesting, but your english is sometimes atrocious.

    • Jay B said

      Who cares, really? If you can understand it then it’s good enough. I’m sure the author appreciates the corrections; in my opinion these comments should be about the content of the article and not the grammar.

      • Altough I will agree that the function of language is transferring knowledge and that the article was well enough written to transfer knowledge thus being adequate enough. I state explicitly in my about page that I wan’t to learn to write English better and constructive critisme like Martin’s is a way for me to become better, so yeah sometimes it’s just ‘the grammar nazi’s’ but once in a while pointing out frequent mistakes is not so bad at all :).

        Thanks for your comment though!

  6. Tim said

    At first. Great work, it’s very fast and well coded.
    Is it possible to implement the “CanCutCorners” property of your old A* implementation into this version?
    Would be great!

    • Hey Tim,

      Pooh the CanCutCorners prop, hmm I kinda hung up on the idea, thought no one used it, it would probably add some time to calculating routes but it’s very easy to implement yourself. At this moment CanCutCorners is ‘always on’ to disable it you should add a check that for each diagonal move checks if the square between the two is free aswell.


      As above the move a->d is valid if you allow corner cutting or if b and c are free, if you don’t want corner cutting check to see if b or c is ocupied. This all should be pretty easy to implement, but it would kinda hard to make it really fast.

      Try it out and tell me if you succeeded (currently way to busy catching up on some stuff, sorry)

  7. Juan Campa said

    One problem I see is that the information about the node being in the open/closed set is stored in the node itself (using binary flags), so you can’t have several pathfindings running at the same time (for example, in different threads) which might be desired if performance becomes an issue.

    Good job,
    Juan Campa

    • Very true! Good observation, you could revert back to lists (which will make this version slower, but will probably make it faster overall if you use more threads) to accomplish that though.

  8. […] New Version A* pathfinding in 3D Roy A. Triesscheijn shares a new version of his 3D A* path finding code utilizing binary heaps. […]

  9. EasyLay said

    Nice work (and ignore the grammar nazis)

  10. DaisyDude said

    Thanks Roy for the code.
    However I want to apply A* in my game. However my game is not grid based. The game consists of surpentine roads, highways, etc. So how can I apply A* to it?

    • You have to think about what parts are accesible from the part you are standing now, try to make some node like connectionist structure. Might be a bit hard to do.

      In games like Unreal Tournament, they place apples (really) in games and let the computer calculate a grid between those apples, to in the end make a node like structure.

      Btw: best ask your next question @ I dont look here to often.

  11. DaisyDude said

    Thanks for such a quick reply,
    The new add u mentioned was not working??
    Anyway, I was thinking that use a 2D array and store all the coordinates with sapcing around 2to 5 X & Z in that array. The boundaries will be the values in the matrix where the buildings will reside so that the cars don’t go in the buildings.
    Basically I want to use A* because I want how other people will drive their vechiles. I am making a GTA free style game with all the traffic, peds, etc. Currently what I am doing is setting waypoints. As the car reaches a specified axis then turn the car, decrease movements, etc. But as the city is too big I have to set coordinates for each car. So I was thinking to just mark the beg and goal positions and the car will automatically get their paths, etc.
    Also if the car collides with me or any other traffic component it must not stay dumb and don’t move. Rather treating the component as the boundary it must instantly make a A* seacrh and try to find other path.

  12. hamo said

    if (cost < node.cost)
       node.cost = cost; = current;

    Algorithm does not seem to be correct. openList is not updated with this new cost.

  13. Unreal said

    Hey dude ,

    Thanks alot i had several problems with implementing my own a* algorythm and your class just helped me a ton ! 🙂
    This is my current progress 🙂 Just ve to tweak it a little and im done ! 😀

Leave a Reply

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

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

Google+ photo

You are commenting using your Google+ 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 )


Connecting to %s

%d bloggers like this: