Fabulous Adventures In Coding : AStar

Path Finding Using A* in C# 3.0, Part Four

Eric Lippert — Wed, 10 Oct 2007 17:17:00 GMT

Finally we are ready to translate our pseudocode into C# 3.0.

What do we need to make the algorithm run? We need a start node, a destination node, a function which tells us the exact distance between two neighbours, and a function which tells us the estimated distance between the last node on a proposed path and the destination node.

We also need the ability to take any node and get a list of its neighbours. We'll represent that by an interface:

interface IHasNeighbours<N>
{
IEnumerable<N> Neighbours { get; }
}

Recall that the pseudocode looked like this:

closed = {}
q = emptyqueue;
q.enqueue(0.0, makepath(start))
while q is not empty
    p = q.dequeueCheapest
    if closed contains p.last then continue;
    if p.last == destination then return p
    closed.add(p.last)
    foreach n in p.last.neighbours
        newpath = p.continuepath(n)
        q.enqueue(newpath.TotalCost + estimateCost(n, destination), newpath)
return null

And now we're all set. The translation from pseudocode to real code is quite straightforward:

static public Path<Node> FindPath<Node>(
    Node start,
    Node destination,
    Func<Node, Node, double> distance,
    Func<Node, double> estimate)
    where Node : IHasNeighbours<Node>
{
    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);
        }
    }
    return null;
}

You might wonder why we check to see if the current-best-path ends in a "closed" node after the dequeue, rather than testing that before we do the enqueue. Think about it this way: suppose we take the best path off the queue, note that its end node is "closed" so that we don't come back to it, and put all of its extensions on the queue. There might be a number of other paths which end in that node presently on the queue. All of them must be, by definition, worse than the path we just took off the priority queue. We should therefore not even consider them as possbilities anymore. If we happen to put more paths with that property on the queue, that's unfortunate. But when we eventually get to them we'll discard them automatically so its not a big deal.

A nice property of the A* algorithm is that it finds the optimal path in a reasonable amount of time provided that:

the estimating function never overestimates the distance to the goal. (Think about what happens if the estimating function sometimes overestimates the distance; if the optimal path is one of the ones overestimated then it will possibly not make it to the front of the priority queue before a nonoptimal solution is found.)
calling the estimating function does not take very long.

One way to ensure that the estimating function never overestimates the distance is to always estimate zero. If you do so then this becomes Dijkstra's algorithm. However, the better your estimating function can get without going over (this really should have been called the "The Price Is Right" algorithm...) the faster this will identify the truly optimal path.

Unfortunately, the space complexity of this algorithm can be really quite high on complicated graphs. There are more complex versions of this algorithm which can deal with the space complexity, but I'm not going to go there.

I have a nice example of using A* to find the shortest path around a lake (but not the longest shortest path!) but I'm not going to post it here until Orcas ships. I'll just put up the whole project file at that time and you can check it out all at once.

Next time: at long last, Eric talks about variance in C#.

Path Finding Using A* in C# 3.0, Part Three

Eric Lippert — Mon, 08 Oct 2007 17:03:00 GMT

In order to make the A* algorithm work we need to get the lowest-estimated-cost-path-discovered-so-far out of the list of paths under consideration. The standard data structure for doing so is called a “priority queue”. Priority queues are so-called because they are typically used to store a list of jobs where each job has an associated priority.

Now, there’s a bit of a semantic problem with priority queues in that we typically think of “priority 1” as being higher priority than “priority 2”, even though 1 is a smaller number than 2. Fortunately for us, that is exactly what we want in this case; the “highest priority” path is the one with the least estimated cost.

There are lots of ways to implement priority queues – I have a nice example of an immutable priority queue that I’ll probably blog about at some point, but for now, let’s go with an old-fashioned mutable priority queue.

A priority queue can be implemented as list of sub-queues with the list sorted in priority order. The implementation is pretty straightforward:

