private void Expand()
{
int num = BinaryHeap.RoundUpToNextMultipleMod1(Math.Max(this.heap.Length + 4, (int)Math.Round((double)((float)this.heap.Length * this.growthFactor))));
if (num > 262144)
{
throw new Exception("Binary Heap Size really large (2^18). A heap size this large is probably the cause of pathfinding running in an infinite loop. \nRemove this check (in BinaryHeap.cs) if you are sure that it is not caused by a bug");
}
BinaryHeap.Tuple[] array = new BinaryHeap.Tuple[num];
for (int i = 0; i < this.heap.Length; i++)
{
array[i] = this.heap[i];
}
this.heap = array;
}
/** Initializes the path. Sets up the open list and adds the first node to it */
public virtual void Initialize()
{
System.DateTime startTime = System.DateTime.Now;
//Resets the binary heap, don't clear it because that takes an awful lot of time, instead we can just change the numberOfItems in it (which is just an int)
//Binary heaps are just like a standard array but are always sorted so the node with the lowest F value can be retrieved faster
open = AstarPath.active.binaryHeap;
open.numberOfItems = 1;
if (hasEndPoint && startNode == endNode)
{
endNode.parent = null;
endNode.h = 0;
endNode.g = 0;
Trace(endNode);
foundEnd = true;
return;
}
//Adjust the costs for the end node
if (hasEndPoint && recalcStartEndCosts)
{
endNodeCosts = endNode.InitialOpen(open, hTarget, (Int3)endPoint, this, false);
callback += ResetCosts; /** \todo Might interfere with other paths since other paths might be calculated before #callback is called */
}
Node.activePath = this;
startNode.pathID = pathID;
startNode.parent = null;
startNode.cost = 0;
startNode.g = startNode.penalty;
startNode.UpdateH(hTarget, heuristic, heuristicScale);
if (recalcStartEndCosts)
{
startNode.InitialOpen(open, hTarget, startIntPoint, this, true);
}
else
{
startNode.Open(open, hTarget, this);
}
searchedNodes++;
//any nodes left to search?
if (open.numberOfItems <= 1)
{
LogError("No open points, the start node didn't open any nodes");
duration += (System.DateTime.Now.Ticks - startTime.Ticks) * 0.0001F;
return;
}
current = open.Remove();
duration += (System.DateTime.Now.Ticks - startTime.Ticks) * 0.0001F;
}
public BinaryHeap(int capacity)
{
capacity = BinaryHeap.RoundUpToNextMultipleMod1(capacity);
this.heap = new BinaryHeap.Tuple[capacity];
this.numberOfItems = 0;
}
/** Updates G score for this node and nodes which have this node set as #parent */
public virtual void UpdateAllG(BinaryHeap open)
{
BaseUpdateAllG (open);
}
/** Opens the nodes connected to this node */
public virtual void Open(BinaryHeap open, Int3 targetPosition, Path path)
{
BaseOpen (open,targetPosition,path);
}
public virtual int[] InitialOpen(BinaryHeap open, Int3 targetPosition, Int3 position, Path path, bool doOpen)
{
return BaseInitialOpen (open,targetPosition,position,path,doOpen);
}
/** Updates G score for this node and nodes which have this node set as #parent. This is to allow inheritance in higher levels than one, you can't call base.base in virtual functions */
public void BaseUpdateAllG(BinaryHeap open)
{
g = parent.g+cost+penalty;
open.Add (this);
if (connections == null) {
return;
}
//Loop through the connections of this node and call UpdateALlG on nodes which have this node set as #parent and has been searched by the pathfinder for this path */
for (int i=0;i<connections.Length;i++) {
if (connections[i].parent == this && connections[i].pathID == pathID) {
connections[i].UpdateAllG (open);
}
}
}
/** Opens the nodes connected to this node. This is a base call and can be called by node classes overriding the Open function to open all connections in the #connections array.
* \see #connections
* \see Open */
public void BaseOpen(BinaryHeap open, Int3 targetPosition, Path path)
{
if (connections == null) {
return;
}
for (int i=0;i<connections.Length;i++) {
Node node = connections[i];
if (!path.CanTraverse (node)) {
continue;
}
if (node.pathID != pathID) {
node.parent = this;
node.pathID = pathID;
node.cost = connectionCosts[i];
node.UpdateH (targetPosition, path.heuristic, path.heuristicScale);
node.UpdateG ();
open.Add (node);
//Debug.DrawLine (position,node.position,Color.cyan);
//Debug.Log ("Opening Node "+node.position.ToString ()+" "+g+" "+node.cost+" "+node.g+" "+node.f);
} else {
//If not we can test if the path from the current node to this one is a better one then the one already used
int tmpCost = connectionCosts[i];//(current.costs == null || current.costs.Length == 0 ? costs[current.neighboursKeys[i]] : current.costs[current.neighboursKeys[i]]);
//Debug.Log ("Trying Node "+node.position.ToString ()+" "+(g+tmpCost+node.penalty)+" "+node.g+" "+node.f);
//Debug.DrawLine (position,node.position,Color.yellow);
if (g+tmpCost+node.penalty < node.g) {
node.cost = tmpCost;
//node.extraCost = extraCost2;
node.parent = this;
node.UpdateAllG (open);
open.Add (node);
//Debug.DrawLine (current.vectorPos,current.neighbours[i].vectorPos,Color.cyan); //Uncomment for @Debug
}
else if (node.g+tmpCost+penalty < g) {//Or if the path from this node ("node") to the current ("current") is better
bool contains = false;
//Make sure we don't travel along the wrong direction of a one way link now, make sure the Current node can be moved to from the other Node.
if (node.connections != null) {
for (int y=0;y<node.connections.Length;y++) {
if (node.connections[y] == this) {
contains = true;
break;
}
}
}
if (!contains) {
continue;
}
parent = node;
cost = tmpCost;
//extraCost = extraCost2;
UpdateAllG (open);
//open.Add (this);
//Debug.DrawLine (current.vectorPos,current.neighbours[i].vectorPos,Color.blue); //Uncomment for @Debug
open.Add (this);
}
}
}
}
public int[] BaseInitialOpen(BinaryHeap open, Int3 targetPosition, Int3 position, Path path, bool doOpen)
{
if (connectionCosts == null) {
return null;
}
int[] costs = connectionCosts;
connectionCosts = new int[connectionCosts.Length];
for (int i=0;i<connectionCosts.Length;i++) {
connectionCosts[i] = (connections[i].position-position).costMagnitude;
}
if (!doOpen) {
for (int i=0;i<connectionCosts.Length;i++) {
Node other = connections[i];
for (int q = 0;q < other.connections.Length;q++) {
if (other.connections[q] == this) {
other.connectionCosts[q] = connectionCosts[i];
break;
}
}
}
}
//int[] tmp = connectionCosts;
//Should we open the node and reset the distances after that or only calculate the distances and don't reset them
if (doOpen) {
Open (open,targetPosition,path);
connectionCosts = costs;
/*for (int i=0;i<connectionCosts.Length;i++) {
for (int q = 0;q < connections[i].connections.Length;q++) {
if (connections[i].connections[q] == this) {
connections[i].connectionCosts[q] = connectionCosts[i];
break;
}
}
}*/
}
return costs;
}
void UpdateAllG (BinaryHeap open) {
BaseUpdateAllG (open);
}
void Open (BinaryHeap open, Int3 targetPosition, Path path) {
base.Open (open, targetPosition, path);
GridGraph graph = gridGraphs[indices >> 24];
int[] neighbourOffsets = graph.neighbourOffsets;
int[] neighbourCosts = graph.neighbourCosts;
GridNode[] nodes = graph.graphNodes;
int index = indices & 0xFFFFFF;
for (int i=0;i<8;i++) {
if (((flags >> i) & 1) == 1) {
Node node = nodes[index+neighbourOffsets[i]];
if (!path.CanTraverse (node)) continue;
if (node.pathID != pathID) {
node.parent = this;
node.pathID = pathID;
node.cost = neighbourCosts[i];
node.UpdateH (targetPosition,path.heuristic,path.heuristicScale);
node.UpdateG ();
open.Add (node);
} else {
//If not we can test if the path from the current node to this one is a better one then the one already used
int tmpCost = neighbourCosts[i];//(current.costs == null || current.costs.Length == 0 ? costs[current.neighboursKeys[i]] : current.costs[current.neighboursKeys[i]]);
if (g+tmpCost+node.penalty < node.g) {
node.cost = tmpCost;
//node.extraCost = extraCost2;
node.parent = this;;
node.UpdateAllG (open);
//open.Add (node);
//Debug.DrawLine (current.vectorPos,current.neighbours[i].vectorPos,Color.cyan); //Uncomment for @Debug
}
else if (node.g+tmpCost+penalty < g) {//Or if the path from this node ("node") to the current ("current") is better
/*bool contains = false;
//[Edit, no one-way links between nodes in a single grid] Make sure we don't travel along the wrong direction of a one way link now, make sure the Current node can be accesed from the Node.
/*for (int y=0;y<node.connections.Length;y++) {
if (node.connections[y].endNode == this) {
contains = true;
break;
}
}
if (!contains) {
continue;
}*/
parent = node;
cost = tmpCost;
//extraCost = extraCost2;
UpdateAllG (open);
//open.Add (this);
//Debug.DrawLine (current.vectorPos,current.neighbours[i].vectorPos,Color.blue); //Uncomment for @Debug
//open.Add (this);
}
}
}
}
}
public override int[] InitialOpen (BinaryHeap open, Int3 targetPosition, Int3 position, Path path, bool doOpen) {
if (doOpen) {
Open (open,targetPosition,path);
}
return base.InitialOpen (open,targetPosition,position,path,doOpen);
}
void UpdateAllG (BinaryHeap open) {
//g = parent.g+cost+penalty;
//f = g+h;
base.UpdateAllG (open);
//Called in the base function
//open.Add (this);
int index = indices & 0xFFFFFF;
int[] neighbourOffsets = gridGraphs[indices >> 24].neighbourOffsets;
GridNode[] nodes = gridGraphs[indices >> 24].graphNodes;
for (int i=0;i<8;i++) {
if (((flags >> i) & 1) == 1) {
Node node = nodes[index+neighbourOffsets[i]];
if (node.parent == this && node.pathID == pathID) {
node.UpdateAllG (open);
}
}
}
}
public Stack <int[]> GetPath(int startX, int startY, int endX, int endY, NodeMap nodeMap)
{
//Initalize start and end position using Vector2
var startPosition = new Vector2(startX, startY);
var endPosition = new Vector2(endX, endY);
//Initalize MinHeap PriorityQueue
IPriorityQueue <Node, float> openQueue = new BinaryHeap <Node, float>(PriorityQueueType.Minimum);
//Initalize MinHeap PriorityQueue which tracks which Nodes are modified in finding path
IPriorityQueue <Node, float> modifiedQueue = new BinaryHeap <Node, float>(PriorityQueueType.Minimum);
//Initalize list which holds Nodes adjacent to currentNode
var adjacentNodes = new List <Node>();
//Initialize stack which contains path
var path = new Stack <int[]>();
//Set startNode and endNode and begin pathfinding
var startNode = nodeMap.GetNode(startPosition);
var endNode = nodeMap.GetNode(endPosition);
startNode.Reset();
endNode.Reset();
startNode.h = calculateH(startPosition, endPosition);
startNode.f = startNode.h;
startNode.opened = true;
modifiedQueue.Enqueue(startNode, startNode.f);
openQueue.Enqueue(startNode, startNode.f);
//Check if either startNode or endNode have a weight of infinity
//if (float.IsInfinity(startNode.w) || float.IsInfinity(endNode.w))
//If so, a path cannot exist so terminate pathfinding
//return path;
while (openQueue.Count != 0)
{
//Set currentNode to Node with the lowest F value
var currentNode = openQueue.Dequeue();
//Mark it as closed since it has been checked
currentNode.closed = true;
//Check if this Node is the end position
if (currentNode.position.X == endPosition.X && currentNode.position.Y == endPosition.Y)
{
//If so, construct final path
endNode = currentNode;
path = Traceback(endNode);
break;
}
//Get all Nodes adjacent to the current position
adjacentNodes = nodeMap.GetAdjacentNodes(currentNode.position);
foreach (var adjacentNode in adjacentNodes)
{
if (adjacentNode != endNode)
{
adjacentNode.w = CalculateWeight(adjacentNode.position);
}
//Check if Node has been closed or the Node has a weight of infinity
if (adjacentNode.closed || float.IsInfinity(adjacentNode.w))
{
//If so, skip Node
continue;
}
//Calculate the total greed valuie from currentNode and adjacent Node
var gtotal = currentNode.g + adjacentNode.g + adjacentNode.w;
//Check if this Node has not yet been observed yet
if (adjacentNode.opened == false)
{
//If not, initialize Node
adjacentNode.g = gtotal;
adjacentNode.h = CalculateHeuristic(adjacentNode.position, endPosition);
adjacentNode.f = adjacentNode.g + adjacentNode.h;
adjacentNode.parent = currentNode;
adjacentNode.opened = true;
//Add it to the openQueue
openQueue.Enqueue(adjacentNode, adjacentNode.f);
//Every Node that is opened is modified so add Node to modifiedQueue
modifiedQueue.Enqueue(adjacentNode, adjacentNode.f);
}
else
{
//Otherwise check if this Node is a better connection
if (gtotal + adjacentNode.h < adjacentNode.f)
{
//If so, update Node's greed value and parent
adjacentNode.g = gtotal;
adjacentNode.f = adjacentNode.g + adjacentNode.h;
adjacentNode.parent = currentNode;
}
}
}
}
//Check if no valid path is found
if (path.Count == 0)
{
//If so, find best possible path to Node closest to goal
path = Traceback(modifiedQueue.Peek);
}
//Reset every modifiedNode for future pathfinding
while (modifiedQueue.Count != 0)
{
modifiedQueue.Dequeue().Reset();
}
return(path);
}
