A* pathfinding search in C# - Part 1

A* pathfinding search in C# - Part 2
A* pathfinding search in C# - Part 3

Code available at GitHub: https://github.com/leniel/AStar

Last Sunday after I got back from church I read Kirill Osenkov’s post Algorithms in C#: shortest path around a polygon (polyline routing). Kirill mentions Eric Lippert’s excellent series of posts titled Path Finding Using A* in C# 3.0. I’ve read that series of posts a long time ago (2007 - I was on the last year of the computer engineering course).

See you how life is interesting: in 2007 after reading Lippert’s posts I wanted to see a running sample of the code but didn’t try to implement that until the time I read Osenkov’s post. Why did it took so long? I don’t know how to answer this.

I studied A* (A star) in the 9th term of the computer engineering course  (Feb-Jun, 2007). The discipline was Artificial Intelligence. At that time I didn’t implement any code for this specific algorithm (I think this is due the so famous excuse during the classes - we just don’t have enough time to do that. We have lots of topics to see yet.) . Therefore, we’ve just run test cases on sheets of paper. Not so good but a start.

Well, Eric wrote the base structure of the A* pathfinding algorithm but didn’t provide a complete running sample. He wrote the following:

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.

Still on Sunday night I took the time to write some code that put up a running A* search.

I basically merged some code I had already posted on the series of posts titled Breadth and depth first search and created some properties here and there to get the whole thing running. It’s nothing more than code reuse.

The following is the basic project’s structure I created:

In Path Finding Using A* in C# 3.0, Part One the A* algorithm is described.

In Path Finding Using A* in C# 3.0, Part Two the data structures necessary to implement the A* algorithm are defined.

In this part Eric writes:

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.

In part two we are presented to an immutable stack that I’ve put inside the Path class.

In Path Finding Using A* in C# 3.0, Part Three we are presented to a priority queue. This structure is responsible for putting the best paths so far in priority order, that is, the path with a lesser cost has a major priority.

In this part Eric writes:

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.

The PriorityQueue lies within its namesake class.

In Path Finding Using A* in C# 3.0, Part Four we get the FindPath method that is the one responsible for finding the shortest path possible. This method uses all of the remaining classes shown on the project structure above.

I put the FindPath method inside the AStar class.

In this part Eric writes:

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.

This is the beautiful PathFind method:

static public Path<TNode> FindPath<TNode>(
    TNode start,
    TNode destination,
    Func<TNode, TNode, double> distance,
    Func<TNode, double> estimate) where TNode : IHasNeighbours<TNode>
{
    var closed = new HashSet<TNode>();

    var queue = new PriorityQueue<double, Path<TNode>>();

    queue.Enqueue(0, new Path<TNode>(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(TNode 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;
}

When you get at the fourth part of Eric’s posts you may get thinking: How can I run this thing? That’s what I try to explain in this post.

The immutable stack implemented in part two of Eric’s posts is a generic one, that is, it accepts an object custom made to represent a Node in the path.

I already had a class Node that I used sometime ago while studying Artificial Intelligence. Take a look at Breadth and depth first search series of posts.

I also needed a Graph that represents the map formed with the connections between the nodes.

The Graph has a NodeList that stores all the nodes currently in the graph.

A node can have neighbors, that is, other nodes connected to it and such connections are represented in the code through the class AdjacencyList. Each node has an AdjacencyList that stores its neighbors.

A connection (edge) between two nodes (vertexes) is represented by the EdgeToNeighbor class that stores the neighbor of a node and the cost to get to that neighbor.

The AdjacencyList, EdgeToNeighbor, Graph, Node and NodeList classes are the outcome of an excellent series of articles by Scott Mitchell titled An Introduction to Data Structures. I got those classes and added them to the project.

This way I had all set to pass in the first two parameters of the PathFind method.

Now I needed to figure out how to create the distance and estimate functions to pass as the third and fourth parameters.

For the distance function I did this:

// Function which tells us the exact distance between two neighbours.
Func<Node, Node, double> distance = (node1, node2) => node1.Neighbors.Cast<EdgeToNeighbor>().Single(etn => etn.Neighbor.Key == node2.Key).Cost;

The distance function receives two nodes as parameters as can be seen in the PathFind method and returns a double value. I then use a lambda expression to find the Cost, (distance) in this case, between the two nodes. I get the neighbors of node1 and cast those to EdgeToNeighbor and execute a query using a lambda expression again to return the Cost only if the key of one of the neighbors of node1 is equal to the key of node2.

For the estimation function I did this:

// Estimation/Heuristic function (Haversine distance)
// It tells us the estimated distance between the last node on a proposed path and the destination node.
Func<Node, double> haversineEstimation = n => Haversine.Distance(n, destination, DistanceType.Kilometers);

Now I’m using as the estimation the Haversine formula. This formula gives the great-circle distances between two points on a sphere (Earth) from their longitudes and latitudes.

I got the Haversine formula code from the post Haversine Formula in C# and in SQL by Seth Long. I just adapted it a little bit so that I could use it with the Node data structure.

The haversine generic delegate receives as input a Node and return a double. Using a lambda expression I get the Haversine distance from node n to the destination node and I set that I want the distance in Kilometers.

With the distance and estimate functions on hand we’re OK to call the PathFind method.
Wait! Don’t we need something before that? Of course we need. We need to populate the graph with nodes. After all we’re after the shortest path between two nodes and we need some nodes to test the A* star pathfinding search. :-)

Next time: I’ll show you how to create a complete scenario using the same Romania map I used in the Breadth and depth first search series of posts.

References
[1] Lippert, Eric. Path Finding Using A* in C# 3.0, Part One. 2007. Available at <http://blogs.msdn.com/ericlippert/archive/2007/10/02/path-finding-using-a-in-c-3-0.aspx>. Accessed on June 13, 2009.

[2] Lippert, Eric. Path Finding Using A* in C# 3.0, Part Two. 2007. Available at <http://blogs.msdn.com/ericlippert/archive/2007/10/04/path-finding-using-a-in-c-3-0-part-two.aspx>. Accessed on June 13, 2009.

[3] Lippert, Eric. Path Finding Using A* in C# 3.0, Part Three. 2007. Available at <http://blogs.msdn.com/ericlippert/archive/2007/10/08/path-finding-using-a-in-c-3-0-part-three.aspx>. Accessed on June 13, 2009.

[4] Lippert, Eric. Path Finding Using A* in C# 3.0, Part Four. 2007. Available at <http://blogs.msdn.com/ericlippert/archive/2007/10/10/path-finding-using-a-in-c-3-0-part-four.aspx>. Accessed on June 13, 2009.

[5] Mitchell, Scott. An Extensive Examination of Data Structures. 2005. Available at <http://msdn.microsoft.com/en-us/library/ms379570(VS.80).aspx>. Accessed on June 13, 2009.

[6] Long, Seth. Haversine Formula in C# and in SQL. 2008. Available at <http://megocode3.wordpress.com/2008/02/05/haversine-formula-in-c>. Accessed on June 13, 2009.