# 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<Node> 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 <= Grid.GetUpperBound(0); x++)
{
for (int y = 0; y <= Grid.GetUpperBound(1); y++)
{
Grid[x, y].Parent = null;
}
}

List<Node> OpenList = new List<Node>(); //Nodes to be considered, ones that may be on the path
List<Node> ClosedList = new List<Node>(); //Explored nodes
Point CurrentPoint = new Point(0, 0);
Node Current = null;
List<Node> 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;
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
if (ClosedList.Contains(neighbour)) continue;

//Skips the node if it's a wall
if (neighbour.IsWall) continue;

//If parent is null, it's our first visit to the node
if (neighbour.Parent == null)
{
neighbour.G = Current.G + 10; //10 is the cost for each horizontal or vertical node moved
neighbour.Parent = Current; //Where it came from, final path can be found by linking parents

//The following way of calculating the H value is called the Manhattan method, it ignores any obstacles
neighbour.H = Math.Abs(neighbour.X - end.X)
+ Math.Abs(neighbour.Y - end.Y); //Calculates total cost by combining the X distance by the Y
neighbour.H *= 10; //Then multiply H by 10 (The cost movement for each square)
OpenList.Add(neighbour);
}
else
{
//Is this a more efficient route than last time?
if (Current.G + 10 < neighbour.G)
{
neighbour.Parent = Current;
neighbour.G = Current.G + 10;
}
}
}

OpenList.Sort(); //This uses the IComparible interface and CompareWith() method to sort
}

//If we finished, end will have a parent, otherwise not
Path = new List<Node>();
Current = Grid[end.X, end.Y]; //Current = end desination node
Path.Add(Current);
while (Current.Parent != null) //Won't run if end doesn't have a parent
{
Path.Add(Current.Parent);
Current = Current.Parent;
}

//Path.Reverse();
//.reverse() (Above) is replaced with the below code
for (int i = 0; i < Path.Count() / 2; i++)
{
Node Temp = Path[i];
Path[i] = Path[Path.Count() - i - 1];
Path[Path.Count() - i - 1] = Temp;
}

//Have we found a path or used all our options?
return OpenList.Count > 1 ? Path : null;
//Below replaced by Ternary Statement above
/*if (OpenList.Count > 1)
return Path;
else
return null;*/
}

private static List<Node> GetNeighbours(Point p)
{
List<Node> Result = new List<Node>();

if (p.X - 1 >= 0)
{
Result.Add(Grid[p.X - 1, p.Y]);
}

if (p.X < Grid.GetUpperBound(0))
{

Result.Add(Grid[p.X + 1, p.Y]);
}

if (p.Y - 1 >= 0)
{
Result.Add(Grid[p.X, p.Y - 1]);
}

if (p.Y < Grid.GetUpperBound(1))
{
Result.Add(Grid[p.X, p.Y + 1]);
}

return Result;
}
}

public class Node : IComparable<Node> //Inherits from the interface IComparable, allows the nodes to be sorted using .sort()
{
public int X; //Position on the X axis
public int Y; //Position on the Y axis
public Node Parent;
public bool IsWall;
public int G; //The amount needed to move from the starting node to the given other
public int H; //The estimated cost to move from that given node to the end point - Called Heuristic (Because it's a guess)
public int F //G+H
{
get { return G + H; } //Automatically calculates the latest value when F is accessed
}

/*This must be added due to IComparable (It requires a method called "CompareTo" to work)
- specifies how to sort the nodes*/
public int CompareTo(Node other)
{
if(this.F < other.F) return -1;
else if (this.F == other.F) return 0;
else return 1;
}

public Node Clone(bool ResetGHandParent)
{
Node Result = new Node();
Result.X = this.X;
Result.Y = this.Y;
Result.IsWall = this.IsWall;
if(ResetGHandParent)
{
Result.G = 0;
Result.H = 0;
Result.Parent = null;
}
else
{
Result.G = this.G;
Result.H = this.H;
Result.Parent = this.Parent;
}
return Result;
}

public Node Clone()
{
Node Result = new Node();
Result.X = this.X;
Result.Y = this.Y;
Result.IsWall = this.IsWall;
Result.G = this.G;
Result.H = this.H;
return Result;
}

public static Node[,] MakeGrid(int Width, int Height)
{
//If they don't set a value for PercentWallChance, we assume they don't want any walls and set the chance to 0
return MakeGrid(Width, Height, 0);
}

public static Node[,] MakeGrid(int Width, int Height, int PercentWallChance)
{
Node[,] Result = new Node[Width,Height];
Random r = new  Random();

for (int x = 0; x < Width; x++)
{
for (int y = 0; y < Height; y++)
{
Result[x,y] = new Node();
Result[x, y].Parent = null;
Result[x, y].X = x;
Result[x, y].Y = y;
Result[x, y].G = 0;
Result[x, y].H = 0;
//This takes advantage of the fact that < is a comparison operator, and will give a boolean result
//r.Next(100) will generate a number from 0 to 99.
//If % wallchance is 100, then r cannot NOT be less than it, and the "<" will always return true (which in turn always sets IsWall to true)
//If % wallchance is 0, then r literally cannot be less it, and the comparison will return false (which in turn always  sets IsWall to false)
Result[x, y].IsWall = r.Next(100) < PercentWallChance;
}
}

return Result;
}
}
}


