public PathNode Remove() {
numberOfItems--;
PathNode returnItem = binaryHeap[1];
//returnItem.heapIndex = 0;//Heap index
binaryHeap[1] = binaryHeap[numberOfItems];
//binaryHeap[1].heapIndex = 1;//Heap index
int swapItem = 1, parent = 1;
do {
parent = swapItem;
int p2 = parent * 2;
if (p2 + 1 <= numberOfItems) {
// Both children exist
if (binaryHeap[parent].F >= binaryHeap[p2].F) {
swapItem = p2;//2 * parent;
}
if (binaryHeap[swapItem].F >= binaryHeap[p2 + 1].F) {
swapItem = p2 + 1;
}
} else if ((p2) <= numberOfItems) {
// Only one child exists
if (binaryHeap[parent].F >= binaryHeap[p2].F) {
swapItem = p2;
}
}
// One if the parent's children are smaller or equal, swap them
if (parent != swapItem) {
PathNode tmpIndex = binaryHeap[parent];
//tmpIndex.heapIndex = swapItem;//Heap index
binaryHeap[parent] = binaryHeap[swapItem];
binaryHeap[swapItem] = tmpIndex;
//binaryHeap[parent].heapIndex = parent;//Heap index
}
} while (parent != swapItem);
return returnItem;
}