/// <summary> /// A* on the graph. /// </summary> /// <param name="pathStoreLoc">The Queue to store the path in.</param> /// <param name="start">The starting node.</param> /// <param name="end">The ending node.</param> public double AStar(Queue<Node> pathStoreLoc, Node start, Node end, Node toIgnore = null) { MinPriorityQueue<Node> nodeList = new MinPriorityQueue<Node>(); initializePathfinding(true); if (toIgnore != null) toIgnore.Visited = false; System.Func<Node, Node, float> Heuristic; if (allowedPaths == Paths.quadDir) Heuristic = ManhattenHeuristic; else if (allowedPaths == Paths.octDir) Heuristic = DiagonalHeuristic; else Heuristic = DiagonalHeuristic; start.CameFrom = null; start.heuristicCost = Heuristic(start, end); start.realCost = 0; nodeList.Enqueue(start, start.heuristicCost); #if DEBUG_PATHFINDER_LOGDEBUG StringBuilder encountered = new StringBuilder(); StringBuilder nodes = new StringBuilder(); nodes.Append("Start node ").Append(start.Number).AppendLine(); encountered.Append("Start node ").Append(start.Number).AppendLine(); nodes.Append("End node ").Append(end.Number).AppendLine(); encountered.Append("End node ").Append(end.Number).AppendLine(); #endif while (nodeList.Count > 0) { //Pick the best looking node, by f-value. Node best = nodeList.Dequeue(); double bestDist = best.realCost; #if DEBUG_PATHFINDER_LOGDEBUG encountered.Append("Node ").Append(best.Number).Append(" ").Append(best).AppendLine(); nodes.Append("Node ").Append(best.Number).AppendLine(); #endif //If this is the end, stop, show the path, and return it. if (best.Equals(end)) { ReconstructPath(pathStoreLoc, end); ShowPath(pathStoreLoc); #if DEBUG_PATHFINDER_LOGDEBUG encountered.Append("Finished!\n\nFinal dist: ") .Append(best.realCost).AppendLine(); Debug.Log(encountered); Debug.Log(nodes); #endif return best.realCost; } best.Visited = true; //string updateString = "updating: "; foreach (Edge e in best.getEdges()) { Node other = e.getNode(); //We already visited this node, move along, if (other.Visited) continue; //Tentative distance. double testDist = e.getWeight() + bestDist; //If the other node isn't in the priority queue, add it. if (!nodeList.Contains(other)) { other.CameFrom = best; other.realCost = testDist; other.heuristicCost = Heuristic(other, end); nodeList.Enqueue(other, other.realCost + other.heuristicCost); #if DEBUG_PATHFINDER_LOGDEBUG encountered.Append(" added ").Append(other.Number) .Append(", total estimated cost ") .Append(other.realCost + other.heuristicCost) .AppendLine(); #endif continue; } //If the other node was a bad path, and this one's better, replace it. else if (other.realCost > testDist) { other.CameFrom = best; other.realCost = testDist; nodeList.Update(other, other.realCost + other.heuristicCost); #if DEBUG_PATHFINDER_LOGDEBUG encountered.Append(" updated ").Append(other.Number) .Append(", total new estimated cost ") .Append(other.realCost + other.heuristicCost) .AppendLine(); #endif } } } #if DEBUG_PATHFINDER_LOGDEBUG encountered.Append("Failed!\n"); Debug.Log(encountered); Debug.Log(nodes); #endif return double.PositiveInfinity; }
/// <summary> /// Returns a sorted list of nodes within a given endurance value. /// Performs a Dijkstra-like algorithm. /// </summary> /// <param name="satifies">The predicate each node must follow.</param> /// <param name="endurance">The maximum endurance to follow out.</param> /// <returns>A sorted list of nodes within a given endurance value.</returns> public List<Node> nodesThatSatisfyPred(Node startNode, System.Predicate<Node> satifies, float endurance = 16.0f, bool stopOnFirst = false, bool isPathfinding = true) { List<Node> foundNodes = new List<Node>(); MinPriorityQueue<Node> nodeList = new MinPriorityQueue<Node>(); initializePathfinding(isPathfinding); startNode.realCost = 0; nodeList.Enqueue(startNode, startNode.realCost); #if DEBUG_PATHFINDER_LOGDEBUG StringBuilder encountered = new StringBuilder(); StringBuilder nodes = new StringBuilder(); nodes.Append("Start node ").Append(startNode.Number).AppendLine(); encountered.Append("Start node ").Append(startNode.Number).AppendLine(); encountered.Append("endurance = ").Append(endurance).AppendLine(); #endif while (nodeList.Count > 0) { //Pick the best looking node, by f-value. Node best = nodeList.Dequeue(); double bestDist = best.realCost; #if DEBUG_PATHFINDER_LOGDEBUG encountered.Append("Node ").Append(best.Number).Append(" ").Append(best).AppendLine(); nodes.Append("Node ").Append(best.Number).AppendLine(); #endif best.Visited = true; if (satifies(best)) { foundNodes.Add(best); if (stopOnFirst) return foundNodes; } //string updateString = "updating: "; foreach (Edge e in best.getEdges()) { Node other = e.getNode(); //We already visited this node, move along, if (other.Visited) continue; //Tentative distance. double testDist = e.getWeight() + bestDist; if (testDist > endurance) continue; //If the other node isn't in the priority queue, add it. if (!nodeList.Contains(other)) { other.CameFrom = best; other.realCost = testDist; nodeList.Enqueue(other, other.realCost); #if DEBUG_PATHFINDER_LOGDEBUG encountered.Append(" added ").Append(other.Number) .Append(", total estimated cost ") .Append(other.realCost).AppendLine(); #endif continue; } //If the other node was a bad path, and this one's better, replace it. else if (other.realCost > testDist) { other.CameFrom = best; other.realCost = testDist; nodeList.Update(other, other.realCost); #if DEBUG_PATHFINDER_LOGDEBUG encountered.Append(" updated ").Append(other.Number) .Append(", total new estimated cost ") .Append(other.realCost).AppendLine(); #endif } } } #if DEBUG_PATHFINDER_LOGDEBUG Debug.Log(encountered); Debug.Log(nodes); #endif return foundNodes; }