Leniel Maccaferri's blog: Haversine

Showing posts with label Haversine. Show all posts

A* pathfinding search in C# - Part 3

Posted by Leniel Maccaferri on 12/02/2009 12:30:00 AM

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

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

This is the last installment in the series about A* (A star) search.

The C# source code implemented is available in the final part of this post.

As promised in the last words of A* pathfinding search in C# - Part 2 today we’re gonna run a test case using the Romania map.

Romania map

If you want to understand the whole process implemented in this solution, please start reading A* pathfinding search in C# - Part 1.

When you run the console application, you get the following screen:

A* Search console application

You start by entering a Start and a Destination city picking up the ones you want from the list of Romania cities.

When you press Enter the console app will show you the shortest or best path based on the A* search algorithm.

As you can see in the above screenshot, the app shows us that the best path to go from Arad to Bucharest is the one that goes as follows:

Note that the Total cost is the cost calculated so far for each path, that is, in the example shown above, Total cost = 348.536 km is the distance in kilometers for travelling from Arad to Pitesti.

No doubt this is the shortest path to follow if you plan to go from Arad to Bucharest. We could choose different possible routes but the total distance traveled would be greater than the one the app calculated for the shortest path. Let’s see why this is so using the method ViewOtherPaths (I commented about it in A* pathfinding search in C# - Part 2).

The following is the output of the console app when the method ViewOtherPaths is uncommented inside the FindPath method. This helps you debug and see why the app has chosen the above shortest path.

A* Search - Sample implementation by Leniel Macaferi, June 7-20, 2009

These are the Cities you can choose as Start and Destination in Romania:

Arad
Bucharest
Craiova
Dobreta
Eforie
Fagaras
Giurgiu
Hirsova
Iasi
Lugoj
Mehadia
Neamt
Oradea
Pitesti
Rimnicu Vilcea
Sibiu
Timisoara
Urziceni
Vaslui
Zerind

Enter a Start city: Arad

Enter a Destination city: Bucharest

Possible paths:

From Arad to Sibiu -> Total cost = 223.236 km
Estimation = 213.803 km
Priority Queue Cost = 437.039 km = (Total cost + Estimation)

From Arad to Timisoara -> Total cost = 48.459 km
Estimation = 408.79 km
Priority Queue Cost = 457.249 km = (Total cost + Estimation)

From Arad to Zerind -> Total cost = 51.908 km
Estimation = 431.034 km
Priority Queue Cost = 482.942 km = (Total cost + Estimation)

Possible paths:

From Arad           to Sibiu          -> Total cost = 223.236 km
From Sibiu          to Rimnicu Vilcea -> Total cost = 301.317 km
Estimation          = 154.102 km
Priority Queue Cost = 455.419 km = (Total cost + Estimation)

From Arad to Timisoara -> Total cost = 48.459 km
Estimation = 408.79 km
Priority Queue Cost = 457.249 km = (Total cost + Estimation)

From Arad           to Sibiu          -> Total cost = 223.236 km
From Sibiu          to Fagaras        -> Total cost = 287.59 km
Estimation          = 178.296 km
Priority Queue Cost = 465.886 km = (Total cost + Estimation)

From Arad to Zerind -> Total cost = 51.908 km
Estimation = 431.034 km
Priority Queue Cost = 482.942 km = (Total cost + Estimation)

From Arad           to Sibiu          -> Total cost = 223.236 km
From Sibiu          to Lugoj          -> Total cost = 397.029 km
Estimation          = 356.126 km
Priority Queue Cost = 753.155 km = (Total cost + Estimation)

From Arad           to Sibiu          -> Total cost = 223.236 km
From Sibiu          to Arad           -> Total cost = 446.473 km
Estimation          = 420.536 km
Priority Queue Cost = 867.009 km = (Total cost + Estimation)

From Arad           to Sibiu          -> Total cost = 223.236 km
From Sibiu          to Oradea         -> Total cost = 444.358 km
Estimation          = 434.745 km
Priority Queue Cost = 879.104 km = (Total cost + Estimation)

Possible paths:

From Arad           to Sibiu          -> Total cost = 223.236 km
From Sibiu          to Rimnicu Vilcea -> Total cost = 301.317 km
From Rimnicu Vilcea to Pitesti        -> Total cost = 348.536 km
Estimation          = 107.572 km
Priority Queue Cost = 456.108 km = (Total cost + Estimation)

From Arad to Timisoara -> Total cost = 48.459 km
Estimation = 408.79 km
Priority Queue Cost = 457.249 km = (Total cost + Estimation)

From Arad to Zerind -> Total cost = 51.908 km
Estimation = 431.034 km
Priority Queue Cost = 482.942 km = (Total cost + Estimation)

From Arad           to Sibiu          -> Total cost = 223.236 km
From Sibiu          to Rimnicu Vilcea -> Total cost = 301.317 km
From Rimnicu Vilcea to Craiova        -> Total cost = 400.614 km
Estimation          = 183.042 km
Priority Queue Cost = 583.656 km = (Total cost + Estimation)

From Arad           to Sibiu          -> Total cost = 223.236 km
From Sibiu          to Rimnicu Vilcea -> Total cost = 301.317 km
From Rimnicu Vilcea to Sibiu          -> Total cost = 379.398 km
Estimation          = 213.803 km
Priority Queue Cost = 593.201 km = (Total cost + Estimation)

From Arad           to Sibiu          -> Total cost = 223.236 km
From Sibiu          to Rimnicu Vilcea -> Total cost = 301.317 km
From Rimnicu Vilcea to Mehadia        -> Total cost = 461.891 km
Estimation          = 299.853 km
Priority Queue Cost = 761.744 km = (Total cost + Estimation)

From Arad           to Sibiu          -> Total cost = 223.236 km
From Sibiu          to Rimnicu Vilcea -> Total cost = 301.317 km
From Rimnicu Vilcea to Lugoj          -> Total cost = 504.328 km
Estimation          = 356.126 km
Priority Queue Cost = 860.454 km = (Total cost + Estimation)

Possible paths:

From Arad           to Sibiu          -> Total cost = 223.236 km
From Sibiu          to Rimnicu Vilcea -> Total cost = 301.317 km
From Rimnicu Vilcea to Pitesti        -> Total cost = 348.536 km
From Pitesti        to Bucharest      -> Total cost = 456.108 km
Estimation          = 0 km
Priority Queue Cost = 456.108 km = (Total cost + Estimation)

From Arad to Timisoara -> Total cost = 48.459 km
Estimation = 408.79 km
Priority Queue Cost = 457.249 km = (Total cost + Estimation)

From Arad to Zerind -> Total cost = 51.908 km
Estimation = 431.034 km
Priority Queue Cost = 482.942 km = (Total cost + Estimation)

From Arad           to Sibiu          -> Total cost = 223.236 km
From Sibiu          to Rimnicu Vilcea -> Total cost = 301.317 km
From Rimnicu Vilcea to Pitesti        -> Total cost = 348.536 km
From Pitesti        to Rimnicu Vilcea -> Total cost = 395.755 km
Estimation          = 154.102 km
Priority Queue Cost = 549.858 km = (Total cost + Estimation)

From Arad           to Sibiu          -> Total cost = 223.236 km
From Sibiu          to Rimnicu Vilcea -> Total cost = 301.317 km
From Rimnicu Vilcea to Pitesti        -> Total cost = 348.536 km
From Pitesti        to Craiova        -> Total cost = 452.104 km
Estimation          = 183.042 km
Priority Queue Cost = 635.146 km = (Total cost + Estimation)

From Arad           to Sibiu          -> Total cost = 223.236 km
From Sibiu          to Rimnicu Vilcea -> Total cost = 301.317 km
From Rimnicu Vilcea to Pitesti        -> Total cost = 348.536 km
From Pitesti        to Fagaras        -> Total cost = 458.356 km
Estimation          = 178.296 km
Priority Queue Cost = 636.653 km = (Total cost + Estimation)

This is the shortest path based on the A* Search Algorithm:

Do you wanna try A* Search again? Yes or No?

A small change
One thing I changed in the code I posted on A* pathfinding search in C# - Part 2 was the foreach that enumerates the shortest path to write it on the screen. Before it read:

// Prints the shortest path.
foreach(Node n in shortestPath.Reverse())
{
    Console.WriteLine(n.Key);
}

Now it reads:

// Prints the shortest path.
foreach(Path<Node> path in shortestPath.Reverse())
{
    if(path.PreviousSteps != null)
    {
        Console.WriteLine(string.Format("From {0, -15}  to  {1, -15} -> Total cost = {2:#.###} {3}",
                          path.PreviousSteps.LastStep.Key, path.LastStep.Key, path.TotalCost, distanceType));
    }
}

As you can see I changed from Node to Path<Node>. To get this working I had to change the type returned by GetEnumerator in the class Path so that it returned Path<Node> instead of Node.

public IEnumerator<Path<Node>> GetEnumerator()
{
    for(Path<Node> p = this; p != null; p = p.PreviousSteps)
        yield return p;
}

This allowed me to enumerate over each path that composes the whole shortest path so that we can show the LastStep of the previous path and the LastStep of the current path. The Total cost travelled so far for each path is also available because we’re working with a path object.

Last note
A* is a really powerful search algorithm.

Hope you liked this series of posts about A* search as I liked to implement and write about it! It was a really good programming exercise.

Visual Studio 2013 Solution with C# Console Application Project
You can get the Microsoft Visual Studio Project at this GitHub repository:

https://github.com/leniel/AStar

To try out the code you can use the free Microsoft Visual C# Express Edition that you can get at: http://www.microsoft.com/express/vcsharp/

A* pathfinding search in C# - Part 2

Posted by Leniel Maccaferri on 6/20/2009 07:07:00 PM

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

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

To illustrate the path finding problem and how it can be solved using A* (A star) I decided to use the Romania map (with the same Cities I used in Breadth and depth first search series of posts). Now I modified it adding more connections between the cities so that we have more fun while debugging the code.

This time I decided to get the real values of latitude and longitude for the cities shown in the Romania map. To accomplish this I used this Google Spreadsheet that is a geocoder. It uses geo-google-docs to get geolocalization data for the address column.

This is the Map created using the data stored in the spreadsheet with the help of a Google Map:

You can pass your mouse over each pin to see the name of the city it represents in the map.

The following picture shows the fictitious connections (paths) between the cities and the cost to move from city to city.

To fill the Graph that represents the Romania map above I created a static private method called FillGraphWithEarthMap inside the AStar class that does the job of creating the Nodes and adding them to the graph (bellow I show a shortened version of such a method because it is a really big method given the fact that we have 20 cities and 41 connections (paths) between them:

/// <summary>
/// Fills a Graph with Romania map information.
/// The Graph contains Nodes that represents Cities of Romania.
/// Each Node has as its key the City name and its Latitude and Longitude associated information.
/// Nodes are vertexes in the Graph. Connections between Nodes are edges.
/// </summary>
/// <param name="graph">The Graph to be filled</param>
/// <param name="distanceType">The DistanceType (KM or Miles) between neighbor cities</param>
private static void FillGraphWithEarthMap(Graph graph, DistanceType distanceType)
{
    // 20 Vertexes in total
    Node arad = new Node("Arad", null, 46.1792414, 21.3150154); // Creating a Node...
    graph.AddNode(arad); // Adding the Node to the Graph...

    Node bucharest = new Node("Bucharest", null, 44.4479237, 26.097879);
    graph.AddNode(bucharest);

    Node craiova = new Node("Craiova", null, 44.3182085, 23.8016427);
    graph.AddNode(craiova);

    .
    .
    .

    // 41 Edges in total
    // Arad <-> Sibiu
    graph.AddUndirectedEdge(arad, sibiu, Haversine.Distance(arad, sibiu, distanceType));
    // Arad <-> Timisoara
    graph.AddUndirectedEdge(arad, timisoara, Haversine.Distance(arad, timisoara, distanceType));
    // Arad <-> Zerind
    graph.AddUndirectedEdge(arad, zerind, Haversine.Distance(arad, zerind, distanceType));

    .
    .
    .
}

The Vertexes
You must create a vertex/Node for each City of the map, passing to the Node’s class constructor the name of the City and its Latitude and Longitude coordinates. Add each Node to the Graph passed as a parameter to the method.

The Edges
You must create an undirected edge/path for each connection between two cities, passing to the constructor the first city, the second city and the cost to go from one city to the other. In this case the cost is calculated through the Haversine class discussed in A* pathfinding search in C# - Part 1.

To help debug the code I created two methods: DistanceBetweenNodes and ViewOtherPaths. Both methods are kept in the AStar class.

This is the DistanceBetweenNodes method:

/// <summary>
/// Prints on screen the distance from a city to its neighbors.
/// </summary>
/// <param name="graph">The Graph</param>
/// <param name="distanceType">The distance type: KM or Miles</param>
private static void DistanceBetweenNodes(Graph graph, DistanceType distanceType)
{
    // First we cast the Graph.Nodes which is a NodeList to type Node and then we order the list of Nodes by the Node Key
    // so that we get the list of cities in ascending order.
    foreach(Node n in graph.Nodes.Cast<Node>().OrderBy(n => n.Key))
    {
        // For each city neighbor we gets its information and print it on screen.
        foreach(EdgeToNeighbor etn in n.Neighbors)
        {
            Console.WriteLine("Distance from {0} to {1} is -> {2:#.##} {3}", n.Key, etn.Neighbor.Key, etn.Cost, distanceType);
        }
    }
}

Inside the FindPath method presented on A* pathfinding search in C# - Part 1, I call the ViewOtherPaths method (it’s commented out):

/// <summary>
/// This is the method responsible for finding the shortest path between a Start and Destination cities using the A*
/// search algorithm.
/// </summary>
/// <typeparam name="TNode">The Node type</typeparam>
/// <param name="start">Start city</param>
/// <param name="destination">Destination city</param>
/// <param name="distance">Function which tells us the exact distance between two neighbours.</param>
/// <param name="estimate">Function which tells us the estimated distance between the last node on a proposed path and the
/// destination node.</param>
/// <returns></returns>
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);
        }

        //ViewOtherPaths(queue, estimate);
    }

    return null;
}

