Add heap data structure

engine/breeze/utils/heap.c

@@ -0,0 +1,121 @@
+#include "heap.h"
+
+#include "../core/memory.h"
+
+typedef struct BzHeap {
+    i32 capacity;
+    i32 size;
+    i32 stride;
+    i32 weightOffset;
+} BzHeap;
+
+#define HEAP_HEAD(heap) (((BzHeap *)heap) - 1)
+
+#define HEAP_LEFT(i) ((i32)(i * 2 + 1))
+#define HEAP_RIGHT(i) ((i32)(i * 2 + 2))
+#define HEAP_PARENT(i) ((i32) ((i - 1) / 2))
+
+static void heapSiftUp(BzHeap *head, void *heap);
+static void heapSiftDown(BzHeap *head, void *heap);
+
+
+
+void *_bzHeapNew(i32 startCapacity, i32 stride, i32 weightOffset) {
+    i32 numBytes = sizeof(BzHeap) + (startCapacity + 1) * stride;
+    BzHeap *heap = bzAlloc(numBytes);
+    heap[0] = (BzHeap) {
+        .capacity=startCapacity,
+        .size=0,
+        .stride=stride,
+        .weightOffset=weightOffset
+    };
+
+    return &heap[1];
+}
+
+void _bzHeapFree(void *heap) {
+    bzFree(HEAP_HEAD(heap));
+}
+
+i32 _bzHeapSize(void *heap) {
+    return HEAP_HEAD(heap)->size;
+}
+bool _bzHeapIsEmpty(void *heap) {
+    return HEAP_HEAD(heap)->size == 0;
+
+}
+i32 _bzHeapPop(void *heap) {
+    BzHeap *head = HEAP_HEAD(heap);
+    BZ_ASSERT(head->size > 0);
+
+    // Move first to index capacity (for output)
+    bzMemMove(((u8 *)heap) + head->capacity * head->stride, heap, head->stride);
+    head->size--;
+    if (head->size == 0) return 0;
+
+    // Move last to index 0
+    bzMemMove(heap, ((u8 *) heap) + head->size * head->stride, head->stride);
+
+    heapSiftDown(head, heap);
+
+    return head->capacity;
+}
+void _bzHeapPush(void *heap) {
+    BzHeap *head = HEAP_HEAD(heap);
+    BZ_ASSERT(head->size + 1 < head->capacity);
+
+    void *item = ((u8 *)heap) + head->capacity * head->stride;
+    bzMemMove(((u8 *)heap) + head->size * head->stride, item, head->stride);
+
+    head->size++;
+    heapSiftUp(head, heap);
+}
+i32 _bzHeapPushTmpIdx(void *heap) {
+    return HEAP_HEAD(heap)->capacity;
+}
+
+static void heapSwap(BzHeap *head, void *heap, i32 aIdx, i32 bIdx) {
+    u8 *aItem = ((u8 *)heap) + aIdx * head->stride;
+    u8 *bItem = ((u8 *)heap) + bIdx * head->stride;
+
+    u8 tmp[head->stride];
+    bzMemMove(tmp, aItem, head->stride);
+
+    bzMemMove(aItem, bItem, head->stride);
+    bzMemMove(bItem, tmp, head->stride);
+}
+static int heapCmp(BzHeap *head, void *heap, i32 lhs, i32 rhs) {
+    int *aWeight = (i32 *) (((u8 *)heap) + lhs * head->stride + head->weightOffset);
+    int *bWeight = (i32 *) (((u8 *)heap) + rhs * head->stride + head->weightOffset);
+    return (*aWeight) - (*bWeight);
+}
+static void heapSiftUp(BzHeap *head, void *heap) {
+    i32 idx = head->size - 1;
+
+    while (idx >= 0) {
+        i32 parent = HEAP_PARENT(idx);
+        if (heapCmp(head, heap, idx, parent) >= 0)
+            break;
+        heapSwap(head, heap, idx, parent);
+        idx = parent;
+    }
+
+}
+static void heapSiftDown(BzHeap *head, void *heap) {
+    i32 idx = 0;
+
+    while (idx < head->size) {
+        i32 l = HEAP_LEFT(idx);
+        i32 r = HEAP_RIGHT(idx);
+        i32 smallest = idx;
+        if (l < head->size && heapCmp(head, heap, l, idx) < 0)
+            smallest = l;
+        if (r < head->size && heapCmp(head, heap, r, smallest) < 0)
+            smallest = r;
+
+        if (smallest == idx) break;
+
+        heapSwap(head, heap, idx, smallest);
+        idx = smallest;
+    }
+}

engine/breeze/utils/heap.h

@@ -0,0 +1,28 @@
+#ifndef BREEZE_HEAP_H
+#define BREEZE_HEAP_H
+
+#include "../defines.h"
+
+void *_bzHeapNew(i32 startCapacity, i32 stride, i32 weightOffset);
+void _bzHeapFree(void *heap);
+
+i32 _bzHeapSize(void *heap);
+bool _bzHeapIsEmpty(void *heap);
+i32 _bzHeapPop(void *heap);
+void _bzHeapPush(void *heap);
+i32 _bzHeapPushTmpIdx(void *heap);
+
+#define bzHeapNew(T, n) (T *) ((T *)_bzHeapNew((n), sizeof(T), offsetof(T, weight)))
+#define bzHeapFree(heap) _bzHeapFree((void *) (heap))
+
+#define bzHeapSize(heap) _bzHeapSize((void *) (heap))
+#define bzHeapIsEmpty(heap) _bzHeapIsEmpty((void *) (heap))
+#define bzHeapPop(heap) ((heap)[_bzHeapPop((void *) (heap))])
+#define bzHeapPush(heap, item) do {             \
+    void *h = (void *) (heap);                  \
+    (heap)[_bzHeapPushTmpIdx(h)] = (item);      \
+    _bzHeapPush(h);                             \
+} while(0)
+
+
+#endif //BREEZE_HEAP_H