Finally its decent

1 files changed, 64 insertions, 14 deletions

diff --git a/src/world.c b/src/world.c
index 37aa818..defbec7 100644
--- a/src/world.c
+++ b/src/world.c
@@ -153,8 +153,9 @@ size_t buildWorldBVHLeafs(BVHNode leafs[WORLD_ENTITY_MAX], const World* world)
 }
 
 size_t buildWorldBVHSubtree(BVHNode* subtree,
-                         const BVHNode* nodes, size_t nodesSize,
-                         const World* world)
+                            const BVHNode* nodes, size_t nodesSize,
+                            int sizeMax,
+                            const World* world)
 {
   size_t subtreeSize = 0;
   bool grouped[nodesSize];
@@ -192,7 +193,7 @@ size_t buildWorldBVHSubtree(BVHNode* subtree,
         overlapBox.min = nodes[index].position;
         overlapBox.max = overlapBox.min;
 
-        for (int innerIndex = 0; innerIndex < branchIndex; ++innerIndex)
+        for (int innerIndex = 0; innerIndex < node.branchCount; ++innerIndex)
         {
           distance += Vector3Distance(node.branches[innerIndex]->position,
                                       nodes[index].position);
@@ -204,23 +205,43 @@ size_t buildWorldBVHSubtree(BVHNode* subtree,
           overlapBox.max = Vector3Max(overlapBox.max, nodes[index].box.max);
         }
 
-        distance /= (float)branchIndex;
+        distance /= (float)node.branchCount;
 
         bool overlaps = false;
 
         // Too big (will count it as a overlap).
-        /* if (Vector3Distance(overlapBox.min, overlapBox.max) >= BVH_BOX_MAX) */
-        /* { */
-        /*   overlaps = true; */
-        /* } */
+        if (Vector3Distance(overlapBox.min, overlapBox.max) >= sizeMax)
+        {
+          overlaps = true;
+        }
 
         // Check for overlap.
-        /* for (int nodeIndex = 0; nodeIndex < nodesSize; ++nodeIndex) */
+        for (int subtreeIndex = 0; subtreeIndex < subtreeSize && !overlaps;
+             ++subtreeIndex)
+        {
+          if (CheckCollisionBoxes(overlapBox, subtree[subtreeIndex].box))
+          {
+            overlaps = true;
+            break;
+          }
+        }
+
+        // Will cause overlap
+        /* if (branchIndex == BVH_MAX_BRANCH_COUNT - 1) */
         /* { */
-        /*   if (CheckCollisionBoxes(overlapBox, nodes[nodeIndex].box)) */
+        /*   for (int innerIndex = 0; innerIndex < nodesSize && !overlaps; */
+        /*        ++innerIndex) */
         /*   { */
-        /*     overlaps = true; */
-        /*     break; */
+        /*     if (innerIndex == index || grouped[innerIndex]) */
+        /*     { */
+        /*       continue; */
+        /*     } */
+
+        /*     if (CheckCollisionBoxes(overlapBox, nodes[innerIndex].box)) */
+        /*     { */
+        /*       overlaps = true; */
+        /*       break; */
+        /*     } */
         /*   } */
         /* } */
 
@@ -234,7 +255,7 @@ size_t buildWorldBVHSubtree(BVHNode* subtree,
 
       if (closest == -1)
       {
-        break;
+        continue;
       }
 
       // Add closest as a branch.
@@ -269,6 +290,33 @@ size_t buildWorldBVHSubtree(BVHNode* subtree,
     ++subtreeSize;
   }
 
+  #ifdef FT_DEBUG_MODE
+  // Test if everything is grouped.
+  for (int index = 0; index < nodesSize; ++index)
+  {
+    if (!grouped[index])
+    {
+      printf("Ungrouped: %d\n", index);
+    }
+  }
+
+  // Check for collisions
+  int overlapCount = 0;
+  
+  for (int outer = 0; outer < subtreeSize - 1; ++outer)
+  {
+    for (int inner = outer + 1; inner < subtreeSize; ++inner)
+    {
+      if (CheckCollisionBoxes(subtree[outer].box, subtree[inner].box))
+      {
+        ++overlapCount;
+      }
+    }
+  }
+
+  printf("subtree size: %ld, overlap count: %d\n", subtreeSize, overlapCount);
+#endif
+
   return subtreeSize;
 }
 
@@ -279,11 +327,13 @@ void buildWorldBVH(World* world)
   // Get leafs.
   BVHNode tree[WORLD_ENTITY_MAX];
   size_t treeSize = buildWorldBVHLeafs(tree, world);
+  int sizeMax = BVH_BOX_MAX;
 
   while (treeSize > 1)
   {
     BVHNode subtree[treeSize];
-    treeSize = buildWorldBVHSubtree(subtree, tree, treeSize, world);
+    sizeMax += BVH_BOX_MAX;
+    treeSize = buildWorldBVHSubtree(subtree, tree, treeSize, sizeMax, world);
     memcpy(tree, subtree, treeSize * sizeof(BVHNode));
   }