Leniel Maccaferri's blog: A* pathfinding search in C#

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");
}