public bool FindPath(ANode root, ANode dest)
{
PQueue queue = new PQueue();
if (!nodes.Contains(root))
{
nodes.Add(root);
foreach (APath x in root.GetPaths())
{
paths.Add(x);
}
}
if (!nodes.Contains(dest))
{
nodes.Add(dest);
foreach (APath x in dest.GetPaths())
{
paths.Add(x);
}
}
ANode currentNode = root;
currentNode.Cost = 0;
List <ANode> notVisited = new List <ANode>(nodes);
while (notVisited.Count > 0)
{
foreach (APath x in currentNode.GetPaths())
{
ANode neighbor = x.GetOtherSide(currentNode);
if (neighbor.Visited)
{
continue;
}
neighbor.UpdateHeuristic(GetHeuristicOf(neighbor, dest));
if (neighbor.Cost > currentNode.Cost + x.Cost)
{
neighbor.UpdateNodeCost(currentNode.Cost + x.Cost, currentNode);
}
queue.Enqueue(neighbor);
}
currentNode.Visited = true;
notVisited.Remove(currentNode);
if (notVisited.Count == 0)
{
break;
}
currentNode = queue.Dequeue();
if (currentNode.Equals(dest))
{
UpdatePath(root, dest);
return(true);
}
}
return(false);
}