1 files changed, 33 insertions, 7 deletions

diff --git a/src/world.c b/src/world.c
index 776c6b6..2b677f9 100644
--- a/src/world.c
+++ b/src/world.c
@@ -118,6 +118,9 @@ 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;
@@ -152,21 +155,44 @@ size_t buildWorldBVHLeafs(BVHNode leafs[WORLD_ENTITY_MAX], const World* world)
   return leafsSize;
 }
 
-BVHNode buildWorldBVHTree(BVHNode* nodes, size_t nodesSize)
+BVHNode buildWorldBVHTree(BVHNode* nodes, size_t nodesSize, const World* world)
 {
   BVHNode node;
-  BVHNode usedNodes[nodesSize];
-  size_t usedNodesSize = 0;
+  /* BVHNode usedNodes[nodesSize]; */
+  /* size_t usedNodesSize = 0; */
   memset(&node, 0, sizeof(BVHNode));
 
   // Add first node to branch.
   node.branches[0] = (BVHNode*)FT_MALLOC(sizeof(BVHNode));
   memcpy(node.branches[0], &nodes[0], sizeof(BVHNode));
-  ++usedNodesSize;
-  
-  for (int index = 0; index < nodesSize; ++index)
+  node.branchCount = 1;
+
+  for (int branchIndex = 1; branchIndex < BVH_MAX_BRANCH_COUNT; ++branchIndex)
   {
-    
+    int closest = -1;
+    float closestDistance = world->size.x;
+
+    // Find closest node.
+    for (int nodeIndex = 0; nodeIndex < nodesSize; ++nodeIndex)
+    {
+      float distance = 0.0;
+      BoundingBox box;
+      box.min = nodes[nodeIndex].box.min;
+      box.max = nodes[nodeIndex].box.max;
+
+      // Get distance
+      for (int index = 0; index < branchIndex; ++index)
+      {
+        distance += Vector3Distance(nodes[nodeIndex].position,
+                                    node.branches[index]->position);
+        box.min = Vector3Min(box.min, nodes[nodeIndex].box.min);
+        box.min = Vector3Min(box.min, node.branches[index]->box.min);
+        box.max = Vector3Max(box.max, nodes[nodeIndex].box.max);
+        box.max = Vector3Max(box.max, node.branches[index]->box.max);
+      }
+
+      distance /= (float)branchIndex;
+    }
   }
 
   return node;