How can I improve upon my A* Pathfinding code?

How can I make it faster, more efficient, and simpler? Are there any obvious mistakes I've made, or things which will make my code "fancier"?

It's my first time working with any form of pathfinding, so I'm sure I've made plenty of mistakes. It still works, though.

I've ended up adding a lot of comments because I keep forgetting how my own code works.

```
public class AStar //Variant on Dijkstra's Algorithm - faster
{
public static Node[,] Grid; //Using the data type created below to make a grid, a 2 dimensional array of nodes (squares)

public static List FindPath(Point start, Point end) //Finds the fastest route from the start to end
{

//Checks that we aren't trying to make a path from one place to the same place
if(start.X == end.X && start.Y == end.Y)
{
return null;
}

//Resets all the parent variables to clear the paths created last time
for (int x = 0; x OpenList = new List(); //Nodes to be considered, ones that may be on the path
List ClosedList = new List(); //Explored nodes
Point CurrentPoint = new Point(0, 0);
Node Current = null;
List Path = null;

OpenList.Add(Grid[start.X, start.Y]); //Add the starting point to OpenList

while (OpenList.Count > 0)
{
//Explores for the "best" choice in the Openlist
Current = OpenList[0];
CurrentPoint.X = Current.X;
CurrentPoint.Y = Current.Y;
if (CurrentPoint == end) break; //If we have reached the end

OpenList.RemoveAt(0); //Removes the starting point
ClosedList.Add(Current);

foreach (Node neighbour in GetNeighbours(CurrentPoint)) //Checks all the squares adjacent to the current point
{
//Skips fully explored nodes which have been explored fully

Some points to consider...

Sorting is slow. Really slow.

At the end of your loop you call OpenList.Sort() to order your nodes by relative cost so that you can work on the lowest-cost nodes first. Depending on how far your search extends, this can lead to large amounts of time doing nothing but sorting the list.

Instead of a per-iteration sort, use a collection that sorts for you on insert. This reduces the number of comparison operations, and the insert itself is usually much quicker than a shuffle.

I've tried a few ordered list implementations, but the OrderedBag class from Wintellect's PowerCollections is my favorite.

To give you an idea of how much faster this is, I generated 1,000 random Nodes and ran them into both List with per-insert sorting and OrderedBag with automatic sort-on-insert. Here are the results:

• List = 379.3 msec

• OrderedBag = 4.15 msec

Pretty big improvement there. And it only gets better the bigger your search area. Here's the results for 10,000 items:

• List = 50.9 sec

• OrderedBag = 18.6 msec

Yup, that 2,500 times faster. I wasn't keen to try it at 100,000. I think you've got the picture by now :grin:

There's an unofficial version of PowerCollections on Nuget if you aren't keen to download the source or binary distributions. Nuget is simpler, anyway.

--Update: OrderedSet

While implementing an AA-Tree class (about 20% slower in debug mode than the Wintellect OrderedBag) I came across a reference to OrderedSet being implemented as a Red-Black Tree. I threw them all together and ran the test again.

Results:
* List with sort-on-insert 380ms
* OrderedBag - 40.9ms
* AATree (my implementation) - 48.6ms
* OrderedSet - 0.75ms

Yes, 0.75ms.

OK, it has some limitations, but for what you're doing it's a speedy solution that's already in the .NET framework. Refer to my Don't try to out-think the framework section below :)

List.Contains is probably not your friend

This line will slow you up significantly:

if (ClosedList.Contains(neighbour)) continue;

Since ClosedList is an un-ordered list, the Contains method is required to iterate through the list every time. It has to scan the whole list to find out that an item is not present, and the list just keeps on getting bigger.

The simple answer here is to use a Dictionary or similar to index your items for quick lookup. You just need a per-node unique key, such as position. You could use a Tuple as the key, but it's simple enough to bit-stuff the X and Y into a 64-bit integer and use that.

Add this to your Node class:

public UInt64 Key { get { return (((UInt64)(UInt32)X) << 32) | (UInt64)(UInt32)Y; } }

Now use a Dictionary for your ClosedList:

Dictionary ClosedList = new List();

Adding nodes becomes:

ClosedList[Current.Key] = Current;

And now you can replace that inefficient Contains call with:

if (ClosedList.ContainsKey(neighbour.Key)) continue;

Caching is GOOD (sometimes)

If you are working in a static map that you know isn't going to change, you can get a big speed-up on subsequent path-finding from the same starting location by caching the state of your path-finding algorithm.

What you want is a structure similar to this:

public class PathfinderState
{
    public Node[,] Grid; // Copy of: AStar.Grid
    public OrderedBag OpenList; // from AStar.FindPath
    public Dictionary ClosedList; // from AStar.FindPath
}

This contains all the information you need to either generate a path or continue your path-finding.

Each time you want to find a path from A to some arbitrary B:

• Get PathfinderState for point A from cache, or empty state if not found in cache

• if state.CloseList contains target point:

• return path

• pass state to pathfinding code to continue processing until target found

• update state cache if necessary

• return path

Saves a lot of time.

There are a couple of potential problems with this:

• If your map changes you have to prune the cache. You might get away with keeping some of it, but mostly you have to dump the lot and start over.

• Memory. Lots and lots of memory.

If you order your cache by last-access you can limit the memory usage a little. But if your map obstructions are going to be changing often, caching is not going to be much help.

Premature Optimization

Or: Don't try to out-think the framework

We've all been guilty of this. I spent 4 hours one day coding a solution to optimize a data mining task involving several million records across ~10 tables. I was so pleased with my code, which ran in about 35 minutes. Then I had to change something, and after half an hour of hacking on it I threw my hands up and just used a LINQ statement. It finished the job in under 5 minutes.

The .NET framework is a lot more mature than we give it credit for sometimes. Don't underestimate it. It's tempting to assume that the generic methods of doing things will be slow, and t

StackExchange Code Review Q#30243, answer score: 12