/* * public functions */ /// <summary> /// A*, modified to find a nearest path in case of failure /// </summary> /// <param name="map">The Map</param> /// <param name="startNode">The starting Node</param> /// <param name="endNode">The ending Node</param> /// <returns>The path as a list of waypoints</returns> public static List<Node> findPath(NodeMap map, Node startNode, Node endNode) { // initialize data PQueue open = new PQueue(); open.enqueue(startNode); List<Node> adjacentNodes = new List<Node>(); // good ol' A* while (open.Count > 0) { // find the open Node with the lowest Fscore and remove it from the open PQueue Node current = open.dequeue(); // if this is our destination Node, we're done; otherwise, close it so we don't travel to it again if (current == endNode) return reconstruct(endNode); current.close(); // find every valid Node adjacent to the current Node adjacentNodes = map.getAdjacentNodes(current); // iterate over all of them for (int i = 0; i < adjacentNodes.Count; i++) { // grab an adjacent Node and calculate a new GScore and HScore for it Node adjacent = adjacentNodes[i]; int tempGScore = current.Gscore + map.pathDistance(current, adjacent); int tempHScore = map.pathDistance(adjacent, endNode); // if we have not opened this Cell, give it new stats and open it if (!adjacent.isOpen) { setAdjacentStats(adjacent, current, tempGScore, tempHScore); open.enqueue(adjacent); } // otherwise, if we have opened it but the new path to it is shorter than the old one, give it new stats and reset its position in the open PQueue else if (tempGScore < adjacent.Gscore) { setAdjacentStats(adjacent, current, tempGScore, tempHScore); //open.resetPosition(adjacent); } } } // no valid path exists, so find the nearest path List<Node> closed = createClosed(map); Node nearestNode = closed[0]; // find the closed Node with the lowest Hscore (distance from the intended end); return a path to that Node if (closed.Count > 0) { for (int i = 1; i < closed.Count; i++) { if (closed[i].Hscore < nearestNode.Hscore) nearestNode = closed[i]; } } return reconstruct(nearestNode); }
/* * public functions */ /// <summary> /// Given an invalid Node, returns the approximate nearest valid Node. /// </summary> /// <param name="end"></param> /// <returns></returns> public static Node nearestValidEnd(NodeMap map, Node start, Node end) { int max = Math.Max(map.height, map.width); PQueue ring = new PQueue(); // iterate outward from the end Node in progressively larger rings, putting all valid Nodes into a PQueue. // Continue until we find a ring with a valid Node. for (int i = 1; i < max; i++) { ring = getRing(map, end, i); if (ring.Count != 0) { break; } } // find the valid Node nearest the intended end Node newEnd = ring.peek(0); // if the PQueue has a lead tie (i.e. there are 2+ nodes of equal distance from the center node), // pick the one closest to the intended start Node. if (ring.hasLeadTie) { int distToStart = map.pathDistance(start, newEnd); int distToEnd = map.pathDistance(end, newEnd); for (int i = 1; i < ring.Count; i++) { if (map.pathDistance(ring.peek(i), end) > distToEnd) { break; } int currentDist = map.pathDistance(ring.peek(i), start); if (currentDist < distToStart) { distToStart = currentDist; newEnd = ring.peek(i); } } } // clean all enqueued Nodes so they can be properly used by the pathfinder for (int i = 0; i < ring.Count; i++) { ring.peek(i).clean(); } return(newEnd); }
/* * public functions */ /// <summary> /// Given an invalid Node, returns the approximate nearest valid Node. /// </summary> /// <param name="end"></param> /// <returns></returns> public static Node nearestValidEnd(NodeMap map, Node start, Node end) { int max = Math.Max(map.height, map.width); PQueue ring = new PQueue(); // iterate outward from the end Node in progressively larger rings, putting all valid Nodes into a PQueue. // Continue until we find a ring with a valid Node. for (int i = 1; i < max; i++) { ring = getRing(map, end, i); if (ring.Count != 0) break; } // if this is a completely invalid NodeMap (yeah, right, I know), return null if (ring.Count == 0) return null; // find the valid Node nearest the intended end Node newEnd = ring.peek(0); // if the PQueue has a lead tie (i.e. there are 2+ nodes of equal distance from the center node), // pick the one closest to the intended start Node. if (ring.hasLeadTie) { int distToStart = map.pathDistance(start, newEnd); int distToEnd = map.pathDistance(end, newEnd); for (int i = 1; i < ring.Count; i++) { if (map.pathDistance(ring.peek(i), end) > distToEnd) break; int currentDist = map.pathDistance(ring.peek(i), start); if (currentDist < distToStart) { distToStart = currentDist; newEnd = ring.peek(i); } } } // clean the FScores of all enqueued Nodes so they can be properly used by the pathfinder for (int i = 0; i < ring.Count; i++) ring.peek(i).Fscore = 0; return newEnd; }
/* * helper functions */ /// <summary> /// Enqueues all valid Nodes in a ring around a center Node, offset from the center by [offset] Nodes. /// For example, an offset of 2 searches a 5x5 ring (x+-2, y+-2) around the center Node. /// Since modifying the Node's Fscores is necessary for enqueueing, each enqueued node's Fscore must be reset after use. /// </summary> /// <param name="map">The NodeMap to search over</param> /// <param name="center">The center Node to search around</param> /// <param name="offset">The ring "radius"</param> /// <returns>A PQueue containing all valid Nodes found in the ring</returns> private static PQueue getRing(NodeMap map, Node center, int offset) { PQueue ring = new PQueue(); int x = center.X; int y = center.Y; // grab left and right columns for (int i = Math.Max(y - offset, 0); i <= y + offset; i++) { Node Xmin = map.getNode(x - offset, i); Node Xmax = map.getNode(x + offset, i); if (Xmin != null && Xmin.isValid) { Xmin.Fscore = map.pathDistance(Xmin, center); ring.enqueue(Xmin); } if (Xmax != null && Xmax.isValid) { Xmax.Fscore = map.pathDistance(Xmax, center); ring.enqueue(Xmax); } } // grab remainder of top and bottom rows for (int i = x - offset + 1; i < x + offset; i++) { Node Ymin = map.getNode(i, Math.Max(0, y - offset)); Node Ymax = map.getNode(i, Math.Min(map.width - 1, y + offset)); if (Ymin != null && Ymin.isValid) { Ymin.Fscore = map.pathDistance(Ymin, center); ring.enqueue(Ymin); } if (Ymax != null && Ymax.isValid) { Ymax.Fscore = map.pathDistance(Ymax, center); ring.enqueue(Ymax); } } return(ring); }
/* * helper functions */ /// <summary> /// Enqueues all valid Nodes in a ring around a center Node, offset from the center by [offset] Nodes. /// For example, an offset of 2 searches a 5x5 ring (x+-2, y+-2) around the center Node. /// Since modifying the Node's Fscores is necessary for enqueueing, each enqueued node's Fscore must be reset after use. /// </summary> /// <param name="map">The NodeMap to search over</param> /// <param name="center">The center Node to search around</param> /// <param name="offset">The ring "radius"</param> /// <returns>A PQueue containing all valid Nodes found in the ring</returns> private static PQueue getRing(NodeMap map, Node center, int offset) { PQueue ring = new PQueue(); int x = center.Xcoord; int y = center.Ycoord; // grab left and right columns for (int i = y - offset; i <= y + offset; i++) { Node Xmin = map.getNode(x - offset, i); Node Xmax = map.getNode(x + offset, i); if (Xmin.isValid) { Xmin.Fscore = map.pathDistance(Xmin, center); ring.enqueue(Xmin); } if (Xmax.isValid) { Xmax.Fscore = map.pathDistance(Xmax, center); ring.enqueue(Xmax); } } // grab remainder of top and bottom rows for (int i = x - offset + 1; i < x + offset; i++) { Node Ymin = map.getNode(i, y - offset); Node Ymax = map.getNode(i, y + offset); if (Ymin.isValid) { Ymin.Fscore = map.pathDistance(Ymin, center); ring.enqueue(Ymin); } if (Ymax.isValid) { Ymax.Fscore = map.pathDistance(Ymax, center); ring.enqueue(Ymax); } } return ring; }
/* * helper functions */ /// <summary> /// Enqueues all valid Nodes in a ring around a center Node, offset from the center by [offset] Nodes. /// For example, an offset of 2 searches a 5x5 ring (x+-2, y+-2) around the center Node. /// Since modifying the Node's Fscores is necessary for enqueueing, each enqueued node's Fscore must be reset after use. /// </summary> /// <param name="map">The NodeMap to search over</param> /// <param name="center">The center Node to search around</param> /// <param name="offset">The ring "radius"</param> /// <returns>A PQueue containing all valid Nodes found in the ring</returns> private static PQueue getRing(NodeMap map, Node center, int offset) { PQueue ring = new PQueue(); int x = center.Xcoord; int y = center.Ycoord; // grab left and right columns for (int i = y - offset; i <= y + offset; i++) { Node Xmin = map.getNode(x - offset, i); Node Xmax = map.getNode(x + offset, i); if (Xmin.isValid) { Xmin.Fscore = map.pathDistance(Xmin, center); ring.enqueue(Xmin); } if (Xmax.isValid) { Xmax.Fscore = map.pathDistance(Xmax, center); ring.enqueue(Xmax); } } // grab remainder of top and bottom rows for (int i = x - offset + 1; i < x + offset; i++) { Node Ymin = map.getNode(i, y - offset); Node Ymax = map.getNode(i, y + offset); if (Ymin.isValid) { Ymin.Fscore = map.pathDistance(Ymin, center); ring.enqueue(Ymin); } if (Ymax.isValid) { Ymax.Fscore = map.pathDistance(Ymax, center); ring.enqueue(Ymax); } } return ring; }
/* * public functions */ /// <summary> /// A*, modified to find a nearest path in case of failure /// </summary> /// <param name="map">The Map</param> /// <param name="startNode">The starting Node</param> /// <param name="endNode">The ending Node</param> /// <returns>The path as a list of waypoints</returns> public static List <Node> findPath(NodeMap map, Node startNode, Node endNode) { // initialize data PQueue open = new PQueue(); open.enqueue(startNode); List <Node> adjacentNodes = new List <Node>(); // good ol' A* while (open.Count > 0) { // find the open Node with the lowest Fscore and remove it from the open PQueue Node current = open.dequeue(); // if this is our destination Node, we're done; otherwise, close it so we don't travel to it again if (current == endNode) { return(reconstruct(endNode)); } current.close(); // find every valid Node adjacent to the current Node adjacentNodes = map.getAdjacentNodes(current); // iterate over all of them for (int i = 0; i < adjacentNodes.Count; i++) { // grab an adjacent Node and calculate a new GScore and HScore for it Node adjacent = adjacentNodes[i]; int tempGScore = current.Gscore + map.pathDistance(current, adjacent); int tempHScore = map.pathDistance(adjacent, endNode); // if we have not opened this Cell, give it new stats and open it if (!adjacent.isOpen) { setAdjacentStats(adjacent, current, tempGScore, tempHScore); open.enqueue(adjacent); } // otherwise, if we have opened it but the new path to it is shorter than the old one, give it new stats and reset its position in the open PQueue else if (tempGScore < adjacent.Gscore) { setAdjacentStats(adjacent, current, tempGScore, tempHScore); //open.resetPosition(adjacent); } } } // no valid path exists, so find the nearest path List <Node> closed = createClosed(map); Node nearestNode = closed[0]; // find the closed Node with the lowest Hscore (distance from the intended end); return a path to that Node if (closed.Count > 0) { for (int i = 1; i < closed.Count; i++) { if (closed[i].Hscore < nearestNode.Hscore) { nearestNode = closed[i]; } } } return(reconstruct(nearestNode)); }
/* * public functions */ /// <summary> /// A*, modified to find a nearest path in case of failure /// </summary> /// <param name="map">The Map</param> /// <param name="startNode">The starting Node</param> /// <param name="endNode">The ending Node</param> /// <returns>The path as a list of waypoints</returns> public static List<Node> findPath(NodeMap map, Node startNode, Node endNode) { /* * Terminology * Gscore cumulative calculated distance from the start Node to the given Node * Hscore estimated distance from the given Node to the end Node. * Overestimating this can result in the calculation of an incorrect (inefficient) path, * but the more it is underestimated, the longer correct path calculation will take * Fscore Gscore + Hscore; estimated total distance from start to end on a path through the given Node. * Priority queues (PQueues) are ordered by ascending Fscore, so shortest estimated paths are examined first * open list A PQueue of Nodes to be examined. Initially contains the start Node * closed list A List<Node> of Nodes that have been examined * adjacent list A PQueue of Nodes adjacent to the current Node */ // initialize the lists PQueue open = new PQueue(); open.enqueue(startNode); List<Node> closed = new List<Node>(); PQueue adjacentNodes = new PQueue(); iterations = 0; // good ol' A* while (open.Count > 0) // iterate until we have examined every appropriate Node { //open.print("Open", true); Node currentNode = open.dequeue(); // look at the Node with the lowest Fscore and remove it from the open list if (currentNode == endNode) // if this is our destination Node, we're done! return reconstruct(endNode); // so return the path closed.Add(currentNode); // otherwise, close this Node so we don't travel to it again adjacentNodes = getAdjacentNodes(map, open, closed, currentNode); // now find every valid Node adjacent to the current Node //adjacentNodes.print("Adjacent", false); while (adjacentNodes.Count != 0) // iterate over all of them, from lowest to highest Fscore { Node adjacentNode = adjacentNodes.dequeue(); // grab the current adjacent Node int tempGScore = currentNode.Gscore + map.pathDistance(currentNode, adjacentNode); // calculate a temporary Gscore as if we were traveling to this Node from the current Node if (!open.contains(adjacentNode) || tempGScore < adjacentNode.Gscore) // if this Node has not been added to the open list, or if tempGscore is less than the Node's current Gscore { int h = map.pathDistance(adjacentNode, endNode); // estimate the Node's Hscore adjacentNode.prev = currentNode; // set the Node's prev pointer to the current Node adjacentNode.Hscore = h; // set the Node's Hscore adjacentNode.Gscore = tempGScore; // set the Node's Gscore adjacentNode.Fscore = tempGScore + h; // set the Node's Fscore } if (!open.contains(adjacentNode)) // if the adjacent Node we just examined is not yet on the open list, add it open.enqueue(adjacentNode); } iterations++; } // no valid path exists, so find the nearest path closed.RemoveAt(0); // remove the start Node from the closed List if (closed.Count > 0) // if there are still Nodes on the closed list { Node nearestNode = closed[0]; // find the closed Node with the lowest Hscore; for (int i = 1; i < closed.Count; i++) // this should be the Node closest to the desired destination, { // so return a path ending with that Node. if (closed[i].Hscore < nearestNode.Hscore) nearestNode = closed[i]; } return reconstruct(nearestNode); } else { List<Node> path = new List<Node>(); // otherwise, our only path was the start Node (i.e. we are completely trapped); path.Add(endNode); // so return a path with just that Node. return path; } }
/* * public functions */ /// <summary> /// A*, modified to find a nearest path in case of failure /// </summary> /// <param name="map">The Map</param> /// <param name="startNode">The starting Node</param> /// <param name="endNode">The ending Node</param> /// <returns>The path as a list of waypoints</returns> public static List <Node> findPath(NodeMap map, Node startNode, Node endNode) { /* * Terminology * Gscore cumulative calculated distance from the start Node to the given Node * Hscore estimated distance from the given Node to the end Node. * Overestimating this can result in the calculation of an incorrect (inefficient) path, * but the more it is underestimated, the longer correct path calculation will take * Fscore Gscore + Hscore; estimated total distance from start to end on a path through the given Node. * Priority queues (PQueues) are ordered by ascending Fscore, so shortest estimated paths are examined first * open list A PQueue of Nodes to be examined. Initially contains the start Node * closed list A List<Node> of Nodes that have been examined * adjacent list A PQueue of Nodes adjacent to the current Node */ // initialize the lists PQueue open = new PQueue(); open.enqueue(startNode); List <Node> closed = new List <Node>(); PQueue adjacentNodes = new PQueue(); iterations = 0; // good ol' A* while (open.Count > 0) // iterate until we have examined every appropriate Node { //open.print("Open", true); Node currentNode = open.dequeue(); // look at the Node with the lowest Fscore and remove it from the open list if (currentNode == endNode) // if this is our destination Node, we're done! { return(reconstruct(endNode)); // so return the path } closed.Add(currentNode); // otherwise, close this Node so we don't travel to it again adjacentNodes = getAdjacentNodes(map, open, closed, currentNode); // now find every valid Node adjacent to the current Node //adjacentNodes.print("Adjacent", false); while (adjacentNodes.Count != 0) // iterate over all of them, from lowest to highest Fscore { Node adjacentNode = adjacentNodes.dequeue(); // grab the current adjacent Node int tempGScore = currentNode.Gscore + map.pathDistance(currentNode, adjacentNode); // calculate a temporary Gscore as if we were traveling to this Node from the current Node if (!open.contains(adjacentNode) || tempGScore < adjacentNode.Gscore) // if this Node has not been added to the open list, or if tempGscore is less than the Node's current Gscore { int h = map.pathDistance(adjacentNode, endNode); // estimate the Node's Hscore adjacentNode.prev = currentNode; // set the Node's prev pointer to the current Node adjacentNode.Hscore = h; // set the Node's Hscore adjacentNode.Gscore = tempGScore; // set the Node's Gscore adjacentNode.Fscore = tempGScore + h; // set the Node's Fscore } if (!open.contains(adjacentNode)) // if the adjacent Node we just examined is not yet on the open list, add it { open.enqueue(adjacentNode); } } iterations++; } // no valid path exists, so find the nearest path closed.RemoveAt(0); // remove the start Node from the closed List if (closed.Count > 0) // if there are still Nodes on the closed list { Node nearestNode = closed[0]; // find the closed Node with the lowest Hscore; for (int i = 1; i < closed.Count; i++) // this should be the Node closest to the desired destination, { // so return a path ending with that Node. if (closed[i].Hscore < nearestNode.Hscore) { nearestNode = closed[i]; } } return(reconstruct(nearestNode)); } else { List <Node> path = new List <Node>(); // otherwise, our only path was the start Node (i.e. we are completely trapped); path.Add(endNode); // so return a path with just that Node. return(path); } }