public void Add(PathNode node) {
if (node == null) throw new System.ArgumentNullException ("Sending null node to BinaryHeap");
if (numberOfItems == binaryHeap.Length) {
int newSize = System.Math.Max(binaryHeap.Length+4,(int)System.Math.Round(binaryHeap.Length*growthFactor));
if (newSize > 1<<18) {
throw new System.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");
}
PathNode[] tmp = new PathNode[newSize];
for (int i=0;i<binaryHeap.Length;i++) {
tmp[i] = binaryHeap[i];
}
#if ASTARDEBUG
Debug.Log ("Resizing binary heap to "+newSize);
#endif
binaryHeap = tmp;
//Debug.Log ("Forced to discard nodes because of binary heap size limit, please consider increasing the size ("+numberOfItems +" "+binaryHeap.Length+")");
//numberOfItems--;
}
binaryHeap[numberOfItems] = node;
//node.heapIndex = numberOfItems;//Heap index
int bubbleIndex = numberOfItems;
uint nodeF = node.F;
while (bubbleIndex != 1) {
int parentIndex = bubbleIndex / 2;
if (nodeF < binaryHeap[parentIndex].F) {
//binaryHeap[bubbleIndex].f <= binaryHeap[parentIndex].f) { /* \todo Wouldn't it be more efficient with '<' instead of '<=' ? * /
//Node tmpValue = binaryHeap[parentIndex];
//tmpValue.heapIndex = bubbleIndex;//HeapIndex
binaryHeap[bubbleIndex] = binaryHeap[parentIndex];
binaryHeap[parentIndex] = node;//binaryHeap[bubbleIndex];
//binaryHeap[bubbleIndex].heapIndex = bubbleIndex; //Heap index
//binaryHeap[parentIndex].heapIndex = parentIndex; //Heap index
bubbleIndex = parentIndex;
} else {
/*if (binaryHeap[bubbleIndex].f <= binaryHeap[parentIndex].f) { /* \todo Wouldn't it be more efficient with '<' instead of '<=' ? *
Node tmpValue = binaryHeap[parentIndex];
//tmpValue.heapIndex = bubbleIndex;//HeapIndex
binaryHeap[parentIndex] = binaryHeap[bubbleIndex];
binaryHeap[bubbleIndex] = tmpValue;
bubbleIndex = parentIndex;
} else {*/
break;
}
}
numberOfItems++;
}