public IDictionary<MapNode, double> GetDistance(MapNode source, IList<MapNode> dest)
{
distanceTo.Clear();
foreach (var n in map)
{
distanceTo[n] = double.MaxValue;
}
distanceTo[source] = 0.0;
pq.Clear();
var ret = new Dictionary<MapNode, double>();
pq.Enqueue(source, 0.0d);
while (pq.Count != 0)
{
var nodeToExam = pq.Dequeue();
Relax(nodeToExam);
if (dest.Contains(nodeToExam))
{
ret[nodeToExam] = distanceTo[nodeToExam];
if (ret.Count == dest.Count)
return ret;
}
}
return ret;
}
private void Relax(MapNode n)
{
var paths = map.AdjacentPath(n);
foreach (var p in paths)
{
double newDist = distanceTo[n] + p.Distance;
MapNode other = p.Other(n);
if (newDist < distanceTo[other])
{
distanceTo[other] = newDist;
pq[other] = newDist;
pathTo[other] = p;
}
}
}
public MapNode Other(MapNode nodeW)
{
if (nodeW.Equals(U)) return V;
else return U;
}