Almost there (:

2 files changed, 124 insertions, 37 deletions

diff --git a/src/world.c b/src/world.c
index d8d473b..40ef21e 100644
--- a/src/world.c
+++ b/src/world.c
@@ -117,9 +117,8 @@ size_t buildWorldBVHLeafs(BVHNode leafs[WORLD_ENTITY_MAX], const World* world)
         world->entities[leaf.entities[index]].box.max);
     }
 
-    // Get position.
     leaf.position = Vector3Scale(Vector3Add(leaf.box.min, leaf.box.max), 0.5);
-    
+
     memset(leaf.branches, 0, BVH_MAX_BRANCH_COUNT * sizeof(BVHNode*));
     leafs[leafsSize] = leaf;
     ++leafsSize;
@@ -155,7 +154,9 @@ size_t buildWorldBVHLeafs(BVHNode leafs[WORLD_ENTITY_MAX], const World* world)
 }
 
 BVHNode buildWorldBVHTree(BVHNode* nodes, size_t nodesSize,
-                          int taken[BVH_MAX_BRANCH_COUNT], const World* world)
+                          int taken[BVH_MAX_BRANCH_COUNT],
+                          BVHNode* level, size_t levelSize,
+                          const World* world)
 {
   BVHNode node;
   memset(&node, 0, sizeof(BVHNode));
@@ -217,10 +218,20 @@ BVHNode buildWorldBVHTree(BVHNode* nodes, size_t nodesSize,
       }
 
       distance /= (float)branchIndex;
-      
-      // TODO: Use bounding box for the thing
 
-      if (distance < closestDistance)
+      // Check for overlap.
+      bool overlaps = false;
+
+      for (int index = 0; index < levelSize; ++index)
+      {
+        if (CheckCollisionBoxes(box, level[index].box))
+        {
+          overlaps = true;
+          break;
+        }
+      }
+
+      if (!overlaps && distance < closestDistance)
       {
         closest = nodeIndex;
         closestDistance = distance;
@@ -257,10 +268,11 @@ BVHNode buildWorldBVHTree(BVHNode* nodes, size_t nodesSize,
     }
   }
 
+  node.position = Vector3Scale(Vector3Add(node.box.min, node.box.max), 0.5);
+
   return node;
 }
 
-// Very messy right now. Mostly been playing around.
 void buildWorldBVH(World* world)
 {
   Entity* entities = world->entities;
@@ -268,40 +280,79 @@ void buildWorldBVH(World* world)
   // Get leafs.
   BVHNode nodes[WORLD_ENTITY_MAX];
   size_t nodesSize = buildWorldBVHLeafs(nodes, world);
-  /* memcpy(world->bvhTest, leafs, sizeof(leafs)); */
-  /* world->bvhTestSize = leafsSize; */
 
-  while (nodesSize > 0)
-  {
-    int taken[BVH_MAX_BRANCH_COUNT];
-    BVHNode node = buildWorldBVHTree(nodes, nodesSize, taken, world);
+  BVHNode level[WORLD_ENTITY_MAX];
+  size_t levelSize = 0;
+  int levelCount = 0;
 
-    // Take out taken nodes.
-    int nodeIndex = 0;
-    int removedCount = 0;
-    
-    for (int nodeCount = 0; nodeCount < nodesSize; ++nodeCount)
+  while (true)
+  {
+    while (nodesSize > 0)
     {
-      bool isTaken = false;
-      
-      for (int takenIndex = 0; takenIndex < node.branchCount; ++takenIndex)
+      int taken[BVH_MAX_BRANCH_COUNT];
+      BVHNode node = buildWorldBVHTree(nodes, nodesSize, taken, level,
+                                       levelSize, world);
+
+      // Take out taken nodes.
+      int nodeIndex = 0;
+      int removedCount = 0;
+
+      for (int nodeCount = 0; nodeCount < nodesSize; ++nodeCount)
       {
-        if (taken[takenIndex] == nodeCount)
+        bool isTaken = false;
+
+        for (int takenIndex = 0; takenIndex < node.branchCount; ++takenIndex)
         {
-          isTaken = true;
-          ++removedCount;
-          break;
+          if (taken[takenIndex] == nodeCount)
+          {
+            isTaken = true;
+            ++removedCount;
+            break;
+          }
+        }
+
+        if (!isTaken)
+        {
+          nodes[nodeIndex] = nodes[nodeCount];
+          ++nodeIndex;
         }
       }
 
-      if (!isTaken)
+      level[levelSize] = node;
+      ++levelSize;
+      nodesSize -= removedCount;
+    }
+
+#ifdef FT_DEBUG_MODE
+    // Check for overlap
+    int overlapCount = 0;
+    
+    for (int inner = 0; inner < levelSize; ++inner)
+    {
+      for (int outer = inner + 1; outer < levelSize; ++outer)
       {
-        nodes[nodeIndex] = nodes[nodeCount];
-        ++nodeIndex;
+        if (CheckCollisionBoxes(level[inner].box, level[outer].box))
+        {
+          ++overlapCount;
+        }
       }
     }
+    
+    printf("BVH level: %d, Size: %ld, Overlap: %d\n", levelCount, levelSize,
+           overlapCount);
+    ++levelCount;
+#endif
 
-    nodesSize -= removedCount;
+    // Reached root node.
+    if (levelSize <= 1)
+    {
+      world->bvh = level[0];
+      break;
+    }
+
+    nodesSize = levelSize;
+    levelSize = 0;
+    memcpy(nodes, level, nodesSize * sizeof(BVHNode));
   }
 }
 
@@ -348,9 +399,28 @@ World createWorld(int seed)
   printf("BVH build time: %lf\n", GetTime() - currentTime);
 #endif
 
+  world.bvhDebugSelect = 0;
+
   return world;
 }
 
