Leniel Maccaferri's blog: geocoder

Showing posts with label geocoder. 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");
}