/// <summary> /// Uses Dijkstra's algorithm to find and return all nodes whose total cost is below the specified number. /// </summary> /// <param name="start">From where the pathfinding should start.</param> /// <param name="range">How many PathfinderLink.Cost-units away the tiles can be.</param> /// <returns></returns> public static List <PathfinderNode> FindNodesWithinRange(PathfinderNode start, int range, IPathfinderAgent agent = null) { CurrentRun++; var closed = new List <PathfinderNode>(); var open = new BinaryHeap <PathfinderNode>(); start.LastVisit = CurrentRun; start.PathCost = 0; open.Add(start); while (open.Count > 0) { PathfinderNode active = open.Remove(); closed.Add(active); active.Status = PathfinderNodeStatus.Closed; foreach (PathfinderLink link in active.GetNeighbors()) { PathfinderNode neighbor = link.Target; if (neighbor.LastVisit != CurrentRun) // Reset nodes that haven't been visited yet this run. { neighbor.Status = PathfinderNodeStatus.Unvisited; neighbor.LastVisit = CurrentRun; } if (!neighbor.IsClosed) { double cost = active.PathCost + link.GetCost(agent); if (cost <= range) { if (!neighbor.IsOpen) { neighbor.PathCost = cost; neighbor.Status = PathfinderNodeStatus.Open; open.Add(neighbor); } else if (cost < neighbor.PathCost) { neighbor.PathCost = cost; } } } } } return(closed); }
/// <summary> /// Calculates the lowest cost path with A* algorithm and returns the path. If no path is found, an empty list is returned. /// </summary> /// <param name="start">The node from which the path starts.</param> /// <param name="end">The destination of the path.</param> /// <param name="agent">The agent trying to find the path. Can be used to modify the cost of links.</param> /// <param name="maximumSearchDepth">How many loops the pathfinder will go through before terminating.</param> /// <returns></returns> public static IEnumerable <PathfinderNode> FindPath(PathfinderNode start, PathfinderNode end, int maximumSearchDepth = int.MaxValue, IPathfinderAgent agent = null) { if (start == end) { return(new List <PathfinderNode>()); } CurrentRun++; var open = new BinaryHeap <PathfinderNode>(); start.LastVisit = CurrentRun; start.PathCost = 0; start.Previous = null; open.Add(start); for (int i = 0; open.Count > 0 && i < maximumSearchDepth; i++) { PathfinderNode active = open.Remove(); active.Status = PathfinderNodeStatus.Closed; foreach (PathfinderLink link in active.GetNeighbors()) { PathfinderNode neighbor = link.Target; if (neighbor == end) { var path = new LinkedList <PathfinderNode>(); end.Previous = active; active = end; while (active != null) { path.AddFirst(active); active = active.Previous; } return(path); } if (neighbor.LastVisit != CurrentRun) // Reset nodes that haven't been visited yet this run. { neighbor.Status = PathfinderNodeStatus.Unvisited; neighbor.LastVisit = CurrentRun; } if (!neighbor.IsClosed) { double cost = active.PathCost + link.GetCost(agent); if (!neighbor.IsOpen) { neighbor.Previous = active; neighbor.PathCost = cost; neighbor.Heuristic = neighbor.CalculateHeuristic(end); neighbor.Status = PathfinderNodeStatus.Open; open.Add(neighbor); } else if (cost < neighbor.PathCost) { neighbor.Previous = active; neighbor.PathCost = cost; } } } } return(new List <PathfinderNode>()); }