public NodeCollection(int gridSize)
{
nodes = new Node[gridSize, gridSize];
for (int x = 0; x < gridSize; x++)
for (int y = 0; y < gridSize; y++)
nodes[x, y] = new Node();
}
/*
* 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);
}
//This method should never be called on it's own.
//It is recursively called by FindPath()
public bool Search(Node currentNode) {
//Mark the current node as considered, to never check again.
currentNode.State = Node.NodeState.Closed;
//Get all of the neighbouring nodes that aren't walls and are in bounds.
List<Node> nextNodes = GetAdjacentWalkableNodes(currentNode);
//Sort the list by the cheapest nodes.
nextNodes.Sort((node1, node2) => node1.F.CompareTo(node2.F));
foreach (var nextNode in nextNodes) {
//Has the path been reached?
if (nextNode.X == goalX && nextNode.Y == goalY) {
return true;
}
else {
//Have we reached a dead end?
if (Search (nextNode))
return true;
//If this isn't true, we reached a dead end.
}
}
//The starting node is a wall itself.
return false;
}
public Node(MapCell cell, Node parent, float cost, float heur)
{
Cell = cell;
Heuristic = heur;
Parent = parent;
Cost = cost;
}
public Node()
{
cost = INITIAL_COST;
inPath = false;
closed = false;
parent = null;
position = new Coord2(-1, -1);
}
/// <summary>
/// Returns a Node[,] representation of a subsection of the map. Coordinates outside map bounds are returned as null
/// </summary>
/// <param name="x">The starting X-coordinate</param>
/// <param name="y">The starting Y-coordinate</param>
/// <param name="width">Subsection width</param>
/// <param name="height">Subsection height</param>
/// <returns>A Node[,] subsection of the map</returns>
public Node[,] getNodes(int x, int y, int width, int height)
{
Node[,] temp = new Node[width, height];
for (int j = 0; j < height; j++)
{
for (int i = 0; i < width; i++)
{
temp[i, j] = getNode(i + x, j + y);
}
}
return temp;
}
/*
* constructors
*/
/// <summary>
/// Default constructor
/// </summary>
/// <param name="m">The Gameworld.Map to copy dimensional and validity data from</param>
public NodeMap(Map m)
{
this.height = m.height;
this.width = m.width;
this.nodes = new Node[width, height];
for (int j = 0; j < height; j++)
{
for (int i = 0; i < width; i++)
{
nodes[i, j] = new Node(m.getCell(i, j).Xcoord, m.getCell(i,j).Ycoord, m.getCell(i,j).isValid);
}
}
}
/*
* constructors
*/
/// <summary>
/// Default constructor
/// </summary>
/// <param name="m">The Gameworld.Map to copy dimensional and validity data from</param>
public NodeMap(Map m)
{
this.height = m.height;
this.width = m.width;
Console.WriteLine(" Node Map Dimensions: ({0}, {1})", this.width, this.height);
this.nodes = new Node[width, height];
for (int j = 0; j < height; j++)
{
for (int i = 0; i < width; i++)
{
nodes[i, j] = new Node(m.getCell(i, j).Xcoord, m.getCell(i,j).Ycoord, m.getCell(i,j).isValid);
}
}
}
/*
* 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.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;
}
/// <summary>
/// Inserts the given Node by its Fscore
/// </summary>
/// <param name="Node">The Node to insert</param>
public void enqueue(Node node)
{
if (node == null)
return;
node.open();
if (pq.Count == 0)
pq.Add(node);
else
{
int i = 0;
while (i < pq.Count)
{
if (node.Fscore >= pq[i].Fscore)
i++;
else
break;
}
pq.Insert(i, node);
}
}
//Populate the map with nodes
public PathAlgorithm(int width, int height) {
nodes = new Node[width, height];
//Instantiate a new node for every tile in the map
for (int i = 0; i < nodes.GetLength (0); i++)
for (int j = 0; j < nodes.GetLength (1); j++) {
nodes [i, j] = new Node (i, j);
//Gets the path cost of the starting node to avoid null reference exceptions
if (nodes [i, j].ParentNode == null)
nodes [i, j].G = 0;
//The path cost of the node is the parent's cost + the cost to get to this tile. (Running total)
else
nodes [i, j].G = nodes [i, j].ParentNode.G + 1;
//The remaining cost is estimated with the Manhattan Distance heuristic. (Faster than Euclidean Distance)
nodes [i, j].H = Math.Abs (goalX - nodes [i, j].X) + Math.Abs (goalY - nodes [i, j].Y);
//The total cost is the G cost + the H cost
nodes [i, j].F = nodes [i, j].G + nodes [i, j].H;
}
}
/// <summary>
/// Find the node with the lowest cost.
/// </summary>
protected virtual void FindLowestCost()
{
currentLowest = target;
for (int x = 0; x < GridSize; x++)
{
for (int y = 0; y < GridSize; y++)
{
// If cost is lower than current, position not closed, and position is valid within level, new lowest is found
if (nodes.Get(currentLowest.position).cost >= nodes.Get(x, y).cost && nodes.Get(x, y).closed == false &&
map.ValidPosition(new Coord2(x, y)))
currentLowest = nodes.Get(x, y);
}
}
}
/// <summary>
/// Get the valid neighbour locations for the given node.
/// </summary>
protected virtual List<Node> GetNeighours(Node node)
{
List<Node> list = new List<Node>();
// Horizontal and vertical
if (map.ValidPosition(node.position + new Coord2(1, 0)))
list.Add(nodes.Get(node.position + new Coord2(1, 0)));
if (map.ValidPosition(node.position + new Coord2(-1, 0)))
list.Add(nodes.Get(node.position + new Coord2(-1, 0)));
if (map.ValidPosition(node.position + new Coord2(0, 1)))
list.Add(nodes.Get(node.position + new Coord2(0, 1)));
if (map.ValidPosition(node.position + new Coord2(0, -1)))
list.Add(nodes.Get(node.position + new Coord2(0, -1)));
// Diagonal
if (map.ValidPosition(node.position + new Coord2(1, 1)))
list.Add(nodes.Get(node.position + new Coord2(1, 1)));
if (map.ValidPosition(node.position + new Coord2(-1, -1)))
list.Add(nodes.Get(node.position + new Coord2(-1, -1)));
if (map.ValidPosition(node.position + new Coord2(1, -1)))
list.Add(nodes.Get(node.position + new Coord2(1, -1)));
if (map.ValidPosition(node.position + new Coord2(-1, 1)))
list.Add(nodes.Get(node.position + new Coord2(-1, 1)));
return list;
}
/// <summary>
/// Clears the current node collection.
/// </summary>
public void Clear()
{
for (int x = 0; x < nodes.GetLength(0); x++)
for (int y = 0; y < nodes.GetLength(1); y++)
nodes[x, y] = new Node();
}
/// <summary>
/// Create a new collection of nodes.
/// </summary>
/// <param name="gridSize">The size of the node grid.</param>
public NodeCollection(int gridSize)
{
nodes = new Node[gridSize, gridSize];
this.gridSize = gridSize;
for (int x = 0; x < gridSize; x++)
for (int y = 0; y < gridSize; y++)
nodes[x, y] = new Node(new Coord2(x, y));
}
/// <summary>
/// Recalcualtes the costs in a set of neighbours based upon the given parent node.
/// </summary>
/// <param name="neighbours">The neighbours to recalculate costs for.</param>
/// <param name="node">The parent node to use.</param>
protected virtual void RecalculateCosts(List<Node> neighbours, Node node)
{
for (int i = 0; i < neighbours.Count; i++)
{
if(map.ValidPosition(neighbours[i].position) && neighbours[i].closed == false)
{
float costToAdd = 0.0f;
if (neighbours[i].position.X != 0 && neighbours[i].position.Y != 0)
costToAdd = D_COST;
else
costToAdd = HV_COST;
float newCost = nodes.Get(node.position).cost + costToAdd;
if (newCost < neighbours[i].cost)
{
neighbours[i].cost = newCost;
neighbours[i].parent = node;
}
}
}
}
public Point[] FindPath(Point start, Point end)
{
//Ищем путь в кэше, если находим, то тут же завершаемся
Point[] PrecompPath = ExaminePathCache(start, end);
if (PrecompPath != null)
return PrecompPath;
List<Point> p = new List<Point>(); //конечный список
current = new Node(start, 0, 0, 0);
current.H = calcH(start, end);
current.F = current.H;
openList.Add(current);
existsArray[current.P.X, current.P.Y] = 1;
bool ex = false;
bool can = true;
while (!ex)
{
int i = findMinF(openList);
current = openList[i];
openList.RemoveAt(i);
closeList.Add(current);
existsArray[current.P.X, current.P.Y] = 2;
//проверка соседних клеток
Point nb = current.P;
nbPoint(new Point(nb.X + 1, nb.Y), current, end, x, y);
nbPoint(new Point(nb.X, nb.Y - 1), current, end, x, y);
nbPoint(new Point(nb.X - 1, nb.Y), current, end, x, y);
nbPoint(new Point(nb.X, nb.Y + 1), current, end, x, y);
if (openList.Count == 0) //открытый списко пустой - путь невозможен
{
ex = true;
can = false;
}
if (existsArray[end.X, end.Y] == 1) //дошли до финала
ex = true;
}
//составление искомого пути
if (can)
{
p.Add(end);
Point pp = end;
int i = 0;
while (pp != start)
{
if (existsArray[pp.X, pp.Y] == 1)
{
i = inList(pp, openList);
if (i < 0)
break;
pp = openList[i].Parent;
}
else if (existsArray[pp.X, pp.Y] == 2)
{
i = inList(pp, closeList);
if (i < 0)
break;
pp = closeList[i].Parent;
}
p.Add(pp);
}
}
Point[] ret = new Point[p.Count];
for (int i = 0; i < p.Count; i++)
ret[i] = p[p.Count - i - 1];
clear();
pathCache.Add(new PrecomputedPath(start, end, ret)); //запись пути в кеш
return ret;
}
public List<Node> GetAdjacentWalkableNodes(Node fromNode) {
//All of the nodes that are in the bounds
List<Node> validNodes = new List<Node> ();
//All of the nodes that are not walls
List<Node> walkableNodes = new List<Node> ();
//The nodes that aren't closed, and are untested or have a shorter path.
List<Node> consideredNodes = new List<Node> ();
//If the nodes are in the map boundaries.
if (fromNode.X + 1 < Game1.mapWidth)
validNodes.Add(nodes[fromNode.X + 1, fromNode.Y]);
if (fromNode.X - 1 > -1)
validNodes.Add (nodes [fromNode.X - 1, fromNode.Y]);
if (fromNode.Y + 1 < Game1.mapHeight)
validNodes.Add (nodes [fromNode.X, fromNode.Y + 1]);
if (fromNode.Y - 1 > -1)
validNodes.Add (nodes [fromNode.X, fromNode.Y - 1]);
//If the nodes are walkable add them to the walkable nodes
foreach (var node in validNodes) {
if (node.IsWalkable ())
walkableNodes.Add (node);
}
//ToArray() is necessary to prevent a collections modified enumeration error
foreach (var node in walkableNodes.ToArray()) {
//If the node is closed already, don't consider it.
if (node.State == Node.NodeState.Closed)
continue;
else if (node.State == Node.NodeState.Open) {
//float traversalCost = Node.GetTraversalCost (node.X, node.Y, node.ParentNode.X, node.ParentNode.Y);
float traversalCost = node.G + node.ParentNode.G;
float gTemp = fromNode.G + traversalCost;
//If you have found a better path to an already considered node, update it and consider it.
if (gTemp < node.G) {
node.ParentNode = fromNode;
consideredNodes.Add (node);
}
//If it hasn't been tested already consider it.
} else {
node.ParentNode = fromNode;
node.State = Node.NodeState.Open;
consideredNodes.Add (node);
}
}
return consideredNodes;
}
private int calcG(Node node)
{
int g = current.G;
if (tissue[node.P.X, node.P.Y].AreaType == VG.Map.AreaEnum.LowDensity)
g += 2;
else if (tissue[node.P.X, node.P.Y].AreaType == VG.Map.AreaEnum.MediumDensity)
g += 3;
else if (tissue[node.P.X, node.P.Y].AreaType == VG.Map.AreaEnum.HighDensity)
g += 4;
VG.Map.BloodStream bs = tissue.IsInStream(node.P.X, node.P.Y);
if (bs != null)
{
int dy = node.P.Y - current.P.Y;
int dx = node.P.X - current.P.X;
#region Direction
bool plus = false;
if (bs.Direction == VG.Map.BloodStreamDirection.NorthSouth)
{
if (dy >= 0)
plus = false;
else
plus = true;
}
else if (bs.Direction == VG.Map.BloodStreamDirection.SouthNorth)
{
if (dy <= 0)
plus = false;
else
plus = true;
}
else if (bs.Direction == VG.Map.BloodStreamDirection.EstWest)
{
if (dx <= 0)
plus = false;
else
plus = true;
}
else if (bs.Direction == VG.Map.BloodStreamDirection.WestEst)
{
if (dx >= 0)
plus = false;
else
plus = true;
}
if (tissue[node.P.X, node.P.Y].AreaType == VG.Map.AreaEnum.LowDensity)
{
if (plus)
g += 2;
else
g -= 1;
}
else
{
if (plus)
g += 2;
else
g -= 2;
}
#endregion
}
return g;
}
/// <summary>
/// Recalcualtes the costs in a set of neighbours based upon the given parent node.
/// </summary>
/// <param name="neighbours">The neighbours to recalculate costs for.</param>
/// <param name="node">The parent node to use.</param>
protected override void RecalculateCosts(List<Node> neighbours, Node node)
{
for (int i = 0; i < neighbours.Count; i++)
{
if(map.ValidPosition(neighbours[i].position) && neighbours[i].closed == false)
{
float costToAdd = 0.0f;
Vector2 dirToNode = new Vector2(MathHelper.Clamp(neighbours[i].position.X - node.position.X, -1, 1),
MathHelper.Clamp(neighbours[i].position.Y - node.position.Y, -1, 1));
if (dirToNode.X != 0 && dirToNode.Y != 0)
costToAdd = D_COST;
else
costToAdd = HV_COST;
float newCost = nodes.Get(node.position.X, node.position.Y).cost + costToAdd;
if (newCost < neighbours[i].cost)
{
neighbours[i].cost = newCost;
neighbours[i].parent = node;
}
}
}
}
/*
* 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;
}
}
/// <summary>
/// Builds a new path between two given locations.
/// </summary>
/// <param name="startPos">The starting position.</param>
/// <param name="targetPos">The target position.</param>
/// <param name="testMode">Whether or not the algorithm is being run in testing mode, or if it is live within a map visualization.</param>
public override void Build(Coord2 startPos, Coord2 targetPos, bool testMode = false)
{
path = new List<Coord2>();
nodes = new NodeCollection(GridSize);
searchedNodes = 0;
this.start = nodes.Get(startPos);
this.target = nodes.Get(targetPos);
// Initialize bot position
nodes.Get(startPos).cost = 0;
bool firstLoop = true;
while (!target.closed)
{
if (firstLoop)
{
currentLowest = start;
firstLoop = false;
}
else
FindLowestCost(); // Find lowest cost
// Mark lowest cost as closed
currentLowest.closed = true;
map.SetRenderColor(currentLowest.position, CLOSED_COLOR);
// Find the neigbour positions
List<Node> neighbours = GetNeighours(currentLowest);
// Update visualization
UpdateVisualization(neighbours);
// Recalculate Costs
RecalculateCosts(neighbours, currentLowest);
// Update number of searched nodes
searchedNodes++;
}
// Trace the completed path, if target has been found
if(target.parent != null)
TracePath();
}
/*
* 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;
}
/// <summary>
/// Returns true if the PQueue contains the given Node, false otherwise
/// </summary>
/// <param name="Node">The Node to check for</param>
/// <returns>True if the PQueue contains the given Node, false otherwise</returns>
public bool contains(Node node)
{
return pq.Contains(node);
}
/// <summary>
/// Calculates the minimum path-based distance between two Nodes
/// </summary>
/// <param name="one">The first Node</param>
/// <param name="two">The second Node</param>
/// <returns>A rough integer distance between the two; 10 per vertical/horizontal move, 14 per vertical move</returns>
public int pathDistance(Node one, Node two)
{
/*
* Theory
* This function is used to calculate the path-based distance between two Nodes;
* i.e., the shortest possible path you can take from point A to point B
* when you can only move at 45-degree and 90-degree angles.
* The diagonal distance is multiplied by 14 instead of the square root of 2 (~1.41) to avoid using floating-point numbers;
* the straight distance is multiplied by 10 to compensate.
*/
int x = Math.Abs(one.Xcoord - two.Xcoord);
int y = Math.Abs(one.Ycoord - two.Ycoord);
int diagonal = Math.Min(x, y);
int straight = Math.Max(x, y) - diagonal;
return (10 * straight) + (14 * diagonal);
}
/// <summary>
/// A wrapper function for path reconstruction
/// </summary>
/// <param name="endNode">The destination Node</param>
/// <returns>The path as a list of waypoints</returns>
private static List<Node> reconstruct(Node endNode)
{
List<Node> path = new List<Node>();
return reconstruct(endNode, path);
}
//функция обрабатывющая "соседнюю" клетку
private void nbPoint(Point nb, Node current, Point end, int X, int Y)
{
//различные проверки клетки - границы массива,закрытый список итд.
if (nb.X >= X || nb.X < 0)
return;
if (nb.Y >= Y || nb.Y < 0)
return;
if (existsArray[nb.X, nb.Y] == 2)
return;
if (!tissue.IsInMap(nb.X, nb.Y))
return;
if( tissue[nb.X,nb.Y].AreaType == VG.Map.AreaEnum.Bone )
return;
//если нет в открытом списке - добавляем
if(existsArray[nb.X,nb.Y] !=1 )
{
Node n = new Node(nb, 0, 0, 0);
n.H = calcH(nb, end);
n.G = calcG(n);
n.F = n.G + n.H;
n.Parent = current.P;
openList.Add(n);
existsArray[n.P.X, n.P.Y] = 1;
}
else //если есть - страдаем некоторой фигней =)
{
int j = inList(nb, openList);
Node n = openList[j];
if (n.G > calcG(n))
{
n.Parent = current.P;
n.G = calcG(n);
n.F = n.G + n.H;
openList.RemoveAt(j);
openList.Add(n);
}
}
}
/// <summary>
/// Actual path reconstruction
/// </summary>
/// <param name="endNode">The ending Node</param>
/// <param name="path">The (initially blank) path</param>
/// <returns>The path as a list of waypoints</returns>
private static List<Node> reconstruct(Node endNode, List<Node> path)
{
// recurse if necessary
if (endNode.prev != null)
path = reconstruct(endNode.prev, path);
// if we are moving in the same direction we moved in last time,
// there is no need to keep the previous Node in the path, so remove it
if (path.Count >= 2)
{
int i = path.Count - 1;
int prevX = path[i].Xcoord - path[i - 1].Xcoord;
int prevY = path[i].Ycoord - path[i - 1].Ycoord;
int curX = endNode.Xcoord - path[i].Xcoord;
int curY = endNode.Ycoord - path[i].Ycoord;
if (theta(prevX, prevY) == theta(curX, curY))
path.RemoveAt(i);
}
// add the current Node and return
path.Add(endNode);
return path;
}
/// <summary>
/// Sets a Cell's "adjacent stats." Called whenever an adjacent Cell is added to the Open structure or otherwise updated.
/// </summary>
/// <param name="adjacent">The adjacent Cell to set/reset</param>
/// <param name="current">The current Cell we are adjacent to</param>
/// <param name="gScore">The adjacent Cell's calculated gScore</param>
/// <param name="hScore">The adjacent Cell's calculated hScore</param>
private static void setAdjacentStats(Node adjacent, Node current, int gScore, int hScore)
{
adjacent.prev = current; // set the Cell's prev pointer to the current Cell
adjacent.Hscore = hScore; // set the Cell's Hscore
adjacent.Gscore = gScore; // set the Cell's Gscore
adjacent.Fscore = gScore + hScore; // set the Cell's Fscore
}