/*
* 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);
}
}