Wikipedia A* pathfinding algorithm takes a lot of time

I've successfully implemented A* pathfinding in C# but it is very slow, and I don't understand why. I even tried not sorting the openNodes list but it's still the same.

The map is 80x80, and there are 10-11 nodes.

I took the pseudocode from here Wikipedia

And this is my implementation:

 public static List<PGNode> Pathfind(PGMap mMap, PGNode mStart, PGNode mEnd)
{
mMap.ClearNodes();

mMap.GetTile(mStart.X, mStart.Y).Value = 0;
mMap.GetTile(mEnd.X, mEnd.Y).Value = 0;

List<PGNode> openNodes = new List<PGNode>();
List<PGNode> closedNodes = new List<PGNode>();
List<PGNode> solutionNodes = new List<PGNode>();

mStart.G = 0;
mStart.H = GetManhattanHeuristic(mStart, mEnd);

openNodes.Add(mStart); // 1) Add the starting square (or node) to the open list.

while (openNodes.Count > 0) // 2) Repeat the following:
{
openNodes.Sort((p1, p2) => p1.F.CompareTo(p2.F));

PGNode current = openNodes[0]; // a) We refer to this as the current square.)

if (current == mEnd)
{
while (current != null)
{
current = current.Parent;
}

return solutionNodes;
}

openNodes.Remove(current);
closedNodes.Add(current); // b) Switch it to the closed list.

List<PGNode> neighborNodes = current.GetNeighborNodes();
double cost = 0;
bool isCostBetter = false;

for (int i = 0; i < neighborNodes.Count; i++)
{
PGNode neighbor = neighborNodes[i];
cost = current.G + 10;
isCostBetter = false;

if (neighbor.Passable == false || closedNodes.Contains(neighbor))
continue; // If it is not walkable or if it is on the closed list, ignore it.

if (openNodes.Contains(neighbor) == false)
{
openNodes.Add(neighbor); // If it isn’t on the open list, add it to the open list.
isCostBetter = true;
}
else if (cost < neighbor.G)
{
isCostBetter = true;
}

if (isCostBetter)
{
neighbor.Parent = current; //  Make the current square the parent of this square.
neighbor.G = cost;
neighbor.H = GetManhattanHeuristic(current, neighbor);
}
}
}

return null;
}


Here's the heuristic I'm using:

    private static double GetManhattanHeuristic(PGNode mStart, PGNode mEnd)
{
return Math.Abs(mStart.X - mEnd.X) + Math.Abs(mStart.Y - mEnd.Y);
}


What am I doing wrong? It's an entire day I keep looking at the same code.

• Without heuristic it should (usually) take longer as you go through more nodes till you find the end. Also, try using a sorted list which remains sorted (preferably a sorted set, that way you dont have to check whether an item exists in the list you can just add it) – Elva Feb 1 '11 at 16:50

I see three things, one wrong, two suspicious.

1) You're sorting on every iteration. Don't. Either use a priority queue, or at the very least do a linear search to find the minimum. You don't actually need the whole list to be sorted at all times!

2) openNodes.Contains() is probably slow (not sure about the specifics of C#'s List, but I bet it does a linear search). You can add a flag to each node and do this in O(1).

3) GetNeighborNodes() could be slow.

• 2) Yeah, Contains() will be quite slow. Rather than store all your nodes in lists, use a Dictionary<int, PGNode>. Then you get O(1) lookup time and can still iterate list a list. If nodes have an id field, use that for the key, otherwise PGNode.GetHashCode() will work. – Leniency Feb 2 '11 at 20:45
• @Leniency: Wouldn't Dictionary<PGNode, PGNode> be better? Two objects may have the same hash code but not be equal. "Consequently, the default implementation of this method must not be used as a unique object identifier for hashing purposes." msdn.microsoft.com/en-us/library/system.object.gethashcode.aspx - .NET 3.5 provides HashSet, which is better - msdn.microsoft.com/en-us/library/bb359438.aspx. – user744 Feb 2 '11 at 21:28
• Good point, forgot about HashSet. – Leniency Feb 3 '11 at 22:34

In addition to the point already made that you should use a priority heap, you've misunderstood the heuristic. You have

if (isCostBetter)
{
...
neighbor.H = GetManhattanHeuristic(current, neighbor);
}

But the heuristic is supposed to be an estimate for the distance to the destination. You should set it once, when you first add the neighbour:
if (openNodes.Contains(neighbor) == false)
{
neighbor.H = GetHeuristic(neighbor, mEnd);
...
}

And as a further minor point, you could simplify the A* by filtering out impassable nodes in GetNeighbourNodes().

• +1, I focused on algorithmic complexity and completely missed the wrong use of the heuristic! – ggambett Feb 1 '11 at 23:14

The meta-answer: you should never just spend a day staring at code looking for performance problems. Five minutes with a profiler would show you exactly where the bottlenecks are. You can download a free trail of most profilers and get it hooked up to your app in a few minutes.

It is not clear what you are comparing when you compare the F of different nodes. Is F a property defined as G + H? It should be. (Side-rant: This is an example of why the uniform access principle is crap.)

More important though, you are re-sorting the nodes every frame. A* calls for the use of a priority queue, which allows efficient - O(lg n) - sorted insertion of a single element, and a set, which allows quick checks for closed nodes. As you have written the algorithm, you have O(n lg n) insertion + sort, which raises the run-time to useless proportions.

(You might get O(n) insertion + sort if C# has a good sorting algorithm. It's still too much. Use a real priority queue.)

http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html

• At one extreme, if h(n) is 0, then only g(n) plays a role, and A* turns into Dijkstra’s algorithm, which is guaranteed to find a shortest path.
• If h(n) is always lower than (or equal to) the cost of moving from n to the goal, then A* is guaranteed to find a shortest path. The lower h(n) is, the more node A* expands, making it slower.
• If h(n) is exactly equal to the cost of moving from n to the goal, then A* will only follow the best path and never expand anything else, making it very fast. Although you can’t make this happen in all cases, you can make it exact in some special cases. It’s nice to know that given perfect information, A* will behave perfectly.
• If h(n) is sometimes greater than the cost of moving from n to the goal, then A* is not guaranteed to find a shortest path, but it can run faster.
• At the other extreme, if h(n) is very high relative to g(n), then only h(n) plays a role, and A* turns into Best-First-Search.

You are using 'manhatten distance'. This is almost always a bad heuristic. Additionally, from looking at that info from the linked page, you can guess that your heuristic is lower than the true cost.

• -1, the problem is not the heuristic, but the implementation. – user744 Feb 1 '11 at 20:04

In addition to the other top answers (which are undoubtedly more significant than this suggestion), another optimisation is to change the closed 'list' into some sort of hash table. You don't need it to be an ordered collection, just to be able to quickly add values and to quickly see if they exist in the collection.

Your Cost and your Heuristic need to have a relationship. It should be a clue that H is calculated in two different spots but never accessed.

• This assumes the property is implemented incorrectly, which is possible since its definition is not shown, but there are two more immediate problems with the code. – user744 Feb 1 '11 at 20:08