New Version A* pathfinding in 3D
Posted by Roy Triesscheijn on July 7th, 2009
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 www.tweakers.net 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.
Situation:
World: 10x10x10
Start: 0,0,0
End: 9,5,8
Nodes blocked: 300
Time:
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.
Efficiency:
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;
openList.Add(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;
}
else
{
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;
node.next = 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))
{
node.next = current;
return node;
}
node.onOpenList = true;
openList.Add(node);
}
}
}
}
}
return null; //no path found
}
(note the class also has a normal FindPath() method that switches start and end for you).
Download.
In case you missed the download link in the middle of the text, download the entire example: here
July 7th, 2009 at 4:09 PM
[...] New Version A* pathfinding in 3D [...]
July 7th, 2009 at 11:28 PM
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!
July 8th, 2009 at 9:42 AM
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.
July 12th, 2009 at 5:22 PM
Hello,
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.
Thanks
July 12th, 2009 at 11:01 PM
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?).
July 12th, 2009 at 11:37 PM
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 (crumb2.next != null)
{
Vector2 wayPoint = new Vector2((float)crumb2.position.X, (float)crumb2.position.Y);
_myTank.Waypoints.Enqueue(wayPoint);
crumb2 = crumb2.next;
}
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 creators.xna.com with a simple tank class and a “waypoint system” http://creators.xna.com/en-US/sample/waypoints
July 13th, 2009 at 7:53 AM
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).
July 14th, 2009 at 11:12 AM
The world class just divides the real world into chunks
Ok, I understand now. It work fine, thanks.
July 18th, 2009 at 3:33 PM
“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.
July 25th, 2009 at 7:18 PM
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.
July 26th, 2009 at 9:35 PM
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!
August 4th, 2009 at 7:24 PM
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!
August 4th, 2009 at 7:30 PM
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.
ab
cd
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)
October 5th, 2009 at 11:54 PM
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
October 6th, 2009 at 8:27 AM
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.
October 15th, 2009 at 3:17 PM
[...] New Version A* pathfinding in 3D Roy A. Triesscheijn shares a new version of his 3D A* path finding code utilizing binary heaps. [...]
October 30th, 2009 at 5:37 PM
Nice work (and ignore the grammar nazis)
October 30th, 2009 at 6:14 PM
Nice work, and about the grammar nazis, I explicitly ask people to give me comments on my English writing so I can improve it. So it’s alright
.
December 11th, 2009 at 10:42 AM
why I cannot upzip your source codes package?
December 15th, 2009 at 8:39 PM
They are packed in winrar, download it at http://www.rarlabs.com. Then you should be able to unpack them normally.
December 29th, 2009 at 7:07 PM
[...] Recent Comments Roy Triesscheijn on AboutLotte on AboutRoy Triesscheijn on Sending objects via high speed asynchronous sockets in C# (Serialization + Socket programming)bar on Sending objects via high speed asynchronous sockets in C# (Serialization + Socket programming)Roy Triesscheijn on New Version A* pathfinding in 3D [...]
March 14th, 2010 at 4:34 PM
Hi, nice work!
I’m playing with 2D graphics so I removed the extra dimension from PathFinder and the algorithm runs approximately 2.5 times faster. I don’t know exactly why, but i might be because PathFinder.surrounding returns nodes outside Z[0] even when there are only two dimensions.
March 15th, 2010 at 9:00 AM
Hey Ulf,
It’s not strange that it goes 2.5 times faster because the surrounding method returns 8 tiles for 2D and 26 tiles for 3D. It’s true that the surrounding method returns coordinates outside the map, but you have to check once if they’re inside the map or not. Here I do that in world.PositionIsFree(tmp) method instead of making surrounding more sophisticated, it should make anything slower.
Glad you liked the code, and I appreciate the comment!
March 22nd, 2010 at 11:44 PM
[...] 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! [...]
June 27th, 2010 at 2:27 PM
[...] you might know I’ve written quite a lot of articles on the A* [...]
September 24th, 2011 at 9:11 PM
[...] http://roy-t.nl/index.php/2009/07/07/new-version-a-pathfinding-in-3d/ [...]