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);
}