## 2 Answers

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<T> 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<Node> with per-insert sorting and OrderedBag<Node> with automatic sort-on-insert. Here are the results:

• List<Node> = 379.3 msec
• OrderedBag<Node> = 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<Node> = 50.9 sec
• OrderedBag<Node> = 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<T>

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<T>.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<int, int> 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<UInt64, Node> for your ClosedList:

Dictionary<UInt64, Node> ClosedList = new List<Node>();


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<Node> OpenList; // from AStar.FindPath
public Dictionary<UInt64, Node> 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:

1. 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.
2. 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 that we can write better code because we don't have to handle all those types and stuff.

But the .NET framework is actually pretty well implemented. Yes, some things are slower than they could be, but those things are pretty rare these days. Most of the slow-downs are to do with making certain types thread-safe or handling edge-cases that you can safely ignore.

It's tempting to assume the worst and write optimized code to do things that a generic function couldn't possibly do better. Like reversing a list:

//Path.Reverse();
//.reverse() (Above) is replaced with the below code
for (int i = 0; i < Path.Count() / 2; i++)
{
Node Temp = Path[i];
Path[i] = Path[Path.Count() - i - 1];
Path[Path.Count() - i - 1] = Temp;
}


Okay, so this one is only done once at the end of the method, so it's no big deal. In fact this is a good reason to leave it alone - a single reverse, even a slow one, is nothing compared to the cost of the rest of the method.

But is it actually faster than the built-in Reverse method?

Surprisingly (to some), no.

Take a List<int> and fill it with 1000 integers. Now reverse it 1000 times using both the built-in Reverse and your code above (modified to int instead of Node of course). Results on my (sadly old and under-powered) laptop:

• Reverse - 83.2 msec
• Custom - 2488 msec

So the framework's generic code is 30 times faster than your hand-optimized code. In fact I did some tests using your code on an int[] array, and it's still slower (marginally) than the List<T>.Reverse. I can go into why, like the levels of indirection and processing involved in reading or writing an element in a List<T> and so on, but that's a book in itself.

The moral: Profile, then optimize (and profile again).

• First, find out where your code is spending its time.
• Write code that you can easily modify later, then profile where it goes slow.
• Find ways to fix those slow areas, and profile again.
• Never assume your replacement code is going to perform better - only profiling will tell you that.

### Other Things

Just tell me when you've read enough. Or else I'll write a novel.

• Wow! I did not expect such a large and detailed reply. I thank you massively, as this will be a huge help for my A-Level. I will definitely make use of this to improve my project. This may be asking too much, but if you could pop me an email at shivmalhotra@hotmail.co.uk I'd appreciate it. I really need some help with some of the points you raised (Not an overly experienced programmer myself). Thanks again, and if you don't want to help any further, no worries. – Shivam Malhotra Aug 26 '13 at 10:10
• Also, I probably am not allowed to use other people's code - Even if I reference it. So I may have to stick with the built in types unfortunately. However, I will definitely try nevertheless – Shivam Malhotra Aug 26 '13 at 10:25
• 1. I think that combining two 32-bit integers into a 64-bit integer for a key is not a clean solution, Tuple<int, int> (or a custom type) would be much better. 2. If you don't actually need the value, use a HashSet instead of Dictionary. The value could be either just the key, or the whole node with a custom equality comparer. – svick Aug 26 '13 at 11:51
• @ShivamMalhotra Maybe you are expected to implement such collection yourself? Writing a binary heap is not hard. – svick Aug 26 '13 at 11:57
• @svick Maybe, any good links to implementing a binary heap like the one Corey mentioned? – Shivam Malhotra Aug 26 '13 at 13:24

I did not read your code thoroughly . Corey already gave many detailed comments on the algorithm. I just want to comment instead of the general structure of your code. You might actually not care about this since maybe you just want your code to work.

Basically, your code is not very object oriented. The fact that you have mostly static methods is a quick give away. I would split your file in 3 or 4 classes. Instead of the grid being of type Node[,], I would defined a Grid class and I would put all relevant methods as (non-static) methods of that class. You have to do some serious thinking about which methods those would be. I might also define a Path class instead of using a List<Node>. That is only necessary if some methods can be included in that Path class. I have not read your code in enough detail to see if that is the case.

I would not make the A-star algorithm a method of the Grid class because you might want to compare many different path finding algorithms. You could maybe define an interface with a findPath(grid, startPoint, endPoint) method.

OO design is never trivial. As I progress on some project, I usually refactor everything every so often. I often realize my initial OO choices were wrong, so don't feel bad if you also refactor many times. You don't have to start from scratch when you refactor: it's really just copy and paste.

The algorithm for A-star takes many screens, so I would break that method in sub-methods. The main A-star method would be composed of only 3 or 4 sub-method calls, which I would name very clearly. That is not related to OO, but I just prefer to keep my methods short for clarity. (If the different algorithms share some of the sub-methods, you could actually use OO in that case to share those sub-methods.)

• Thanks, I actually do care about my code being object-oriented! So, I am going to follow your suggestions, thanks – Shivam Malhotra Aug 28 '13 at 6:13
• OO is great, but you can have too much of a good thing. I contributed to one project where the lead was a real OO-Nazi. He rejected half of my submissions because I had methods that were more than 50 lines long and I didn't use enough interfaces. shrug To each their own :) – Corey Aug 28 '13 at 22:53