public List <TNode> FindFirstPath(TNode start, TNode goal) { if (start.CompareTo(goal) == 0) { return(new List <TNode>()); } visitedVertexes.Add(start); foreach (TNode node in heuristicImpl.GetNeighboars(start)) { bool isVisited = visitedVertexes.FirstOrDefault(it => it.CompareTo(node) == 0) != null; if (!isVisited) { List <TNode> result = FindFirstPath(start, goal); if (result != null) { result.Insert(0, start); return(result); } } } return(null); }
public IEnumerable <TNode> FindPath() { while (notEvaluatedNodes.Count > 0) { TNode current = notEvaluatedNodes.Keys.First(); if (current.CompareTo(goal) == 0) { // TODO reconstruct_path return(ReconstructPath(current)); } notEvaluatedNodes.Remove(current); nodesVisited.Add(current, true); foreach (TNode neighboar in heuristicImpl.GetNeighboars(current)) { if (nodesVisited.ContainsKey(neighboar)) { continue; } if (!notEvaluatedNodes.ContainsKey(neighboar)) { notEvaluatedNodes.Add(neighboar, int.MaxValue); } // compute heuristic cost double tentativeDistance = costFromStartToNode.GetValueOrDefault(current) + heuristicImpl.DistanceBetween(current, neighboar); // This is not a better path. double costOrDefault = costFromStartToNode.ContainsKey(neighboar) ? costFromStartToNode.GetValueOrDefault(neighboar) : double.MaxValue; if (tentativeDistance >= costOrDefault) { 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)); //costToGoal = costToGoal.OrderBy((keyItem) => keyItem.Key).ToDictionary((keyItem) => keyItem.Key, (valueItem) => valueItem.Value); notEvaluatedNodes = notEvaluatedNodes.OrderBy(keyIt => costToGoal.GetValueOrDefault(keyIt.Key)).ToDictionary((keyItem) => keyItem.Key, (valueItem) => valueItem.Value); } } return(null); }
public List <TNode> FindPath(TNode start, TNode goal) { InitSets(start); while (unexploredNodes.Count > 0) { TNode current = unexploredNodes.Dequeue(); if (current.CompareTo(goal) == 0) { return(ReconstructPath(goal)); } foreach (TNode neighboar in heuristicImpl.GetNeighboars(current)) { if (nodeToParent.ContainsKey(neighboar)) { nodeToParent.Add(neighboar, current); unexploredNodes.Enqueue(neighboar); } } } return(null); }