Fixed height thing for real this time

1 files changed, 64 insertions, 64 deletions

diff --git a/src/world.c b/src/world.c
index 26ee3b9..5719c3c 100644
--- a/src/world.c
+++ b/src/world.c
@@ -7,9 +7,13 @@ void buildWorldBVH(World* world)
   Entity* entities = world->entities;
   bool grouped[WORLD_ENTITY_MAX];
   WorldUID groupedList[WORLD_ENTITY_MAX];
-  int groupedListSize = 0;
   int ungroupedCount = WORLD_ENTITY_MAX;
-  memset(grouped, 0, sizeof(grouped));
+  
+  for (int index = 0; index < WORLD_ENTITY_MAX; ++index)
+  {
+    grouped[index] = false;
+    groupedList[index] = index;
+  }
 
   world->bvhTestSize = 0;
 
@@ -21,15 +25,14 @@ void buildWorldBVH(World* world)
     for (int leafIndex = 0; leafIndex < BVH_MAX; ++leafIndex)
     {
       int closest = -1;
+      int closestGroupedIndex = 0;
       float closestDistance = world->size.x;
 
       // Find closest.
-      for (int index = 0; index < WORLD_ENTITY_MAX; ++index)
+      for (int groupedIndex = WORLD_ENTITY_MAX - ungroupedCount;
+           groupedIndex < WORLD_ENTITY_MAX; ++groupedIndex)
       {
-        if (grouped[index])
-        {
-          continue;
-        }
+        int index = groupedList[groupedIndex];
         
         // First entity.
         if (leafIndex == 0)
@@ -59,10 +62,11 @@ void buildWorldBVH(World* world)
         bool overlaps = false;
 
         // Check if overlap will be caused.
-        for (int groupedIndex = 0; groupedIndex < groupedListSize;
-             ++groupedIndex)
+        for (int innerGroupedIndex = 0;
+             innerGroupedIndex < WORLD_ENTITY_MAX - ungroupedCount;
+             ++innerGroupedIndex)
         {
-          int overlapIndex = groupedList[groupedIndex];
+          int overlapIndex = groupedList[innerGroupedIndex];
           bool isPartOf = false;
 
           for (int partOfIndex = 0; partOfIndex < leafIndex + 1; ++partOfIndex)
@@ -93,6 +97,7 @@ void buildWorldBVH(World* world)
         {
           closestDistance = distance;
           closest = index;
+          closestGroupedIndex = groupedIndex;
         }
       }
 
@@ -104,8 +109,12 @@ void buildWorldBVH(World* world)
       {
         leaf.entities[leafIndex] = closest;
         grouped[closest] = true;
-        groupedList[groupedListSize] = closest;
-        ++groupedListSize;
+        
+        // Bring grouped entities toward the front.
+        WorldUID temp = groupedList[closestGroupedIndex];
+        groupedList[closestGroupedIndex] =
+          groupedList[WORLD_ENTITY_MAX - ungroupedCount];
+        groupedList[WORLD_ENTITY_MAX - ungroupedCount] = temp;
         --ungroupedCount;
       }
     }
@@ -210,65 +219,56 @@ void freeWorld(World world)
 
 float getWorldHeightAtLocation(const World* world, float x, float y)
 {
-  float toMapX = (float)world->texture.width / world->size.x;
-  float toMapY = (float)world->texture.height / world->size.z;
-  int pixelX = x * toMapX;
-  int pixelY = y * toMapY;
+  float mapX = (float)world->texture.width / world->size.x * x;
+  float mapY = (float)world->texture.height / world->size.z * y;
+  RayCollision result;
 
-  int verticeStart = (pixelY * (world->texture.width - 1) + pixelX) * 18;
-  float* vertices = &world->heightmap.meshes[0].vertices[verticeStart];
+  for (int yOffset = -1; yOffset < 2; ++yOffset)
+  {
+    for (int xOffset = -1; xOffset < 2; ++xOffset)
+    {
+      int pixelX = mapX + xOffset;
+      int pixelY = mapY + yOffset;
 
-  // Clamp x and y to prevent ray being out of bounds.
-  Vector2 min = (Vector2){vertices[0], vertices[2]};
-  Vector2 max = (Vector2){vertices[0], vertices[2]};
+      if (pixelX < 0 || pixelX >= world->texture.width ||
+          pixelY < 0 || pixelY >= world->texture.height)
+      {
+        continue;
+      }
 
-  for (int index = 0; index < 18; index += 3)
-  {
-    Vector2 vertex = (Vector2){vertices[index], vertices[index + 2]};
-    min = Vector2Min(min, vertex);
-    max = Vector2Max(max, vertex);
-  }
+      int verticeStart = (pixelY * (world->texture.width - 1) + pixelX) * 18;
+      float* vertices = &world->heightmap.meshes[0].vertices[verticeStart];
 
-  // Cast to triangles at pixel. Really hacky indeed.
-  Ray ray = (Ray){
-    .position = (Vector3){
-      Clamp(x, min.x, max.x),
-      world->size.y + 10.0,
-      Clamp(y, min.y, max.y)},
-    .direction = (Vector3){0.0, -1.0, 0.0}
-  };
-
-  RayCollision result = GetRayCollisionTriangle(
-    ray,
-    (Vector3){vertices[0], vertices[1], vertices[2]},
-    (Vector3){vertices[3], vertices[4], vertices[5]},
-    (Vector3){vertices[6], vertices[7], vertices[8]});
-
-  // Test other triangle.
-  if (!result.hit)
-  {
-    result = GetRayCollisionTriangle(
-      ray,
-      (Vector3){vertices[9], vertices[10], vertices[11]},
-      (Vector3){vertices[12], vertices[13], vertices[14]},
-      (Vector3){vertices[15], vertices[16], vertices[17]});
-  }
+      // Cast to triangles at pixel. Really hacky indeed.
+      Ray ray = (Ray){
+        .position = (Vector3){x, world->size.y * 2.0, y},
+        .direction = (Vector3){0.0, -1.0, 0.0}
+      };
 
-  if (result.hit)
-  {
-    return result.point.y;
-  }
-  else // Fall back.
-  {
-    float height = 0.0;
-    
-    for (int index = 1; index < 18; index += 3)
-    {
-      height += vertices[index];
+      result = GetRayCollisionTriangle(
+        ray,
+        (Vector3){vertices[0], vertices[1], vertices[2]},
+        (Vector3){vertices[3], vertices[4], vertices[5]},
+        (Vector3){vertices[6], vertices[7], vertices[8]});
+
+      // Test other triangle.
+      if (!result.hit)
+      {
+        result = GetRayCollisionTriangle(
+          ray,
+          (Vector3){vertices[9], vertices[10], vertices[11]},
+          (Vector3){vertices[12], vertices[13], vertices[14]},
+          (Vector3){vertices[15], vertices[16], vertices[17]});
+      }
+
+      if (result.hit)
+      {
+        return result.point.y;
+      }
     }
-    
-    return height / 6.0;
   }
+
+  return 0.0;
 }
 
 // Abortions are good. Get more abortions.