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 }); }
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[current] + heuristicImpl.HeuristicDistanceBetween(current, neighboar); // This is not a better path. double costOrDefault = costFromStartToNode.ContainsKey(neighboar) ? costFromStartToNode[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); }