1 files changed, 132 insertions, 4 deletions

diff --git a/src/world.c b/src/world.c
index cd34c55..8d69970 100644
--- a/src/world.c
+++ b/src/world.c
@@ -158,29 +158,156 @@ BVHNode buildWorldBVHTree(BVHNode* leafs, size_t leafsSize,
 {
   BVHNode* nodes = leafs;
   size_t nodesSize = leafsSize;
+  int startNode = 0;
 
   // Yet another copy and paste half fix.
   while (nodesSize > 1)
   {
     BVHNode node;
+    memset(&node, 0, sizeof(BVHNode));
     int branches[BVH_MAX_BRANCH_COUNT];
     
     for (int branchIndex = 0; branchIndex < BVH_MAX_BRANCH_COUNT;
          ++branchIndex)
     {
       int closest = -1;
-      float closestDistance = Vector3Length(world->size);
+      float closestDistance = Vector3LengthSqr(world->size);
       
       for (int nodeIndex = 0; nodeIndex < nodesSize; ++nodeIndex)
       {
         // First branch.
         if (branchIndex == 0)
         {
-          closest = nodeIndex;
+          closest = startNode;
+          break;
+        }
+
+        float distance = 0.0;
+        BoundingBox box = node.branches[0]->box;
+        bool alreadyUsed = false;
+
+        // Get distance and box with node at index.
+        for (int index = 0; index < branchIndex; ++index)
+        {
+          // Already a branch.
+          if (branches[index] == nodeIndex)
+          {
+            alreadyUsed = true;
+            break;
+          }
+          
+          distance += Vector3Distance(node.branches[index]->position,
+                                      nodes[nodeIndex].position);
+          box.min = Vector3Min(box.min, node.branches[index]->box.min);
+          box.min = Vector3Min(box.min, nodes[nodeIndex].box.min);
+          box.max = Vector3Max(box.max, node.branches[index]->box.max);
+          box.max = Vector3Max(box.max, nodes[nodeIndex].box.max);
+        }
+
+        if (alreadyUsed)
+        {
           continue;
         }
+
+        distance /= (float)branchIndex;
+
+        // Check for overlap.
+        bool overlaps = false;
+
+        for (int index = 0; index < nodesSize; ++index)
+        {
+          alreadyUsed = false;
+          
+          for (int innerBranch = 0; innerBranch < node.branchCount;
+               ++innerBranch)
+          {
+            if (branches[innerBranch] == index)
+            {
+              alreadyUsed = true;
+              break;
+            }
+          }
+          
+          if (!alreadyUsed && index != nodeIndex &&
+              CheckCollisionBoxes(nodes[index].box, box))
+          {
+            overlaps = true;
+            break;
+          }
+        }
+
+        if (overlaps)
+        {
+          distance *= 2.0;
+        }
+
+        if (distance < closestDistance)
+        {
+          closest = nodeIndex;
+          closestDistance = distance;
+        }
+      }
+
+      if (closest == -1)
+      {
+        break;
+      }
+
+      // Add closest node to branches.
+      node.branches[node.branchCount] = (BVHNode*)FT_MALLOC(sizeof(BVHNode));
+
+      if (node.branches[node.branchCount] == NULL)
+      {
+        ALLOCATION_ERROR;
+      }
+
+      memcpy(node.branches[node.branchCount], &nodes[closest], sizeof(BVHNode));
+      branches[node.branchCount] = closest;
+      ++node.branchCount;
+    }
+
+    // Create node bounding box and get position.
+    node.box = node.branches[0]->box;
+    
+    for (int index = 1; index < node.branchCount; ++index)
+    {
+      node.box.min = Vector3Min(node.box.min, node.branches[index]->box.min);
+      node.box.max = Vector3Max(node.box.max, node.branches[index]->box.max);
+    }
+
+    node.position = Vector3Scale(Vector3Add(node.box.min, node.box.max), 0.5);
+
+    // Remove taken nodes.
+    nodes[branches[0]] = node;
+
+    int nodeIndex = 0;
+    int removedCount = 0;
+
+    for (int index = 0; index < nodesSize; ++index)
+    {
+      bool notUsed = true;
+
+      for (int branchIndex = 1; branchIndex < node.branchCount; ++branchIndex)
+      {
+        if (branches[branchIndex] == index)
+        {
+          notUsed = false;
+          ++removedCount;
+          break;
+        }
+      }
+
+      if (notUsed)
+      {
+        nodes[nodeIndex] = nodes[index];
+        ++nodeIndex;
       }
     }
+
+    nodesSize -= removedCount;
+    ++startNode;
+    startNode %= nodesSize;
+    printf("%ld\n", nodesSize);
   }
 
   return nodes[0];
@@ -193,6 +320,9 @@ void buildWorldBVH(World* world)
   // Get leafs.
   BVHNode leafs[WORLD_ENTITY_MAX];
   size_t leafsSize = buildWorldBVHLeafs(leafs, world);
+  world->bvh = buildWorldBVHTree(leafs, leafsSize, world);
+
+  world->bvhDebugSelect = 0;
 }
 
 World createWorld(int seed)
@@ -238,8 +368,6 @@ World createWorld(int seed)
   printf("BVH build time: %lf\n", GetTime() - currentTime);
 #endif
 
-  world.bvhDebugSelect = 0;
-
   return world;
 }