public static FindPathResult <T> FindPath <T>(this iExplorer <T> explorer, T start, T goal, CancellationToken cancellationToken, int maxChecks = c_MaxChecks, int bufferSize = c_BufferSize) { var result = new FindPathResult <T>(); var startNode = new PathNode <T>(start); if (start == null || goal == null || explorer == null || explorer.iReachable(start, goal) == false) { return(result); } var closedSet = result.m_ClosedSet; var openSet = new FastPriorityQueue <PathNode <T> >(bufferSize); result.m_OpenSet = openSet; startNode.СameFrom = null; startNode.PathCost = 0.0f; startNode.PathCostEstimated = explorer.iGetShortestPossiblePath(start, goal); startNode.Cost = 0.0f; openSet.Enqueue(startNode, startNode.Cost); // do while has variants while (openSet.Count > 0 && closedSet.Count < maxChecks && openSet.Count < bufferSize) { // cancellation check if (cancellationToken.IsCancellationRequested) { return(result); } // get next node var currentNode = openSet.First(); openSet.Remove(currentNode); // goal check if (currentNode.Master.Equals(goal)) { implGetPathForNode(currentNode, result.m_Path); return(result); } // close current closedSet.Add(currentNode.Master); // proceed connections foreach (var neighborNode in explorer.iGetNeighbours(currentNode.Master)) { if (closedSet.Contains(neighborNode)) // skip if already checked { continue; } var pathCost = currentNode.PathCost + explorer.iGetPathCost(currentNode.Master, neighborNode); // can use Dictionary instead FirstOrDefault var openNode = openSet.FirstOrDefault(n => n.Master.Equals(neighborNode)); if (openNode != null) { // if presented and part is shorter then reset his parent and cost if (openNode.PathCost > pathCost) { openNode.СameFrom = currentNode; openNode.PathCost = pathCost; // update priority openNode.Cost = openNode.PathCostEstimated + openNode.PathCost; openSet.UpdatePriority(openNode, openNode.Cost); } } else { // if not presented add as variant var pathNode = new PathNode <T>(neighborNode); pathNode.СameFrom = currentNode; pathNode.PathCost = pathCost; pathNode.PathCostEstimated = explorer.iGetShortestPossiblePath(pathNode.Master, goal); pathNode.Cost = pathNode.PathCostEstimated + pathNode.PathCost; openSet.Enqueue(pathNode, pathNode.Cost); } } } return(result); }
public static IEnumerable <TNode> FindPath( TNode start, TNode end, Func <TNode, IEnumerable <TNode> > getNeighbors, Func <TNode, TNode, float> getCost, Func <TNode, TNode, float> getHeuristicCost, IEqualityComparer <TNode> equalityComparer, int maxNodes = 1024) { var openList = new FastPriorityQueue <PriorityNode>(maxNodes); var closedList = new HashSet <PriorityNode>(new PriorityNodeEqualityComparer(equalityComparer)); var startNode = new PriorityNode(start, getHeuristicCost(start, end)); openList.Enqueue(startNode, startNode.Priority); while (openList.Any()) { var currentNode = openList.Dequeue(); if (equalityComparer.Equals(currentNode.Node, end)) { var path = new List <TNode>(); while (currentNode.Parent != null) { path.Add(currentNode.Node); currentNode = currentNode.Parent; } return(((IEnumerable <TNode>)path).Reverse()); } closedList.Add(currentNode); foreach (var neighbor in getNeighbors(currentNode.Node)) { var cost = currentNode.RealCost + getCost(currentNode.Node, neighbor); if (float.IsInfinity(cost)) { continue; } var openListNeighbor = openList.FirstOrDefault(x => equalityComparer.Equals(x.Node, neighbor)); if (openListNeighbor != null && cost < openListNeighbor.RealCost) { openList.Remove(openListNeighbor); openListNeighbor = null; } var closedListNeighbor = closedList.FirstOrDefault(x => equalityComparer.Equals(x.Node, neighbor)); if (closedListNeighbor != null && cost < closedListNeighbor.RealCost) { closedList.Remove(closedListNeighbor); closedListNeighbor = null; } if (openListNeighbor == null && closedListNeighbor == null) { if (openList.Count == openList.MaxSize) { continue; } var realCost = currentNode.RealCost + cost; var heuristicCost = getHeuristicCost(neighbor, end); var newNode = new PriorityNode(currentNode, neighbor, realCost, heuristicCost); openList.Enqueue(newNode, newNode.Priority); } } } return(new List <TNode>()); }