Finally some speed and no overlap!

3 files changed, 74 insertions, 60 deletions

diff --git a/src/utils.h b/src/utils.h
index 08a7420..f660b66 100644
--- a/src/utils.h
+++ b/src/utils.h
@@ -13,7 +13,7 @@
 #ifndef UTIL_H
 #define UTIL_H
 
-typedef struct Game Game;
+#define FT_DEBUG_MODE
 
 #define FT_NAMEMAX 256
 
@@ -29,6 +29,8 @@ typedef struct Game Game;
 #define ALLOCATION_ERROR TraceLog(LOG_ERROR, "Allocation error in %s:%d", \
                                   __FILE__, __LINE__);
 
+typedef struct Game Game;
+
 typedef enum FTError {
   FTERROR = -1,
   FTSUCCESS = 0
diff --git a/src/world.c b/src/world.c
index 14f7ed7..94b6400 100644
--- a/src/world.c
+++ b/src/world.c
@@ -6,14 +6,8 @@ size_t buildWorldBVHLeafs(BVHNode leafs[WORLD_ENTITY_MAX], const World* world)
   size_t leafsSize = 0;
   const Entity* entities = world->entities;
   bool grouped[WORLD_ENTITY_MAX];
-  WorldUID groupedList[WORLD_ENTITY_MAX];
   int ungroupedCount = WORLD_ENTITY_MAX;
-
-  for (int index = 0; index < WORLD_ENTITY_MAX; ++index)
-  {
-    grouped[index] = false;
-    groupedList[index] = index;
-  }
+  memset(grouped, 0, sizeof(grouped));
 
   while (ungroupedCount > 0)
   {
@@ -26,10 +20,12 @@ size_t buildWorldBVHLeafs(BVHNode leafs[WORLD_ENTITY_MAX], const World* world)
       float closestDistance = world->size.x;
 
       // Find closest.
-      for (int groupedIndex = WORLD_ENTITY_MAX - ungroupedCount;
-           groupedIndex < WORLD_ENTITY_MAX; ++groupedIndex)
+      for (int index = 0; index < WORLD_ENTITY_MAX; ++index)
       {
-        int index = groupedList[groupedIndex];
+        if (grouped[index])
+        {
+          continue;
+        }
         
         // First entity.
         if (leafIndex == 0)
@@ -39,50 +35,44 @@ size_t buildWorldBVHLeafs(BVHNode leafs[WORLD_ENTITY_MAX], const World* world)
         }
 
         float distance = 0.0;
-        Vector3 min = entities[index].position;
-        Vector3 max = min;
+        BoundingBox overlapBox;
+        overlapBox.min = entities[index].position;
+        overlapBox.max = overlapBox.min;
 
         for (int innerIndex = 0; innerIndex < leafIndex; ++innerIndex)
         {
           distance += Vector3Distance(
             entities[leaf.entities[innerIndex]].position,
             entities[index].position);
-          min = Vector3Min(min, entities[leaf.entities[innerIndex]].position);
-          min = Vector3Min(min, entities[index].position);
-          max = Vector3Max(max, entities[leaf.entities[innerIndex]].position);
-          max = Vector3Max(max, entities[index].position);
+          overlapBox.min = Vector3Min(
+            overlapBox.min,
+            entities[leaf.entities[innerIndex]].position);
+          overlapBox.min = Vector3Min(
+            overlapBox.min,
+            entities[index].position);
+          overlapBox.max = Vector3Max(
+            overlapBox.max,
+            entities[leaf.entities[innerIndex]].position);
+          overlapBox.max = Vector3Max(
+            overlapBox.max,
+            entities[index].position);
         }
 
-        BoundingBox overlapBox = (BoundingBox){min, max};
         distance /= (float)leafIndex;
 
         bool overlaps = false;
 
-        // Check if overlap will be caused.
-        for (int innerGroupedIndex = 0;
-             innerGroupedIndex < WORLD_ENTITY_MAX - ungroupedCount;
-             ++innerGroupedIndex)
+        // Too big (will count it as a overlap).
+        if (Vector3Distance(overlapBox.min, overlapBox.max) >= BVH_BOX_MAX)
         {
-          int overlapIndex = groupedList[innerGroupedIndex];
-          bool isPartOf = false;
-
-          for (int partOfIndex = 0; partOfIndex < leafIndex + 1; ++partOfIndex)
-          {
-            if (overlapIndex == leaf.entities[partOfIndex])
-            {
-              isPartOf = true;
-              break;
-            }
-          }
-
-          if (isPartOf)
-          {
-            continue;
-          }
+          overlaps = true;
+        }
 
-          // TODO: Make use of entity bounding boxes.
-          if (CheckCollisionBoxSphere(
-                overlapBox, entities[overlapIndex].position, 0.5))
+        // Check if overlap is already happening.
+        for (int nodeIndex = 0; nodeIndex < leafsSize && !overlaps;
+             ++nodeIndex)
+        {
+          if (CheckCollisionBoxes(overlapBox, leafs[nodeIndex].box))
           {
             overlaps = true;
             break;
@@ -94,7 +84,6 @@ size_t buildWorldBVHLeafs(BVHNode leafs[WORLD_ENTITY_MAX], const World* world)
         {
           closestDistance = distance;
           closest = index;
-          closestGroupedIndex = groupedIndex;
         }
       }
 
@@ -106,12 +95,6 @@ size_t buildWorldBVHLeafs(BVHNode leafs[WORLD_ENTITY_MAX], const World* world)
       {
         leaf.entities[leafIndex] = closest;
         grouped[closest] = true;
-        
-        // Bring grouped entities toward the front.
-        WorldUID temp = groupedList[closestGroupedIndex];
-        groupedList[closestGroupedIndex] =
-          groupedList[WORLD_ENTITY_MAX - ungroupedCount];
-        groupedList[WORLD_ENTITY_MAX - ungroupedCount] = temp;
         --ungroupedCount;
       }
     }
@@ -139,16 +122,35 @@ size_t buildWorldBVHLeafs(BVHNode leafs[WORLD_ENTITY_MAX], const World* world)
     ++leafsSize;
   }
 
-    // test
+#ifdef FT_DEBUG_MODE
+  // Test if everything is grouped.
   for (int index = 0; index < WORLD_ENTITY_MAX; ++index)
   {
     if (!grouped[index])
     {
-      printf("%d\n", index);
+      printf("Ungrouped: %d\n", index);
     }
   }
 
-  printf("size: %ld\n", leafsSize);
+  // Test for leaf collision.
+  for (int outer = 0; outer < leafsSize; ++outer)
+  {
+    for (int inner = 0; inner < leafsSize; ++inner)
+    {
+      if (outer == inner)
+      {
+        continue;
+      }
+
+      if (CheckCollisionBoxes(leafs[outer].box, leafs[inner].box))
+      {
+        printf("Leaf collision: %d and %d\n", outer, inner);
+      }
+    }
+  }
+
+  printf("leaf count: %ld\n", leafsSize);
+#endif
 
   return leafsSize;
 }
