/// <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 Caminho <TNode> FindPath <TNode>(
TNode start,
TNode destination,
Func <TNode, TNode, double> distance,
Func <TNode, double> estimate) where TNode : IVizinhos <TNode>
{
var closed = new HashSet <TNode>();
var queue = new FilaPrioridade <double, Caminho <TNode> >();
queue.Enqueue(0, new Caminho <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);
}
/// <summary>
/// This method can be used to view the other paths inside the FilaPrioridade.
/// </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>(FilaPrioridade <double, Caminho <TNode> > queue, Func <TNode, double> estimate)
{
Console.WriteLine("\nCaminhos possíveis:\n");
// 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 <Caminho <TNode> > > kvp in queue)
{
// For each path in the Queue...
foreach (Caminho <TNode> otherPath in kvp.Value)
{
// Reverse the Caminho so that we get the order of the cities in a more meaningful way...
var otherPathReversed = otherPath.Cast <Caminho <Node> >().Reverse();
// Prints on screen the Cities that are part of this path.
foreach (Caminho <Node> path in otherPathReversed)
{
if (path.PreviousSteps != null)
{
Console.WriteLine(string.Format("De {0, -14} para {1, -14} -> Custo total = {2:#.###} {3}",
path.PreviousSteps.LastStep.Key, path.LastStep.Key, path.TotalCost, DistanceType.km));
}
}
// Get the estimation cost of the other possible 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("Estimativa = {0:0.###} {1}", otherPathEstimation, DistanceType.km);
Console.WriteLine("Custo do caminho = {0:0.###} {1} = (Custo total + Estimativa)", kvp.Key, DistanceType.km);
}
Console.WriteLine();
}
}
