Question

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 80×80, 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);

        solutionNodes.Add(mStart);
        solutionNodes.Add(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)
                {
                    solutionNodes.Add(current);
                    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.

Answer 1

First off, use a profiler. Use tools to tell you what is slow; it is often surprising what is slow. And use a debugger. Make a map with five nodes in it and step through every line of the code as it tries to find the path. Did anything unexpected happen? If so, figure out what is going on.

Second, leaving aside your performance problem, I think you also have a correctness problem. Can you explain to us why you believe the Manhattan distance is a reasonable heuristic?

(For those reading this who are unfamiliar with the metric, the “Manhattan distance” or “taxi distance” is the distance between two points you’d have to travel if you lived in a city laid out on a grid. That is, to go 14 miles due northeast you’d have to travel 10 miles north and then 10 miles east for a total of 20 miles.)

The A* algorithm requires for its correctness that the heuristic always underestimates the actual distance required to travel between two points. If there are any “diagonal shortcut” streets in the graph then the Manhattan distance overestimates the distance on those paths and therefore the algorithm will not necessarily find the shortest path.

Why aren’t you using the Euclidean distance as your heuristic?

Have you tried your algorithm using “always zero” as the heuristic? That is guaranteed to be an underestimate. (Doing so gives you an implementation of Dijkstra’s Algorithm.)

Third, you seem to be doing an awful lot of sorting in this implementation. Surely you could be using a priority queue; that’s a lot cheaper than sorting.

Fourth, I have an implementation of A* in C# 3 on my blog that you are welcome to use; no warranty expressed or implied, use at your own risk.

http://blogs.msdn.com/b/ericlippert/archive/tags/astar/

My code is very straightforward; the algorithm in my implementation looks like this:

var closed = new HashSet<Node>();
var queue = new PriorityQueue<double, Path<Node>>();
queue.Enqueue(0, new Path<Node>(start));
while (!queue.IsEmpty)
{
    var path = queue.Dequeue();
    if (closed.Contains(path.LastStep)) continue;
    if (path.LastStep.Equals(destination)) return path;
    closed.Add(path.LastStep);
    foreach(Node n in path.LastStep.Neighbours)
    {
        double d = distance(path.LastStep, n);
        var newPath = path.AddStep(n, d);
        queue.Enqueue(newPath.TotalCost + estimate(n), newPath);
    }
}

The idea is that we maintain a priority queue of paths; that is, a queue of paths that is always able to tell you the path so far with the least distance. Then we check to see if we’ve made it to our destination; if so, we’re done. If not, then we enqueue a bunch of new paths based on their (under)estimated distance to the goal.

Fifth, that pseudocode in Wikipedia could be improved. The fact that my actual code is in many ways easier to follow than the pseudocode indicates that maybe there is too much detail in the pseudocode.