public DijkstraFindAllPathsResponse <TNode> FindAllPaths(TNode start)
{
InitSets(start);
while (unexploredSet.Count > 0)
{
TNode current = unexploredSet.Pool();
double distanceFromNode = distanceFromStartToNode[current];
foreach (TNode neightboar in heuristicImpl.GetNeighboars(current))
{
double distanceFromStart = distanceFromNode + heuristicImpl.VertexLength(current, neightboar);
double distanceToN = distanceFromStartToNode.ContainsKey(neightboar) ? distanceFromStartToNode[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
});
}
private void AddWithPriotiry(TNode node, IComparable priority)
{
if (!notEvaluatedNodes.Contains(node))
{
notEvaluatedNodes.Add(node, priority);
}
}
private void InitSets(TNode start)
{
distanceFromStartToNode = new Dictionary <TNode, double>
{
{ start, 0 }
};
unexploredSet = new MaxPriorityHeap <TNode>();
unexploredSet.Add(start, 0);
nodeToParent = new Dictionary <TNode, TNode>();
}
private void InitializeSets(TNode start, TNode goal)
{
nodesVisited = new Dictionary <TNode, bool>();
notEvaluatedNodes = new MaxPriorityHeap <TNode>();
notEvaluatedNodes.Add(start, heuristicImpl.HeuristicDistanceBetween(start, goal));
Dictionary <TNode, TNode> dictionary = new Dictionary <TNode, TNode>();
nodeToParent = dictionary;
costFromStartToNode = new Dictionary <TNode, double>
{
{ start, 0 }
};
//costToGoal = new Dictionary<TNode, double>
//{
// { start, heuristicImpl.DistanceBetween(start, goal) }
//};
}