This is the ViewOtherPaths method:

/// <summary>
/// This method can be used to view the other paths inside the PriorityQueue.
/// </summary>
/// <typeparam name="TNode">The Node type</typeparam>
/// <param name="queue">The priority queue</param>
/// <param name="estimate">Function which tells us the estimated distance between the last node on a proposed path and the
/// destination node.</param>
private static void ViewOtherPaths<TNode>(PriorityQueue<double, Path<TNode>> queue, Func<TNode, double> estimate)
{
    // The priority queue is composed of KeyValuePairs which has as key a double value (the TotalCost) and
    // has as Value a Queue which contains Paths.
    foreach(KeyValuePair<double, Queue<Path<TNode>>> kvp in queue)
    {
        // For each path in the Queue...
        foreach(Path<TNode> otherPath in kvp.Value)
        {
            // Reverse the Path so that we get the order of the cities in a more meaningful way...
            var otherPathReversed = otherPath.Cast<Node>().Reverse();

            // Prints on screen the Cities that are part of this path.
            foreach(Node n in otherPathReversed)
            {
                Console.WriteLine(n.Key);
            }

            // Get the total cost of the other path.
            double otherPathTotalCost = otherPath.TotalCost;
            // Get the estimation cost of the other path.
            double otherPathEstimation = estimate(otherPath.LastStep);

            // Prints on the screen the relevant information so that it gets easier to debug the code and see how
            // the A* search algorithm really does the job...
            Console.WriteLine("Total Cost other path = {0}", otherPathTotalCost);
            Console.WriteLine("Estimation other path = {0}", otherPathEstimation);
            Console.WriteLine(@"Priority Queue Cost other path = {0} =
                            Total Cost other path + Estimation other path =
                            {1}", kvp.Key, otherPathTotalCost + otherPathEstimation);
        }

        Console.WriteLine();
    }

    Console.WriteLine();
}

I also use two additional methods to get the Start and Destination cities entered by the user when s/he runs the code. Both methods keep a loop until the user enters the name of a city that is indeed part of the Graph we’ve just created:

/// <summary>
/// Gets the Destination city.
/// </summary>
/// <param name="graph">The Graph</param>
/// <returns>Name of Destination city</returns>
private static string GetDestinationCity(Graph graph)
{
    string destinationCity;

    do
    {
        Console.Write("\nEnter a Destination city: ");

        destinationCity = Console.ReadLine();
    }
    while(!graph.Nodes.ContainsKey(destinationCity));

    return destinationCity;
}

/// <summary>
/// Gets the Destination city.
/// </summary>
/// <param name="graph">The Graph</param>
/// <returns>Name of Destination city</returns>
private static string GetDestinationCity(Graph graph)
{
    string destinationCity;

    do
    {
        Console.Write("\nEnter a Destination city: ");

        destinationCity = Console.ReadLine();
    }
    while(!graph.Nodes.ContainsKey(destinationCity));

    return destinationCity;
}

This is the main entry point of the Console Application I created:

static void Main(string[] args)
{
    do
    {
        // Creating the Graph...
        Graph graph = new Graph();

        //FillGraphWithGridMap(graph);

        FillGraphWithEarthMap(graph, DistanceType.Kilometers);

        // Prints on the screen the distance from a city to its neighbors.
        // Used mainly for debug information.
        // DistanceBetweenNodes(graph, DistanceType.Kilometers);

        Console.WriteLine("A* Search - Sample implementation by Leniel Macaferi, June 7-20, 2009\n");
        Console.WriteLine("These are the Cities you can choose as Start and Destination in Romania: \n");

        // Prints on screen the cities that you can choose as Start and Destination.
        foreach(Node n in graph.Nodes.Cast<Node>().OrderBy(n => n.Key))
        {
            Console.WriteLine(n.Key);
        }

        string startCity = GetStartCity(graph);

        string destinationCity = GetDestinationCity(graph);

        Node start = graph.Nodes[startCity];

        Node destination = graph.Nodes[destinationCity];

        // 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;

        // Estimation/Heuristic function (Manhattan distance)
        // It tells us the estimated distance between the last node on a proposed path and the destination node.
        //Func<Node, double> manhattanEstimation = n => Math.Abs(n.X - destination.X) + Math.Abs(n.Y - destination.Y);

        // 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);

        //Path<Node> shortestPath = FindPath(start, destination, distance, manhattanEstimation);
        Path<Node> shortestPath = FindPath(start, destination, distance, haversineEstimation);

        // Prints the shortest path.
        foreach(Node n in shortestPath.Reverse())
        {
            Console.WriteLine(n.Key);
        }

        Console.Write("\nDo you wanna try A* Search again? Yes or No? ");
    }
    while(Console.ReadLine().ToLower() == "yes");
}

The main entry point above calls all the other methods I showed in this post.

This is all you need to run the A* pathfinding search algorithm! :-)

I think the code is well documented through comments.

I wish this helps you understand how to put together the working pieces necessary to run a test case.

Next time: I’ll run a test case using the Graph we have assembled in this post.

A* pathfinding search in C# - Part 1

Posted by Leniel Maccaferri on 6/13/2009 10:33:00 PM

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:

A* Search Solution Explorer

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.

Leniel Maccaferri's blog

Pages

A* pathfinding search in C# - Part 3

A* pathfinding search in C# - Part 2

A* pathfinding search in C# - Part 1

Contact Form

TOP Posts

Archive

Music