private static SearchNode <T> TraversePath(T goal, PList open, PList closed, IGraphProvider <T> _provider)
{
if (open.Count == 0)
{
return(null);
}
var current = open.FindBest();
if (current.Node == goal)
{
return(current);
}
closed.Add(current);
var edges = _provider.GetEdges(current.Node);
foreach (var e in edges)
{
if (closed.HasNode(e.Node))
{
continue;
}
var openChild = open.FindNode(e.Node);
if (!openChild)
{
openChild = new SearchNode <T>(e.Node, e.Cost, _provider.H(e.Node, goal), current);
open.Add(openChild);
}
else
{
float g = current.G + e.Cost;
if ((g + openChild.H) < openChild.F)
{
openChild.SetParent(current, e.Cost);
}
}
}
return(TraversePath(goal, open, closed, _provider));
}
