Overhaul pathfinding algorithm

engine/CMakeLists.txt

@@ -96,6 +96,6 @@ file(COPY ${BreezeHeaders} DESTINATION "include")
 
 if (${BUILD_BREEZE_TESTS})
     MESSAGE(STATUS "Building breeze tests is enabled")
-    #add_subdirectory(tests)
+    add_subdirectory(tests)
 endif()
 

engine/breeze/util/heap.c

@@ -15,10 +15,8 @@ typedef struct BzHeapHead {
 #define HEAP_RIGHT(i) ((i32)(i * 2 + 2))
 #define HEAP_PARENT(i) ((i32) ((i - 1) / 2))
 
-static void heapSiftUp(BzHeapHead *head, void *heap);
-static void heapSiftDown(BzHeapHead *head, void *heap);
-
-
+static void heapSiftUp(BzHeapHead *head, void *heap, i32 idx);
+static void heapSiftDown(BzHeapHead *head, void *heap, i32 idx);
 
 void *_bzHeapCreate(i32 startCapacity, i32 stride, i32 weightOffset) {
     i32 numBytes = sizeof(BzHeapHead) + (startCapacity + 1) * stride;
@@ -60,7 +58,7 @@ i32 _bzHeapPop(void *heap) {
     // Move last to index 0
     bzMemMove(heap, ((u8 *) heap) + head->size * head->stride, head->stride);
 
-    heapSiftDown(head, heap);
+    heapSiftDown(head, heap, 0);
 
     return head->capacity;
 }
@@ -72,13 +70,20 @@ void _bzHeapPush(void *heap) {
     bzMemMove(((u8 *)heap) + head->size * head->stride, item, head->stride);
 
     head->size++;
-    heapSiftUp(head, heap);
+    heapSiftUp(head, heap, head->size - 1);
 }
 i32 _bzHeapPushIdx(void *heap) {
     BzHeapHead *head = HEAP_HEAD(heap);
     BZ_ASSERT(head->size < head->capacity);
     return head->size;
 }
+void _bzHeapUpdate(void *heap, i32 idx) {
+    BzHeapHead *head = HEAP_HEAD(heap);
+    BZ_ASSERT(idx >= 0 && idx < head->size);
+
+    heapSiftUp(head, heap, idx);
+    heapSiftDown(head, heap, idx);
+}
 
 static void heapSwap(BzHeapHead *head, void *heap, i32 aIdx, i32 bIdx) {
     u8 *aItem = ((u8 *)heap) + aIdx * head->stride;
@@ -95,9 +100,7 @@ static int heapCmp(BzHeapHead *head, void *heap, i32 lhs, i32 rhs) {
     int *bWeight = (i32 *) (((u8 *)heap) + rhs * head->stride + head->weightOffset);
     return (*aWeight) - (*bWeight);
 }
-static void heapSiftUp(BzHeapHead *head, void *heap) {
-    i32 idx = head->size - 1;
-
+static void heapSiftUp(BzHeapHead *head, void *heap, i32 idx) {
     while (idx >= 0) {
         i32 parent = HEAP_PARENT(idx);
         if (heapCmp(head, heap, idx, parent) >= 0)
@@ -107,9 +110,7 @@ static void heapSiftUp(BzHeapHead *head, void *heap) {
     }
 
 }
-static void heapSiftDown(BzHeapHead *head, void *heap) {
-    i32 idx = 0;
-
+static void heapSiftDown(BzHeapHead *head, void *heap, i32 idx) {
     while (idx < head->size) {
         i32 l = HEAP_LEFT(idx);
         i32 r = HEAP_RIGHT(idx);

engine/breeze/util/heap.h

@@ -12,6 +12,7 @@ bool _bzHeapIsEmpty(void *heap);
 i32 _bzHeapPop(void *heap);
 void _bzHeapPush(void *heap);
 i32 _bzHeapPushIdx(void *heap);
+void _bzHeapUpdate(void *heap, i32 idx);
 
 #define bzHeapCreate(T, n) (T *) ((T *)_bzHeapCreate((n), sizeof(T), offsetof(T, weight)))
 #define bzHeapDestroy(heap) _bzHeapDestroy((void *) (heap))
@@ -25,6 +26,7 @@ i32 _bzHeapPushIdx(void *heap);
     (heap)[_bzHeapPushIdx(h)] = (__VA_ARGS__);   \
     _bzHeapPush(h);                                 \
 } while(0)
+#define bzHeapUpdate(heap, idx) _bzHeapUpdate((void *) heap, idx)
 
 
 #endif //BREEZE_HEAP_H

engine/tests/heap_test.c

@@ -29,6 +29,25 @@ int main() {
         printf("%d\n", node.weight);
     }
 
+    printf("\n\n");
+    bzHeapDestroy(heap);
+
+    heap = bzHeapCreate(Node, 10);
+    bzHeapPush(heap, (Node) {3});
+    bzHeapPush(heap, (Node) {8});
+    bzHeapPush(heap, (Node) {10});
+    bzHeapPush(heap, (Node) {5});
+    bzHeapPush(heap, (Node) {12});
+    bzHeapPush(heap, (Node) {7});
+
+    heap[3].weight = 20;
+    bzHeapUpdate(heap, 3);
+
+    while (!bzHeapIsEmpty(heap)) {
+        Node node = bzHeapPop(heap);
+        printf("%d ", node.weight);
+    }
+
     bzHeapDestroy(heap);