public IList <Node> Find(Node start, Node goal)
{
Start = start;
Goal = goal;
Parent.Add(start, start);
Open.Enqueue(Start, Heuristic(Start));
while (Open.Count > 0)
{
Node node = Open.Dequeue();
if (node.Id == Goal.Id)
{
return(BuildPath());
}
AddUpdate(Closed, node.Id, node);
foreach (ProposedStep step in Grid.Neighbours(node))
{
Node neighour = step.Node;
if (!Closed.ContainsKey(neighour.Id))
{
if (!Open.Contains(neighour))
{
AddUpdate(GHistory, neighour.Id, double.PositiveInfinity);
TryRemove(Parent, neighour);
}
double gOld = TryGetValue(GHistory, neighour.Id);
// Compute Cost
if (UseLineOfSight && LineOfSight(TryGetValue(Parent, node), neighour))
{
var sParent = TryGetValue(Parent, node);
var sParentCost = Cost(sParent, neighour);
if (TryGetValue(GHistory, sParent.Id) + sParentCost < TryGetValue(GHistory, neighour.Id))
{
AddUpdate(Parent, neighour, sParent);
AddUpdate(GHistory, neighour.Id, TryGetValue(GHistory, node.Id) + sParentCost);
}
}
else
{
if (TryGetValue(GHistory, node.Id) + step.Direction.Cost < TryGetValue(GHistory, neighour.Id))
{
AddUpdate(Parent, neighour, node);
AddUpdate(GHistory, neighour.Id, TryGetValue(GHistory, node.Id) + step.Direction.Cost);
}
}
// Compute Cost End
if (TryGetValue(GHistory, neighour.Id) < gOld)
{
if (Open.Contains(neighour))
{
Open.Remove(neighour);
}
Open.Enqueue(neighour, TryGetValue(GHistory, neighour.Id) + Heuristic(neighour));
}
}
}
}
return(new List <Node>());
}