class PriorityQueue<P, V>
{
    private SortedDictionary<P, Queue<V>> list = new SortedDictionary<P, Queue<V>>();
    public void Enqueue(P priority, V value)
    {
        Queue<V> q;
        if (!list.TryGetValue(priority, out q))
        {
            q = new Queue<V>();
            list.Add(priority, q);
        }
        q.Enqueue(value);
    }
    public V Dequeue()
    {
        // will throw if there isn’t any first element!
        var pair = list.First();
        var v = pair.Value.Dequeue();
        if (pair.Value.Count == 0) // nothing left of the top priority.
            list.Remove(pair.Key);
        return v;
    }
    public bool IsEmpty
    {
        get { return !list.Any(); }
    }
}

We could implement IEnumerable<V>, a Peek operation, etc, but this is all we need for A*, so let’s not go crazy here.

Next time: now we have all the parts we need to implement A*, so let's do it already.

Path Finding Using A* in C# 3.0, Part Two

Eric Lippert — Thu, 04 Oct 2007 17:33:00 GMT

In order to implement the A* algorithm in C# 3.0 I am going to need to implement some custom data structures. Today we’ll consider how to implement the “path”.

You’ll notice in my description of the algorithm that the only operations we perform on paths are:

Make a new path consisting of a single node.
Make a new path by adding a new node onto the end of an existing path.
Fetch the last element on the path.
Get the total cost of a path.

This cries out to me “Eric! A path is an immutable stack!”

A mutable stack like System.Collections.Generic.Stack<T> is clearly not suitable. We want to be able to take an existing path and create new paths from it for all of the neighbours of its last element, but pushing a new node onto the standard stack modifies the stack. We’d have to make copies of the stack before pushing it, which is silly because then we’d be duplicating all of its contents unnecessarily.

Immutable stacks do not have this problem. Pushing onto an immutable stack merely creates a brand-new stack which links to the old one as its tail. Since the stack is immutable, there is no danger of some other code coming along and messing with the tail contents. You can keep on using the old stack to your heart’s content.

ASIDE: Immutable data structures are the way of the future in C#. It is much easier to reason about a data structure if you know that it will never change. Since they cannot be modified, they are automatically threadsafe. Since they cannot be modified, you can maintain a stack of past “snapshots” of the structure, and suddenly undo-redo implementations become trivial. On the down side, they do tend to chew up memory, but hey, that’s what garbage collection was invented for, so don’t sweat it. I’ll be talking more about programming using immutable data structures in this space over the next few months.

Let’s make an immutable stack of nodes which tracks the total cost of the whole path. Later on, we’ll see that we need to enumerate the nodes of this thing, so we’ll do a trivial implementation of IEnumerable while we’re at it. And since we don’t know exactly what a node will look like, let’s make the thing generic:

class Path<Node> : IEnumerable<Node>
{
    public Node LastStep { get; private set; }
    public Path<Node> PreviousSteps { get; private set; }
    public double TotalCost { get; private set; }
    private Path(Node lastStep, Path<Node> previousSteps, double totalCost)
    {
        LastStep = lastStep;
        PreviousSteps = previousSteps;
        TotalCost = totalCost;
    }
    public Path(Node start) : this(start, null, 0) {}
    public Path<Node> AddStep(Node step, double stepCost)
    {
        return new Path<Node>(step, this, TotalCost + stepCost);
    }
    public IEnumerator<Node> GetEnumerator()
    {
        for (Path<Node> p = this; p != null; p = p.PreviousSteps)
            yield return p.LastStep;
    }
    IEnumerator IEnumerable.GetEnumerator()
    {
        return this.GetEnumerator();
    }
}

Well, that was painless. Next time: implementing the priority queue.

Path Finding Using A* in C# 3.0, Part One

Eric Lippert — Wed, 03 Oct 2007 02:31:00 GMT

