public IPathGraph <TKey, TNode> Find(
IPathMaker <TKey, TNode> pathMaker,
TKey startKey,
Func <TNode, bool> pathingBlocked,
IGraph <TKey> graph
)
{
var explored = pathMaker.GetGraph();
var currentNode = pathMaker.MakeNode(startKey, null);
var frontier = new PriorityList <float, TNode>();
while (currentNode != null)
{
explored.Add(currentNode);
var surroundingNodes = GetSurroundingNodes(graph, currentNode, explored);
if (surroundingNodes != null)
{
ExploreSurroundingNodes(pathingBlocked, currentNode, frontier, surroundingNodes, pathMaker, explored);
}
if (frontier.Count == 0)
{
currentNode = null;
break;
}
currentNode = frontier[frontier.Count - 1];
frontier.RemoveAt(frontier.Count - 1);
}
return(explored);
}
void AddNodeToFrontier(IPathGraph <TKey, TNode> path, TNode newNode, PriorityList <float, TNode> frontier)
{
var key = newNode.Key;
var travelCost = newNode.TraveledCost;
var lastNode = newNode.Previous;
var n = GetFrontierNodeIndex(path, frontier, key);
if (n >= 0)
{
if (frontier[n].TraveledCost > travelCost)
{
var node = frontier[n];
frontier[n].TraveledCost = travelCost;
frontier[n].Previous = lastNode;
frontier.RemoveAt(n);
frontier.Insert(travelCost, node);
}
}
else
{
frontier.Insert(travelCost, newNode);
}
}