////////////////////////////////////////////////////////////////////////// public void Init(FindPathOptions options, iExplorer <T> exlorer, T start, T[] goal) { if (Result == FindPathResult.None) { // save data Options = options; Explorer = exlorer; Start = start; Goal = goal; // set running state Result = FindPathResult.Running; // allocate collections Path = new LinkedList <T>(); OpenSet = new FastPriorityQueue <PathNode <T> >(Pathfinder.c_AdaptiveBufferExperiance.Average); ClosedSet = new HashSet <T>(); ClosedNodes = new LinkedList <PathNode <T> >(); // create start node, add to open set var startNode = new PathNode <T>(start) { СameFrom = null, PathCost = 0.0f, PathCostEstimated = 0.0f, Cost = 0.0f }; OpenSet.Enqueue(startNode, startNode.Cost); } }
public static FindPathProcess <T> FindPath <T>(this iExplorer <T> explorer, FindPathOptions options, T start, params T[] goal) { // set default options if (options == null) { options = FindPathOptions.c_Default; } // create result var result = new FindPathProcess <T>(); // common sense check if (start == null || goal.Length == 0 || explorer == null) { result.Complete(FindPathResult.BadArguments); return(result); } // find only reachable goals goal = goal .Where(n => explorer.Reachable(start, n)) .ToArray(); // is reachable if (goal.Length == 0) { result.Complete(FindPathResult.NotReachable); return(result); } // init result inner data result.Init(options, explorer, start, goal); return(result); }
public static FindPathResult <T> FindPath <T>(iExplorer <T> explorer, T start, List <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.PathLengthFromStart = 0.0f; start.PathLengthEstimated = implGetShortestPossiblePath(explorer, start, goal); openSet.Enqueue(start, 0); while (closedSet.Count < c_MaxElementsInQueue && openSet.Count > 0) { var currentNode = openSet.Dequeue(); foreach (var n in goal) { if (currentNode == n) { 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 = implGetShortestPossiblePath(explorer, neighbourNode as T, goal); openSet.Enqueue(neighbourNode, neighbourNode.PathLengthEstimated + neighbourNode.PathLengthFromStart); } } } return(null); }
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); }
public static FindPathProcess <T> FindPath <T>(this iExplorer <T> explorer, out FindPathProcess <T> findPathProcess, FindPathOptions options, T start, params T[] goal) { findPathProcess = explorer.FindPath(options, start, goal); return(findPathProcess); }
////////////////////////////////////////////////////////////////////////// public static FindPathProcess <T> FindPath <T>(this iExplorer <T> explorer, T start, params T[] goal) { return(explorer.FindPath(FindPathOptions.c_Default, start, goal)); }