////////////////////////////////////////////////////////////////////////// public static FindPathResult <T> FindPath <T>(iExplorer <T> explorer, T start, T goal) where T : PathNodeFPQN { var result = new FindPathResult <T>(c_MaxElementsInQueue, c_MaxElementsInQueue); if (start == null || goal == null || explorer == null) { return(result); } var closedSet = result.m_ClosedSet; var openSet = result.m_OpenSet; start.СameFrom = null; start.PathLengthFromStart = 0.0f; start.PathLengthEstimated = explorer.iGetShortestPossiblePath(start, goal); openSet.Enqueue(start, 0); while (closedSet.Count < closedSet.MaxSize && openSet.Count > 0 && openSet.Count < openSet.MaxSize) { var currentNode = openSet.Dequeue(); if (currentNode == goal) { implGetPathForNode <T>(currentNode, result.m_Path); return(result); } closedSet.Enqueue(currentNode, currentNode.PathLengthEstimated + currentNode.PathLengthFromStart); foreach (var neighbourNode in explorer.iGetNeighbours(currentNode as T)) { if (closedSet.Contains(neighbourNode)) // skip if already checked { continue; } var pathLengthFromStart = currentNode.PathLengthFromStart + explorer.iGetPathCost(currentNode as T, neighbourNode); if (openSet.Contains(neighbourNode)) { // if presented and part is shorter then reset his parent and cost if (neighbourNode.PathLengthFromStart > pathLengthFromStart) { neighbourNode.СameFrom = currentNode; neighbourNode.PathLengthFromStart = pathLengthFromStart; openSet.UpdatePriority(neighbourNode, neighbourNode.PathLengthEstimated + neighbourNode.PathLengthFromStart); } } else { // if not presented add as wariant neighbourNode.СameFrom = currentNode; neighbourNode.PathLengthFromStart = pathLengthFromStart; neighbourNode.PathLengthEstimated = explorer.iGetShortestPossiblePath(neighbourNode as T, goal); openSet.Enqueue(neighbourNode, neighbourNode.PathLengthEstimated + neighbourNode.PathLengthFromStart); } } } return(result); }
private static float implGetShortestPossiblePath <T>(iExplorer <T> explorer, T start, List <T> goal) { var shortestPath = float.MaxValue; foreach (var n in goal) { var currentShortestPath = explorer.iGetShortestPossiblePath(start, n); if (shortestPath > currentShortestPath) { shortestPath = currentShortestPath; } } return(shortestPath); }
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); }