1 files changed, 47 insertions, 6 deletions

diff --git a/src/world.c b/src/world.c
index 2b677f9..098f15a 100644
--- a/src/world.c
+++ b/src/world.c
@@ -100,8 +100,7 @@ size_t buildWorldBVHLeafs(BVHNode leafs[WORLD_ENTITY_MAX], const World* world)
     }
 
     // Get bounding box.
-    leaf.box.min = world->entities[leaf.entities[0]].box.min;
-    leaf.box.max = world->entities[leaf.entities[0]].box.max;
+    leaf.box = world->entities[leaf.entities[0]].box;
 
     for (int index = 1; index < BVH_MAX; ++index)
     {
@@ -164,6 +163,13 @@ BVHNode buildWorldBVHTree(BVHNode* nodes, size_t nodesSize, const World* world)
 
   // Add first node to branch.
   node.branches[0] = (BVHNode*)FT_MALLOC(sizeof(BVHNode));
+
+  if (node.branches[0] == NULL)
+  {
+    ALLOCATION_ERROR;
+    return node;
+  }
+  
   memcpy(node.branches[0], &nodes[0], sizeof(BVHNode));
   node.branchCount = 1;
 
@@ -177,10 +183,9 @@ BVHNode buildWorldBVHTree(BVHNode* nodes, size_t nodesSize, const World* world)
     {
       float distance = 0.0;
       BoundingBox box;
-      box.min = nodes[nodeIndex].box.min;
-      box.max = nodes[nodeIndex].box.max;
+      box = nodes[nodeIndex].box;
 
-      // Get distance
+      // Get distance and bounding box.
       for (int index = 0; index < branchIndex; ++index)
       {
         distance += Vector3Distance(nodes[nodeIndex].position,
@@ -192,6 +197,42 @@ BVHNode buildWorldBVHTree(BVHNode* nodes, size_t nodesSize, const World* world)
       }
 
       distance /= (float)branchIndex;
+      
+      // TODO: Use bounding box for the thing
+
+      if (distance < closestDistance)
+      {
+        closest = nodeIndex;
+        closestDistance = distance;
+      }
+    }
+
+    if (closest == -1)
+    {
+      node.branches[branchIndex] = NULL;
+    }
+    else
+    {
+      node.branches[branchIndex] = (BVHNode*)FT_MALLOC(sizeof(BVHNode));
+
+      if (node.branches[branchIndex] == NULL)
+      {
+        ALLOCATION_ERROR;
+      }
+
+      memcpy(node.branches[branchIndex], &nodes[closest], sizeof(BVHNode));
+    }
+  }
+
+  // Create node bounding box.
+  node.box = node.branches[0]->box;
+  
+  for (int index = 0; index < BVH_MAX_BRANCH_COUNT; ++index)
+  {
+    if (node.branches[index] != NULL)
+    {
+      node.box.min = Vector3Min(node.box.min, node.branches[index]->box.min);
+      node.box.max = Vector3Max(node.box.max, node.branches[index]->box.max);
     }
   }
 
@@ -203,7 +244,7 @@ void buildWorldBVH(World* world)
 {
   Entity* entities = world->entities;
 
-  // Get leafs
+  // Get leafs.
   BVHNode leafs[WORLD_ENTITY_MAX];
   size_t leafsSize = buildWorldBVHLeafs(leafs, world);
   memcpy(world->bvhTest, leafs, sizeof(leafs));