public void insert(Node node)
{
if (size == nodes.Length)
growArray();
nodes[size] = node;
node.positionInHeap = size;
size++;
percolateUp(size - 1);
}
public Path computePath(float startX, float startY, float endX, float endY, out bool pathFound)
{
timer.Reset();
timer.Start();
Heap open = new Heap();
//List<Node> closed = new List<Node>();
bool[,] closed = new bool[width, height];
int closedSize = 0;
Node[,] openMap = new Node[width, height];
Node goal = getNode((int) Math.Round(endX / SCALE), (int) Math.Round(endY / SCALE));
Node start = getNode((int) Math.Round(startX / SCALE), (int) Math.Round(startY / SCALE));
if (goal != null && start != null)
{
open.insert(start);
openMap[(int)start.getPosition().X, (int)start.getPosition().Y] = start;
}
//else a path can't be found so skip to the end and return default path.
while (!open.isEmpty())
{
//get node with lowest f value from open list (last entry in list)
Node n = open.removeMin();
openMap[(int)n.getPosition().X, (int)n.getPosition().Y] = null;
//closed.Add(n);
closed[(int)n.getPosition().X, (int)n.getPosition().Y] = true;
closedSize++;
if (n.getPosition() == goal.getPosition()) //found a path
{
Node parent = n.getParent();
List<Vector3> pathNodes = new List<Vector3>();
Vector2 pos = n.getPosition();
pathNodes.Add(new Vector3(pos.X * SCALE, pos.Y * SCALE, scaledHeightMap[(int)pos.X, (int)pos.Y]));
while (parent != null)
{
pos = parent.getPosition();
pathNodes.Add(new Vector3(pos.X * SCALE, pos.Y * SCALE, scaledHeightMap[(int)pos.X, (int)pos.Y]));
parent = parent.getParent();
}
pathNodes.Add(level.getPositionAt(startX, startY));
Console.WriteLine("closed size: " + closedSize);
Console.WriteLine(timer.ElapsedMilliseconds + "ms total to run A*");
timer.Stop();
pathNodes.Reverse();
pathNodes.Add(level.getPositionAt(endX, endY));
pathFound = true;
return new Path(pathNodes);
}
//generate successors for n
List<Node> successors = getSuccessors(n);
for (int i = 0, m = successors.Count; i < m; i++)
{
Node s = successors[i];
//if (!contains(closed, s))
if(closed[(int)s.getPosition().X, (int)s.getPosition().Y] == false)
{
//s.h is estimated distance to goal
//s.hVal = Vector2.Distance(s.getPosition(), goal.getPosition());
s.hVal = Vector2.DistanceSquared(s.getPosition(), goal.getPosition());
//s.g is n.g + cost from n to s
s.gVal = n.gVal + s.getDistanceToParent();
//If it isn’t on the open list, add it to the open list.
//Node oldNode = open.contains(s.getPosition(), out oldNodePos);
int sx = (int)s.getPosition().X;
int sy = (int)s.getPosition().Y;
Node oldNode = openMap[sx, sy];
if (oldNode == null)
{
open.insert(s);
openMap[sx, sy] = s;
}
else
{
//If it is on the open list already, check to see if this path to that square is better,
//using G cost as the measure. A lower G cost means that this is a better path.
//If so, change the parent of the square to the current square, and recalculate the G and F scores of the square.
//If you are keeping your open list sorted by F score, you may need to resort the list to account for the change.
if (s.gVal < oldNode.gVal)
{
oldNode.setParent(n);
oldNode.gVal = s.gVal;
open.reorderNode(oldNode.positionInHeap);
}
}
}
}
}
Console.WriteLine("closed size: " + closedSize);
Console.WriteLine("Took " + timer.ElapsedMilliseconds + "ms to not find a path");
timer.Stop();
//couldn't find a path so return a path containing just the start node
List<Vector3> noPath = new List<Vector3>();
noPath.Add(level.getPositionAt(startX, startY));
pathFound = false;
return new Path(noPath);
}
/**
* Determines if it is possible to move between two adjacent nodes.
*
* @param a The first node.
* @param b The second node.
* @return True if it is possible to move from a to b.
*/
private bool canMove(Node a, Node b)
{
Vector2 posA = a.getPosition();
Vector2 posB = b.getPosition();
float diff = Math.Abs(scaledHeightMap[(int)posA.X, (int)posA.Y] - scaledHeightMap[(int)posB.X, (int)posB.Y]);
return diff <= MAX_MOVE_DIST;
}
/**
* Gets a list of successor nodes for a specified node.
* Each successor node's parent is set to the speified node.
*
* @param node The node to get successors for.
*/
private List<Node> getSuccessors(Node node)
{
List<Node> successors = new List<Node>();
for (int i = 0; i < BORDER_NODES.Length; i++)
{
Vector2 pos = node.getPosition() + BORDER_NODES[i];
Node n = getNode((int)pos.X, (int)pos.Y);
if (n != null && canMove(node, n))
{
n.setParent(node);
successors.Add(n);
}
}
return successors;
}
/**
* Sets the parent of this node and calculates the
* distance to it. If a null parameter is provided
* then this node has no parent.
*
* @param parent The parent node to set or null if the node is to have no parent.
*/
public void setParent(Node parent)
{
this.parent = parent;
if (parent != null)
distToParent = Vector2.Distance(pos, parent.pos);
else
distToParent = 0.0f;
}