As we get into the home stretch for the Orcas release I’ve been spending a little time lately just playing around implementing some “classic” algorithms and data structures in C#. The one I implemented today is the famous A* algorithm.

Surely you’ve played games like Age of Empires. You know the kind of games I mean. There are a bunch of little dudes walking around a map containing barriers (mountains, walls, etc) and somehow the game’s pseudo-intelligence finds the shortest possible path for the guys to walk from point A to point B. Most games do this with a surprisingly straightforward algorithm.

Suppose you’ve got a bunch of “nodes”. Say, intersections in a city. Each node has a bunch of “neighbours” – intersections that can be immediately reached by locomoting down a particular street involved in the intersection. Each “edge” (ie, street) between nodes has a “cost” – the amount of time it takes to travel from one node to the neighbour on that street. Perhaps the cost is based on its length, traffic density, etc.

A “path from A to B” consists of a list of nodes starting with A, ending with B, where every step along the path is from neighbour to neighbour. The total cost of a path is obviously the sum of all the costs of every transition from neighbour to neighbour on the path.

Let’s start by considering a really stupid, awful, buggy path finding algorithm, and then we’ll make it better.

q = emptyqueue
q.enqueue(makepath(start))
while q is not empty
p = q.dequeue
if p.last == destination then return p
foreach n in p.last.neighbours
q.enqueue(p.continuepath(n))
// failed to find path.
return null

What’s wrong with this picture?

Well, suppose we have four nodes,arranged like this: A <--> C <--> D <--> B. Does this algorithm find the obvious path from A to B?

Run the code in your head: it starts by putting the path “A” on the queue. Then it takes it off and puts “A-C” on the queue. Then it takes it off and puts “A-C-A” and “A-C-D” on the queue. Then it takes “A-C-A” off the queue and puts “A-C-A-C” on the queue... OK, eventually we get there, but we seem to be spending a lot of time going in circles. And if D does not actually connect to B, if you can't get there from here, then we really do go into an infinite loop.

Clearly this is all screwed up. We need to disallow paths which loop back on themselves, because those are clearly not going to be the best possible path. We're at best wasting time looking at them, and at worst going into an infinite loop that chews up all the memory in the system.

Let’s fix that up. We’ll create a “closed” set, ie, the set of “nodes we have already considered, so do not loop back to them.”

closed = {}
q = emptyqueue;
q.enqueue(makepath(start))
while q is not empty
    p = q.dequeue
    if closed contains p.last then continue
    if p.last == destination then return p
    closed.add(p.last)
    foreach n in p.last.neighbours
        q.enqueue(p.continuepath(n))
return null

This is better, in that it actually terminates in a finite amount of time. But my goodness, is this algorithm ever slow. Essentially the algorithm is “At every intersection, check to see if you're at your destination. If not, pick a direction – any direction! - and walk as far as you can like that without revisiting your steps. If you ever run out of options, backtrack until you get to an intersection that goes somewhere you haven’t tried yet. Keep doing that until either you get to your destination or you have tried every possible intersection.”

I guarantee you that if you try that in a reasonably sized city, you will not get from point A to point B in any kind of reasonable time. And when you do succeed, the path which you come up with may well be absolutely crazy. Obviously this is not how people navigate between points in cities.

The problem is that we are picking which neighbour to try next essentially at random. We can do better than that. We want to direct our search so that the path we spend most of our time concentrating on is the path which is most likely to lead us to the goal.

Now, that sounds like a bit of a chicken-and-egg problem. Finding the best path is precisely what we are trying to do, so how can we determine which path is the one we want to be concentrating on?

We shall have to guess. We will rank all potential paths based on the sum of two factors:

First, what is the total cost of each potential path so far? This is known.
Second, what is the estimated cost of getting from the end of each potential path to the goal? This is a guess.

And now our algorithm becomes:

which is the famous A* algorithm! Surprisingly straightforward, eh?

Next time: actually implementing this thing in C# 3.0.