public DijkstraFindAllPathsResponse <TNode> FindAllPaths(TNode start)
{
InitSets(start);
while (unexploredSet.Count > 0)
{
TNode current = unexploredSet.Pool();
double distanceFromNode = distanceFromStartToNode.GetValueOrDefault(current);
foreach (TNode neightboar in heuristicImpl.GetNeighboars(current))
{
double distanceFromStart = distanceFromNode + heuristicImpl.VertexLength(current, neightboar);
double distanceToN = distanceFromStartToNode.ContainsKey(neightboar) ? distanceFromStartToNode.GetValueOrDefault(neightboar) : double.MaxValue;
if (distanceFromStart < distanceToN)
{
ReplaceInDictionary(distanceFromStartToNode, neightboar, distanceFromStart);
ReplaceInDictionary(nodeToParent, neightboar, current);
if (!unexploredSet.Contains(neightboar))
{
unexploredSet.Add(neightboar, distanceFromStart);
}
}
}
}
return(new DijkstraFindAllPathsResponse <TNode>
{
distances = distanceFromStartToNode,
nodeToParent = nodeToParent
});
}
public IEnumerable <TNode> FindPath(TNode start, TNode goal)
{
InitializeSets(start, goal);
while (notEvaluatedNodes.Count > 0)
{
TNode current = notEvaluatedNodes.Peek();
if (current.CompareTo(goal) == 0)
{
// TODO reconstruct_path
return(ReconstructPath(current));
}
notEvaluatedNodes.Pool();
nodesVisited.Add(current, true);
foreach (TNode neighboar in heuristicImpl.GetNeighboars(current))
{
if (nodesVisited.ContainsKey(neighboar))
{
continue;
}
// compute heuristic cost
double tentativeDistance = costFromStartToNode.GetValueOrDefault(current) + heuristicImpl.HeuristicDistanceBetween(current, neighboar);
// This is not a better path.
double costOrDefault = costFromStartToNode.ContainsKey(neighboar) ? costFromStartToNode.GetValueOrDefault(neighboar) : double.MaxValue;
if (tentativeDistance >= costOrDefault)
{
AddWithPriotiry(neighboar, int.MaxValue);
continue;
}
// This path is the best until now. Record it!
ReplaceInDictionary(nodeToParent, neighboar, current);
ReplaceInDictionary(costFromStartToNode, neighboar, tentativeDistance);
//ReplaceInDictionary(costToGoal, neighboar, heuristicImpl.DistanceBetween(neighboar, goal));
AddWithPriotiry(neighboar, heuristicImpl.HeuristicDistanceBetween(neighboar, goal));
//notEvaluatedNodes = notEvaluatedNodes.OrderBy(keyIt => costToGoal.GetValueOrDefault(keyIt.Key)).ToDictionary((keyItem) => keyItem.Key, (valueItem) => valueItem.Value);
}
}
return(null);
}
