1 files changed, 188 insertions, 0 deletions
diff --git a/Minecraft.World/BinaryHeap.cpp b/Minecraft.World/BinaryHeap.cpp
new file mode 100644
index 00000000..3a3d1c1e
--- /dev/null
+++ b/Minecraft.World/BinaryHeap.cpp
@@ -0,0 +1,188 @@
+#include "stdafx.h"
+#include "Node.h"
+#include "System.h"
+#include "BasicTypeContainers.h"
+#include "BinaryHeap.h"
+
+// 4J Jev, add common ctor code.
+void BinaryHeap::_init()
+{
+	heap = NodeArray(1024);
+	sizeVar = 0;
+}
+
+BinaryHeap::BinaryHeap()
+{
+	_init();
+}
+
+BinaryHeap::~BinaryHeap()
+{
+	delete[] heap.data;
+}
+
+Node *BinaryHeap::insert(Node *node)
+{
+    /* if (node->heapIdx >=0) throw new IllegalStateException("OW KNOWS!"); 4J Jev, removed try/catch */
+
+    // Expand if necessary.
+	if (sizeVar == heap.length)
+    {
+        NodeArray newHeap = NodeArray(sizeVar << 1);
+
+        System::arraycopy(heap, 0, &newHeap, 0, sizeVar);
+
+		delete[] heap.data;
+        heap = newHeap;
+    }
+
+    // Insert at end and bubble up.
+    heap[sizeVar] = node;
+    node->heapIdx = sizeVar;
+    upHeap(sizeVar++);
+
+    return node;
+}
+
+void BinaryHeap::clear()
+{
+    sizeVar = 0;
+}
+
+Node *BinaryHeap::peek()
+{
+    return heap[0];
+}
+
+Node *BinaryHeap::pop()
+{
+    Node *popped = heap[0];
+    heap[0] = heap[--sizeVar];
+    heap[sizeVar] = NULL;
+    if (sizeVar > 0) downHeap(0);
+    popped->heapIdx=-1;
+    return popped;
+}
+
+void BinaryHeap::remove(Node *node)
+{
+    // This is what node.heapIdx is for.
+    heap[node->heapIdx] = heap[--sizeVar];
+    heap[sizeVar] = NULL;
+    if (sizeVar > node->heapIdx)
+    {
+        if (heap[node->heapIdx]->f < node->f)
+        {
+            upHeap(node->heapIdx);
+        }
+        else
+        {
+            downHeap(node->heapIdx);
+        }
+    }
+    // Just as a precaution: should make stuff blow up if the node is abused.
+    node->heapIdx = -1;
+}
+
+void BinaryHeap::changeCost(Node *node, float newCost)
+{
+    float oldCost = node->f;
+    node->f = newCost;
+    if (newCost < oldCost)
+    {
+        upHeap(node->heapIdx);
+    }
+    else
+    {
+        downHeap(node->heapIdx);
+    }
+}
+
+int BinaryHeap::size()
+{
+    return sizeVar;
+}
+
+void BinaryHeap::upHeap(int idx)
+{
+    Node *node = heap[idx];
+    float cost = node->f;
+    while (idx > 0)
+    {
+        int parentIdx = (idx - 1) >> 1;
+        Node *parent = heap[parentIdx];
+        if (cost < parent->f)
+        {
+            heap[idx] = parent;
+            parent->heapIdx = idx;
+            idx = parentIdx;
+        }
+        else break;
+    }
+    heap[idx] = node;
+    node->heapIdx = idx;
+}
+
+void BinaryHeap::downHeap(int idx)
+{
+    Node *node = heap[idx];
+    float cost = node->f;
+
+    while (true)
+    {
+        int leftIdx = 1 + (idx << 1);
+        int rightIdx = leftIdx + 1;
+
+        if (leftIdx >= sizeVar) break;
+
+        // We definitely have a left child.
+        Node *leftNode = heap[leftIdx];
+        float leftCost = leftNode->f;
+        // We may have a right child.
+        Node *rightNode;
+        float rightCost;
+
+        if (rightIdx >= sizeVar)
+        {
+            // Only need to compare with left.
+            rightNode = NULL;
+            rightCost = Float::POSITIVE_INFINITY;
+        }
+        else
+        {
+            rightNode = heap[rightIdx];
+            rightCost = rightNode->f;
+        }
+
+        // Find the smallest of the three costs: the corresponding node
+        // should be the parent.
+        if (leftCost < rightCost)
+        {
+            if (leftCost < cost)
+            {
+                heap[idx] = leftNode;
+                leftNode->heapIdx = idx;
+                idx = leftIdx;
+            }
+            else break;
+        }
+        else
+        {
+            if (rightCost < cost)
+            {
+                heap[idx] = rightNode;
+                rightNode->heapIdx = idx;
+                idx = rightIdx;
+            }
+            else break;
+        }
+    }
+
+    heap[idx] = node;
+    node->heapIdx = idx;
+}
+
+bool BinaryHeap::isEmpty()
+{
+    return sizeVar==0;
+}
+\ No newline at end of file