/// <summary>
/// Computes a distance table for the given node.
/// </summary>
/// <param name="source">The source node.</param>
/// <returns>The computed distance table.</returns>
public DistanceTable GetDistanceTable(Node source)
{
var agenda = new List <Node>();
var table = new DistanceTable(source);
foreach (var v in source.ParentGraph.Nodes)
{
table.Distances[v] = double.PositiveInfinity;
table.Previous[v] = null;
agenda.Add(v);
}
table.Distances[source] = 0;
while (agenda.Count > 0)
{
var current = agenda.Aggregate((min, n) => table.Distances[n] < table.Distances[min] ? n : min);
agenda.Remove(current);
foreach (var edge in current.OutgoingEdges)
{
double newDistance = table.Distances[current] + _getDistance(edge);
var next = edge.GetOtherNode(current);
if (newDistance < table.Distances[next])
{
table.Distances[next] = newDistance;
table.Previous[next] = current;
}
}
}
return(table);
}
/// <inheritdoc />
public IList <Node> FindPath(Node source, Node destination)
{
var table = new DistanceTable(source);
var closedSet = new HashSet <Node>();
var openSet = new HashSet <Node> {
source
};
var heuristics = new Dictionary <Node, double> {
[source] = _heuristic(source, destination)
};
while (openSet.Count > 0)
{
var current = openSet.Aggregate((min, n) => heuristics[n] < heuristics[min] ? n : min);
if (current == destination)
{
return(table.GetShortestPath(destination));
}
openSet.Remove(current);
closedSet.Add(current);
foreach (var edge in current.OutgoingEdges)
{
var neighbour = edge.GetOtherNode(current);
if (closedSet.Contains(neighbour))
{
continue;
}
double newDistance = table.Distances.GetOrDefault(current, double.PositiveInfinity) + _getDistance(edge);
if (!openSet.Add(neighbour) &&
newDistance >= table.Distances.GetOrDefault(neighbour, double.PositiveInfinity))
{
continue;
}
table.Previous[neighbour] = current;
table.Distances[neighbour] = newDistance;
heuristics[neighbour] = newDistance + _heuristic(neighbour, destination);
}
}
return(table.GetShortestPath(destination));
}