+void drawBVHDebug(BVHNode bvh, int level, int selected)
+{
+  Color colors[] = {RED, GREEN, BLUE, ORANGE, YELLOW, PINK};
+  int colorSize = 6;
+
+  if (level == selected)
+  {
+    DrawBoundingBox(bvh.box, colors[level % colorSize]);
+    return;
+  }
+
+  for (int index = 0; index < bvh.branchCount; ++index)
+  {
+    drawBVHDebug(*bvh.branches[index], level + 1, selected);
+  }
+}
+
 void updateWorld(World* world, Game* game)
 {
   DrawModel(world->heightmap, Vector3Zero(), 1.0, WHITE);
@@ -362,20 +432,37 @@ void updateWorld(World* world, Game* game)
 
   // Draw BVH leafs.
 #ifdef FT_DEBUG_MODE
-  for (int index = 0; index < world->bvhTestSize; ++index)
+  if (IsKeyPressed(KEY_RIGHT))
   {
-    Color colors[] = {RED, GREEN, BLUE, ORANGE, YELLOW, PINK};
-    DrawBoundingBox(world->bvhTest[index].box, colors[index % 6]);
-    DrawSphereEx(world->bvhTest[index].box.min, 0.3, 2, 2, BLUE);
-    DrawSphereEx(world->bvhTest[index].box.max, 0.3, 2, 2, BLUE);
+    ++world->bvhDebugSelect;
   }
+  if (IsKeyPressed(KEY_LEFT))
+  {
+    --world->bvhDebugSelect;
+  }
+  
+  drawBVHDebug(world->bvh, 0, world->bvhDebugSelect);
 #endif
 }
 
+void freeWorldBVH(BVHNode bvh)
+{
+  // Play it safe to prevent memory leaks.
+  for (int index = 0; index < BVH_MAX_BRANCH_COUNT; ++index)
+  {
+    if (bvh.branches[index] != NULL)
+    {
+      freeWorldBVH(*bvh.branches[index]);
+      FT_FREE(bvh.branches[index]);
+    }
+  }
+}
+
 void freeWorld(World world)
 {
   UnloadTexture(world.texture);
   UnloadModel(world.heightmap);
+  freeWorldBVH(world.bvh);
 }
 
 float getWorldHeightAtLocation(const World* world, float x, float y)
diff --git a/src/world.h b/src/world.h
index 0ccb5c2..00f23c6 100644
--- a/src/world.h
+++ b/src/world.h
@@ -8,7 +8,7 @@
 #define WORLD_H
 
 #define BVH_MAX 2 // Max entities per node.
-#define BVH_MAX_BRANCH_COUNT 3
+#define BVH_MAX_BRANCH_COUNT 5
 #define BVH_BOX_MAX 100.0
 
 #define WORLD_ENTITY_MAX 1000
@@ -34,8 +34,8 @@ typedef struct {
   Model heightmap;
   Entity entities[WORLD_ENTITY_MAX];
   BVHNode bvh;
-  BVHNode bvhTest[WORLD_ENTITY_MAX];
-  size_t bvhTestSize;
+
+  int bvhDebugSelect;
 } World;
 
 World createWorld(int seed);