UGGGGG

2 files changed, 74 insertions, 5 deletions

diff --git a/src/world.c b/src/world.c
index e280015..559b620 100644
--- a/src/world.c
+++ b/src/world.c
@@ -1,19 +1,76 @@
 #include "world.h"
 #include "game.h"
 
-// Bottom up method because bottom up better. Just if politicians agreed ):
+int32_t getMortonCode(Vector3 position)
+{
+  int32_t mortonCode = 0;
+  int16_t x = roundf(position.x);
+  int16_t z = roundf(position.z);
+  
+  for (int bit = 0; bit < 32; ++bit)
+  {
+    if (bit % 2)
+    {
+      mortonCode |= IS_BIT_SET(x, (bit / 2)) << bit;
+    }
+    else
+    {
+      mortonCode |= IS_BIT_SET(z, (bit / 2)) << bit;
+    }
+  }
+
+  return mortonCode;
+}
+
+void sortEntitiesUID(WorldUID entities[WORLD_ENTITY_MAX], const World* world)
+{
+  // Lazy selection sort.
+  for (int outer = 0; outer < WORLD_ENTITY_MAX - 1; ++outer)
+  {
+    int minIndex = outer;
+
+    for (int inner = outer + 1; inner < WORLD_ENTITY_MAX; ++inner)
+    {
+      int32_t entityCode = getMortonCode(
+        world->entities[entities[inner]].position);
+      int32_t minCode = getMortonCode(
+        world->entities[entities[minIndex]].position);
+
+      if (entityCode < minCode)
+      {
+        minIndex = inner;
+      }
+    }
+
+    WorldUID temp = entities[outer];
+    entities[outer] = entities[minIndex];
+    entities[minIndex] = temp;
+  }
+
+  // Test
+  /* for (int index = 0; index < WORLD_ENTITY_MAX; ++index) */
+  /* { */
+  /*   printf("%d\n", getMortonCode(world->entities[entities[index]].position)); */
+  /*   PRINT_VECTOR3(world->entities[entities[index]].position); */
+  /* } */
+}
+
 // Very messy right now. Mostly been playing around.
 void buildWorldBVH(World* world)
 {
   Entity* entities = world->entities;
   bool grouped[WORLD_ENTITY_MAX];
+  WorldUID sorted[WORLD_ENTITY_MAX];
 
   // This is a mess thats not going to work.
   for (int index = 0; index < WORLD_ENTITY_MAX; ++index)
   {
     grouped[index] = false;
+    sorted[index] = index;
   }
 
+  //sortEntitiesUID(sorted, world);
+
   for (int nodeIndex = 0; nodeIndex < BVH_LEAF_COUNT; ++nodeIndex)
   {
     for (int leafIndex = 0; leafIndex < BVH_MAX; ++leafIndex)
@@ -52,6 +109,8 @@ void buildWorldBVH(World* world)
         Vector3 min = entities[index].position;
         Vector3 max = entities[index].position;
 
+        bool overlaps = false;
+
         for (int innerIndex = 0; innerIndex < leafIndex; ++innerIndex)
         {
           distance += Vector3Distance(
@@ -95,19 +154,23 @@ void buildWorldBVH(World* world)
           if (CheckCollisionBoxSphere(
                 overlapBox, entities[overlapIndex].position, 0.5))
           {
-            distance *= BVH_OVERLAP_MULTIPLIER;
+            overlaps = true;
             break;
           }
         }
 
-        if (distance < closestDistance)
+        if (!overlaps && distance < closestDistance)
         {
           closestDistance = distance;
           closest = index;
         }
       }
 
-      if (closest != -1)
+      if (closest == -1)
+      {
+        leaf->entities[leafIndex] = -1;
+      }
+      else
       {
         leaf->entities[leafIndex] = closest;
         grouped[closest] = true;
@@ -122,6 +185,11 @@ void buildWorldBVH(World* world)
 
         for (int index = 1; index < BVH_MAX; ++index)
         {
+          if (leaf->entities[index] == -1)
+          {
+            continue;
+          }
+          
           leaf->box.min = Vector3Min(
             leaf->box.min,
             world->entities[leaf->entities[index]].position);
diff --git a/src/world.h b/src/world.h
index 46bd248..69f81bd 100644
--- a/src/world.h
+++ b/src/world.h
@@ -22,7 +22,7 @@ typedef int16_t WorldUID;
 
 typedef struct BVHNode {
   BoundingBox box;
-  WorldUID entities[BVH_MAX];
+  WorldUID entities[BVH_MAX]; // Only for leafs.
   struct BVHNode* branch1;
   struct BVHNode* branch2;
 } BVHNode;
@@ -34,6 +34,7 @@ typedef struct {
   Entity entities[WORLD_ENTITY_MAX];
   BVHNode bvh;
   BVHNode bvhTest[BVH_LEAF_COUNT];
+  int bvhTestSize;
 } World;
 
 World createWorld(int seed);