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