@@ -161,6 +163,8 @@ void buildWorldBVH(World* world)
   // Get leafs
   BVHNode leafs[WORLD_ENTITY_MAX];
   size_t leafsSize = buildWorldBVHLeafs(leafs, world);
+  memcpy(world->bvhTest, leafs, sizeof(leafs));
+  world->bvhTestSize = leafsSize;
 }
 
 World createWorld(int seed)
@@ -198,7 +202,10 @@ World createWorld(int seed)
 
   double currentTime = GetTime();
   buildWorldBVH(&world);
-  printf("%lf\n", GetTime() - currentTime);
+
+#ifdef FT_DEBUG_MODE
+  printf("BVH build time: %lf\n", GetTime() - currentTime);
+#endif
 
   return world;
 }
@@ -213,11 +220,15 @@ void updateWorld(World* world, Game* game)
   }
 
   // Draw BVH leafs.
-  /* for (int index = 0; index < world->bvhTestSize; ++index) */
-  /* { */
-  /*   Color colors[] = {RED, GREEN, BLUE, ORANGE, YELLOW, PINK}; */
-  /*   DrawBoundingBox(world->bvhTest[index].box, colors[index % 6]); */
-  /* } */
+#ifdef FT_DEBUG_MODE
+  for (int index = 0; index < world->bvhTestSize; ++index)
+  {
+    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);
+  }
+#endif
 }
 
 void freeWorld(World world)
diff --git a/src/world.h b/src/world.h
index 1840af3..60cbe9f 100644
--- a/src/world.h
+++ b/src/world.h
@@ -7,10 +7,9 @@
 #ifndef WORLD_H
 #define WORLD_H
 
-#define BVH_MAX 4 // Max entities per node.
-#define BVH_LEAF_COUNT 250
-#define BVH_OVERLAP_MULTIPLIER 100.0
+#define BVH_MAX 2 // Max entities per node.
 #define BVH_MAX_BRANCH_COUNT 3
+#define BVH_BOX_MAX 100.0
 
 #define WORLD_ENTITY_MAX 1000
 #define WORLD_SIZE (Vector3){1000.0, 100.0, 1000.0}
@@ -33,6 +32,8 @@ typedef struct {
   Model heightmap;
   Entity entities[WORLD_ENTITY_MAX];
   BVHNode bvh;
+  BVHNode bvhTest[WORLD_ENTITY_MAX];
+  size_t bvhTestSize;
 } World;
 
 World createWorld(int seed);