/*
* public functions
*/
/// <summary>
/// The core function; calls all valid auxiliary functions and returns the path (advanced features can be toggled)
/// Includes a boolean toggle for use of advanced pathfinding functions
/// </summary>
/// <param name="map">The Map</param>
/// <param name="start">The starting Cell</param>
/// <param name="end">The ending Cell</param>
/// <param name="advanced"> A boolean toggle for advanced functions</param>
/// <returns>The path as a list of waypoints</returns>
public static List<CellComponent> between(Map map, CellComponent start, CellComponent end, bool advanced)
{
// begin timing the operation
DateTime startTime = DateTime.Now;
// convert the given Cell-based data to Node-based data
NodeMap nodeMap = new NodeMap(map);
Node nodeStart = nodeMap.getNode(start.X, start.Y);
Node nodeEnd = nodeMap.getNode(end.X, end.Y);
// perform advanced pre-calculation tasks
if (advanced)
{
// if the end Node is invalid, replace it with the nearest valid Node
if (!nodeEnd.isValid)
{
nodeEnd = Advanced.nearestValidEnd(nodeMap, nodeStart, nodeEnd);
}
}
// find the path
List<Node> nodePath = Basic.findPath(nodeMap, nodeStart, nodeEnd);
// convert the path from List<Node> format back to List<Cell> format
List<CellComponent> path = new List<CellComponent>(nodePath.Count);
for (int i = 0; i < nodePath.Count; i++)
path.Add(map.GetCellAt(nodePath[i].X, nodePath[i].Y));
// grab and print path data
float dist = (float)(nodePath[nodePath.Count - 1].Gscore);
span = DateTime.Now - startTime;
//printPath(path, dist);
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)
{
// 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>
/// The core function; calls all valid auxiliary functions and returns the path (advanced features can be toggled)
/// Includes a boolean toggle for use of advanced pathfinding functions
/// </summary>
/// <param name="map">The Map</param>
/// <param name="start">The starting Cell</param>
/// <param name="end">The ending Cell</param>
/// <param name="advanced"> A boolean toggle for advanced functions</param>
/// <returns>The path as a list of waypoints</returns>
public static List <Cell> between(Map map, Cell start, Cell end, bool advanced)
{
// begin timing the operation
startTime = DateTime.Now;
intendedEnd = end;
// convert the given Cell-based data to Node-based data
NodeMap nodeMap = new NodeMap(map);
Node nodeStart = nodeMap.getNode(start.Xcoord, start.Ycoord);
Node nodeEnd = nodeMap.getNode(end.Xcoord, end.Ycoord);
// perform advanced pre-calculation tasks
if (advanced)
{
// if the end Node is invalid, replace it with the nearest valid Node
if (!nodeEnd.isValid)
{
DateTime tempStartTime = DateTime.Now;
nodeEnd = Advanced.nearestValidEnd(nodeMap, nodeStart, nodeEnd);
TimeSpan tempSpan = DateTime.Now - tempStartTime;
Console.WriteLine("-> Path end changed from ({0}, {1}) to ({2}, {3}) in {4}", intendedEnd.Xcoord, intendedEnd.Ycoord, nodeEnd.Xcoord, nodeEnd.Ycoord, tempSpan);
}
}
// find the path
List <Node> nodePath = Basic.findPath(nodeMap, nodeStart, nodeEnd);
// perform advanced post-calculation tasks
if (advanced)
{
}
// convert the path from List<Node> format back to List<Cell> format
List <Cell> path = new List <Cell>(nodePath.Count);
for (int i = 0; i < nodePath.Count; i++)
{
path.Add(map.getCell(nodePath[i].Xcoord, nodePath[i].Ycoord));
}
// grab and print path data
float dist = (float)(nodePath[nodePath.Count - 1].Gscore);
span = DateTime.Now - startTime;
printPath(path, dist);
return(path);
}
/// <summary>
/// Iterates over the full Map and returns a closed List of Nodes
/// </summary>
/// <param name="map">The NodeMap to search</param>
/// <returns>A List of all closed Nodes</returns>
private static List <Node> createClosed(NodeMap map)
{
List <Node> closed = new List <Node>();
for (int j = 0; j < map.height; j++)
{
for (int i = 0; i < map.width; i++)
{
Node temp = map.getNode(i, j);
if (temp.isClosed)
{
closed.Add(temp);
}
}
}
return(closed);
}
/*
* 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;
}
/*
* public functions
*/
/// <summary>
/// The core function; calls all valid auxiliary functions and returns the path (advanced features can be toggled)
/// Includes a boolean toggle for use of advanced pathfinding functions
/// </summary>
/// <param name="map">The Map</param>
/// <param name="start">The starting Cell</param>
/// <param name="end">The ending Cell</param>
/// <param name="advanced"> A boolean toggle for advanced functions</param>
/// <returns>The path as a list of waypoints</returns>
public static List<Cell> between(Map map, Cell start, Cell end, bool advanced)
{
// begin timing the operation
startTime = DateTime.Now;
intendedEnd = end;
// convert the given Cell-based data to Node-based data
NodeMap nodeMap = new NodeMap(map);
Node nodeStart = nodeMap.getNode(start.Xcoord, start.Ycoord);
Node nodeEnd = nodeMap.getNode(end.Xcoord, end.Ycoord);
// perform advanced pre-calculation tasks
if (advanced)
{
// if the end Node is invalid, replace it with the nearest valid Node
if (!nodeEnd.isValid)
{
DateTime tempStartTime = DateTime.Now;
nodeEnd = Advanced.nearestValidEnd(nodeMap, nodeStart, nodeEnd);
TimeSpan tempSpan = DateTime.Now - tempStartTime;
Console.WriteLine("-> Path end changed from ({0}, {1}) to ({2}, {3}) in {4}", intendedEnd.Xcoord, intendedEnd.Ycoord, nodeEnd.Xcoord, nodeEnd.Ycoord, tempSpan);
}
}
// find the path
List<Node> nodePath = Basic.findPath(nodeMap, nodeStart, nodeEnd);
// perform advanced post-calculation tasks
if (advanced)
{
}
// convert the path from List<Node> format back to List<Cell> format
List<Cell> path = new List<Cell>(nodePath.Count);
for (int i = 0; i < nodePath.Count; i++)
path.Add(map.getCell(nodePath[i].Xcoord, nodePath[i].Ycoord));
// grab and print path data
float dist = (float)(nodePath[nodePath.Count - 1].Gscore);
span = DateTime.Now - startTime;
printPath(path, dist);
return path;
}
/*
* 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>
/// The core function; calls all valid auxiliary functions and returns the path (advanced features can be toggled)
/// Includes a boolean toggle for use of advanced pathfinding functions
/// </summary>
/// <param name="map">The Map</param>
/// <param name="start">The starting Cell</param>
/// <param name="end">The ending Cell</param>
/// <param name="advanced"> A boolean toggle for advanced functions</param>
/// <returns>The path as a list of waypoints</returns>
public static List <CellComponent> between(Map map, CellComponent start, CellComponent end, bool advanced)
{
// begin timing the operation
DateTime startTime = DateTime.Now;
// convert the given Cell-based data to Node-based data
NodeMap nodeMap = new NodeMap(map);
Node nodeStart = nodeMap.getNode(start.X, start.Y);
Node nodeEnd = nodeMap.getNode(end.X, end.Y);
// perform advanced pre-calculation tasks
if (advanced)
{
// if the end Node is invalid, replace it with the nearest valid Node
if (!nodeEnd.isValid)
{
nodeEnd = Advanced.nearestValidEnd(nodeMap, nodeStart, nodeEnd);
}
}
// find the path
List <Node> nodePath = Basic.findPath(nodeMap, nodeStart, nodeEnd);
// convert the path from List<Node> format back to List<Cell> format
List <CellComponent> path = new List <CellComponent>(nodePath.Count);
for (int i = 0; i < nodePath.Count; i++)
{
path.Add(map.GetCellAt(nodePath[i].X, nodePath[i].Y));
}
// grab and print path data
float dist = (float)(nodePath[nodePath.Count - 1].Gscore);
span = DateTime.Now - startTime;
//printPath(path, dist);
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);
}
}
/*
* helper functions
*/
/// <summary>
/// Puts all valid Nodes adjacent to the given Node in a PQueue and returns it
/// </summary>
/// <param name="map">The Map</param>
/// <param name="closed">The closed list</param>
/// <param name="currentNode">The current (center) Node</param>
/// <returns>A PQueue of all traversable adjacent Nodes</returns>
private static PQueue getAdjacentNodes(NodeMap map, PQueue open, List <Node> closed, Node currentNode)
{
int x = currentNode.Xcoord;
int y = currentNode.Ycoord;
List <Node> immediate = new List <Node>();
List <Node> diagonal = new List <Node>();
PQueue adjacentNodes = new PQueue();
// grab all adjacent Nodes (or null values) and store them here
Node[,] temp = map.getNodes(x - 1, y - 1, 3, 3);
// iterate over all adjacent Nodes; add the ones that are open and in bounds to the appropriate List<Node>
for (int j = 0; j < 3; j++)
{
for (int i = 0; i < 3; i++)
{
if (temp[i, j] != null && !closed.Contains(temp[i, j]))
{
// if the Node is horizontally or vertically adjacent,
// add the Node to the list of immediately adjacent Nodes
if (Math.Abs(2 - i - j) == 1)
{
immediate.Add(temp[i, j]);
}
// otherwise, if the Node is valid, add it to the list of diagonally adjacent Nodes
else if (temp[i, j].isValid)
{
diagonal.Add(temp[i, j]);
}
}
}
}
// iterate over all immediately adjacent Nodes. If they are valid, enqueue them;
// otherwise, remove the neighboring diagonally adjacent Nodes from the diagonal List
for (int i = 0; i < immediate.Count(); i++)
{
if (!immediate[i].isValid)
{
Node one, two = null;
if (immediate[i].Xcoord == x) // the Node is vertically adjacent
{
one = map.getNode(x + 1, immediate[i].Ycoord);
two = map.getNode(x - 1, immediate[i].Ycoord);
}
else // the Node is horizontally adjacent
{
one = map.getNode(immediate[i].Xcoord, y - 1);
two = map.getNode(immediate[i].Xcoord, y + 1);
}
if (one != null)
{
diagonal.Remove(one);
}
if (two != null)
{
diagonal.Remove(two);
}
}
else
{
adjacentNodes.enqueue(immediate[i]);
}
}
// enqueue all remaining diagonally adjacent Nodes
for (int i = 0; i < diagonal.Count(); i++)
{
adjacentNodes.enqueue(diagonal[i]);
}
// return the finished PQueue
return(adjacentNodes);
}
/*
* 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));
}
/// <summary>
/// Iterates over the full Map and returns a closed List of Nodes
/// </summary>
/// <param name="map">The NodeMap to search</param>
/// <returns>A List of all closed Nodes</returns>
private static List<Node> createClosed(NodeMap map)
{
List<Node> closed = new List<Node>();
for (int j = 0; j < map.height; j++)
{
for (int i = 0; i < map.width; i++)
{
Node temp = map.getNode(i, j);
if (temp.isClosed)
closed.Add(temp);
}
}
return closed;
}
/*
* 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;
}
}
/*
* helper functions
*/
/// <summary>
/// Puts all valid Nodes adjacent to the given Node in a PQueue and returns it
/// </summary>
/// <param name="map">The Map</param>
/// <param name="closed">The closed list</param>
/// <param name="currentNode">The current (center) Node</param>
/// <returns>A PQueue of all traversable adjacent Nodes</returns>
private static PQueue getAdjacentNodes(NodeMap map, PQueue open, List<Node> closed, Node currentNode)
{
int x = currentNode.Xcoord;
int y = currentNode.Ycoord;
List<Node> immediate = new List<Node>();
List<Node> diagonal = new List<Node>();
PQueue adjacentNodes = new PQueue();
// grab all adjacent Nodes (or null values) and store them here
Node[,] temp = map.getNodes(x - 1, y - 1, 3, 3);
// iterate over all adjacent Nodes; add the ones that are open and in bounds to the appropriate List<Node>
for (int j = 0; j < 3; j++)
{
for (int i = 0; i < 3; i++)
{
if (temp[i, j] != null && !closed.Contains(temp[i, j]))
{
// if the Node is horizontally or vertically adjacent,
// add the Node to the list of immediately adjacent Nodes
if (Math.Abs(2 - i - j) == 1)
immediate.Add(temp[i, j]);
// otherwise, if the Node is valid, add it to the list of diagonally adjacent Nodes
else if (temp[i, j].isValid)
diagonal.Add(temp[i, j]);
}
}
}
// iterate over all immediately adjacent Nodes. If they are valid, enqueue them;
// otherwise, remove the neighboring diagonally adjacent Nodes from the diagonal List
for (int i = 0; i < immediate.Count(); i++)
{
if (!immediate[i].isValid)
{
Node one, two = null;
if (immediate[i].Xcoord == x) // the Node is vertically adjacent
{
one = map.getNode(x + 1, immediate[i].Ycoord);
two = map.getNode(x - 1, immediate[i].Ycoord);
}
else // the Node is horizontally adjacent
{
one = map.getNode(immediate[i].Xcoord, y - 1);
two = map.getNode(immediate[i].Xcoord, y + 1);
}
if (one != null)
diagonal.Remove(one);
if (two != null)
diagonal.Remove(two);
}
else {
adjacentNodes.enqueue(immediate[i]);
}
}
// enqueue all remaining diagonally adjacent Nodes
for (int i = 0; i < diagonal.Count(); i++)
adjacentNodes.enqueue(diagonal[i]);
// return the finished PQueue
return adjacentNodes;
}