refine physics collision but wall lag too many collisions. raylib.

raylib_collision3d.c

#define WIN32_LEAN_AND_MEAN
#define _WINSOCK_DEPRECATED_NO_WARNINGS
#define NOGDI
#define NOUSER
#define MMNOSOUND

#include "raylib.h"
#include "raymath.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <float.h>

// Terrain generation parameters
int t_width = 8;
int t_height = 8;
float t_scale = 30.0f;
float t_magnitude = 0.5f;
float t_offset = 0.02f;
float gravity = 8.0f; // Feet/s²

// Physics simulation parameters
// float simulationRate = 250.0f;
// float stepSize = 1.0f / 250.0f; // 0.004s

// float simulationRate = 60.0f;
// float stepSize = 1.0f / 60.0f;


// Simple pseudo-noise function
float simple_noise(float x, float z) {
    int xi = (int)(x * 1000.0f);
    int zi = (int)(z * 1000.0f);
    int n = xi * 57 + zi * 131;
    n = (n << 13) ^ n;
    return (1.0f - ((n * (n * n * 15731 + 789221) + 1376312589) & 0x7fffffff) / 1073741824.0f) * 0.5f;
}

typedef struct {
    Vector3 size;
    Vector3 position;
    Color color;
} ObjBlock;

typedef struct {
    Vector3 size;
    float radius;
    float height;
    Vector3 position;
    Color color;
    float maxSpeed;
    Vector3 input;
    Vector3 velocity;
    float jumpGround;
    bool onGround;
} ObjPlayer;

typedef struct {
    ObjBlock* blocks;
    int count;
    int capacity;
} ObjWorld;

typedef struct {
    ObjBlock** blocks;
    int count;
    int capacity;
} Candidates;

typedef struct {
    ObjBlock** blocks;
    int count;
    int capacity;
} DebugCubes;

typedef struct {
    ObjBlock* block;
    Vector3 contactPoint;
    Vector3 normal;
    float overlap;
    bool onGround;
} Collision;

typedef struct {
    Collision* collisions;
    int count;
    int capacity;
} Collisions;

typedef struct {
    Collision* collisions;
    int count;
    int capacity;
} DebugCollisions;

// Initialize dynamic array
void init_dynamic_array(void** array, int* count, int* capacity, int initial_capacity, size_t element_size) {
    *array = malloc(initial_capacity * element_size);
    if (!*array) {
        fprintf(stderr, "Failed to allocate memory\n");
        exit(1);
    }
    *count = 0;
    *capacity = initial_capacity;
}

// Free dynamic array
void free_dynamic_array(void** array, int* count, int* capacity) {
    free(*array);
    *array = NULL;
    *count = 0;
    *capacity = 0;
}

// Append to dynamic array
void append_to_dynamic_array(void** array, int* count, int* capacity, void* element, size_t element_size) {
    if (*count >= *capacity) {
        int new_capacity = *capacity == 0 ? 4 : *capacity * 2;
        void* new_array = realloc(*array, new_capacity * element_size);
        if (!new_array) {
            fprintf(stderr, "Failed to reallocate memory\n");
            exit(1);
        }
        *array = new_array;
        *capacity = new_capacity;
    }
    memcpy((char*)*array + (*count) * element_size, element, element_size);
    (*count)++;
}

// Initialize world
void init_world(ObjWorld* world, int initial_capacity) {
    init_dynamic_array((void**)&world->blocks, &world->count, &world->capacity, initial_capacity, sizeof(ObjBlock));
}

// Free world
void free_world(ObjWorld* world) {
    free_dynamic_array((void**)&world->blocks, &world->count, &world->capacity);
}

// Append block
void append_block(ObjWorld* world, ObjBlock block) {
    append_to_dynamic_array((void**)&world->blocks, &world->count, &world->capacity, &block, sizeof(ObjBlock));
}

// Generate world
void generate_world(ObjWorld* world, int size) {
  init_world(world, size * size + 4 * size * 3); // Account for ground + walls

  // Generate flat terrain at y=1.0
  for (int x = 0; x < size; x++) {
      for (int z = 0; z < size; z++) {
          float height = 1.0f; // Fixed ground height
          Color color = (Color){100, 150, 50, 255}; // Greenish ground

          ObjBlock block = {
              .size = {1.0f, 1.0f, 1.0f},
              .position = {(float)x, height, (float)z},
              .color = color
          };
          append_block(world, block);
      }
  }

  // Generate walls along edges (x=0, x=size-1, z=0, z=size-1)
  int wall_height = 3; // 3 blocks high
  Color wall_color = (Color){128, 128, 128, 255}; // Gray walls

  // Walls at x=0 and x=size-1
  for (int z = 0; z < size; z++) {
      for (int y = 1; y <= wall_height; y++) {
          // Wall at x=0
          ObjBlock block1 = {
              .size = {1.0f, 1.0f, 1.0f},
              .position = {0.0f, (float)y, (float)z},
              .color = wall_color
          };
          append_block(world, block1);

          // Wall at x=size-1
          ObjBlock block2 = {
              .size = {1.0f, 1.0f, 1.0f},
              .position = {(float)(size - 1), (float)y, (float)z},
              .color = wall_color
          };
          append_block(world, block2);
      }
  }

  // Walls at z=0 and z=size-1 (excluding corners to avoid duplicates)
  for (int x = 1; x < size - 1; x++) {
      for (int y = 1; y <= wall_height; y++) {
          // Wall at z=0
          ObjBlock block1 = {
              .size = {1.0f, 1.0f, 1.0f},
              .position = {(float)x, (float)y, 0.0f},
              .color = wall_color
          };
          append_block(world, block1);

          // Wall at z=size-1
          ObjBlock block2 = {
              .size = {1.0f, 1.0f, 1.0f},
              .position = {(float)x, (float)y, (float)(size - 1)},
              .color = wall_color
          };
          append_block(world, block2);
      }
  }
}


// Get block at (x, y, z) with range check for Y
ObjBlock* get_block(ObjWorld* world, int x, float y, int z) {
  ObjBlock* highest_block = NULL;
  float highest_top = -FLT_MAX;

  for (int i = 0; i < world->count; i++) {
      ObjBlock* block = &world->blocks[i];
      float block_x = block->position.x;
      float block_z = block->position.z;
      if (fabsf(block_x - (float)x) <= 0.5f && fabsf(block_z - (float)z) <= 0.5f) {
          float block_top = block->position.y + block->size.y * 0.5f;
          if (block_top <= y + 0.5f && block_top > highest_top) { // Relaxed y check
              highest_top = block_top;
              highest_block = block;
          }
      }
  }

  if (highest_block) {
      printf("get_block: Found block at (%.1f, %.1f, %.1f), Top: %.1f, Queried y: %.2f, x=%d, z=%d\n",
             highest_block->position.x, highest_block->position.y, highest_block->position.z, highest_top, y, x, z);
  } else {
      printf("get_block: No block found at x=%d, z=%d, y=%.2f\n", x, z, y);
  }

  return highest_block;
}

// Initialize candidates
void init_candidates(Candidates* candidates, int initial_capacity) {
    init_dynamic_array((void**)&candidates->blocks, &candidates->count, &candidates->capacity, initial_capacity, sizeof(ObjBlock*));
}

// Free candidates
void free_candidates(Candidates* candidates) {
    free_dynamic_array((void**)&candidates->blocks, &candidates->count, &candidates->capacity);
}

// Append candidate
void append_candidate(Candidates* candidates, ObjBlock* block) {
    append_to_dynamic_array((void**)&candidates->blocks, &candidates->count, &candidates->capacity, &block, sizeof(ObjBlock*));
}

// Initialize debug cubes
void init_debug_cubes(DebugCubes* debug_cubes, int initial_capacity) {
    init_dynamic_array((void**)&debug_cubes->blocks, &debug_cubes->count, &debug_cubes->capacity, initial_capacity, sizeof(ObjBlock*));
}

// Free debug cubes
void free_debug_cubes(DebugCubes* debug_cubes) {
    free_dynamic_array((void**)&debug_cubes->blocks, &debug_cubes->count, &debug_cubes->capacity);
}

// Append debug cube
void append_debug_cube(DebugCubes* debug_cubes, ObjBlock* block) {
    append_to_dynamic_array((void**)&debug_cubes->blocks, &debug_cubes->count, &debug_cubes->capacity, &block, sizeof(ObjBlock*));
}

// Initialize collisions
void init_collisions(Collisions* collisions, int initial_capacity) {
    init_dynamic_array((void**)&collisions->collisions, &collisions->count, &collisions->capacity, initial_capacity, sizeof(Collision));
}

// Free collisions
void free_collisions(Collisions* collisions) {
    free_dynamic_array((void**)&collisions->collisions, &collisions->count, &collisions->capacity);
}

// Append collision
void append_collision(Collisions* collisions, Collision collision) {
    append_to_dynamic_array((void**)&collisions->collisions, &collisions->count, &collisions->capacity, &collision, sizeof(Collision));
}

// Initialize debug collisions
void init_debug_collisions(DebugCollisions* debug_collisions, int initial_capacity) {
    init_dynamic_array((void**)&debug_collisions->collisions, &debug_collisions->count, &debug_collisions->capacity, initial_capacity, sizeof(Collision));
}

// Free debug collisions
void free_debug_collisions(DebugCollisions* debug_collisions) {
    free_dynamic_array((void**)&debug_collisions->collisions, &debug_collisions->count, &debug_collisions->capacity);
}

// Append debug collision
void append_debug_collision(DebugCollisions* debug_collisions, Collision collision) {
    append_to_dynamic_array((void**)&debug_collisions->collisions, &debug_collisions->count, &debug_collisions->capacity, &collision, sizeof(Collision));
}

// Check if point is in player's bounding cylinder
bool point_in_player_bounding_cylinder(Vector3 point, ObjPlayer* player) {
    float cylinder_center_y = player->position.y - player->height / 2.0f;
    float dx = point.x - player->position.x;
    float dy = point.y - cylinder_center_y;
    float dz = point.z - player->position.z;
    float r_sq = dx * dx + dz * dz;

    return fabsf(dy) < player->height / 2.0f && r_sq < player->radius * player->radius;
}

// Compare function for sorting collisions by overlap
int compare_collisions(const void* a, const void* b) {
    const Collision* ca = (const Collision*)a;
    const Collision* cb = (const Collision*)b;
    return (ca->overlap < cb->overlap) ? -1 : (ca->overlap > cb->overlap) ? 1 : 0;
}

// Helper function to project velocity for sliding
Vector3 slide_velocity(Vector3 velocity, Vector3 normal) {
  float dot = Vector3DotProduct(velocity, normal);
  if (dot < 0.0f) {
      Vector3 normal_component = Vector3Scale(normal, dot);
      return Vector3Subtract(velocity, normal_component);
  }
  return velocity;
}

// Resolve collisions
void resolve_collisions(ObjWorld* world, Collisions* collisions, ObjPlayer* player) {
  if (collisions->count > 1) {
      qsort(collisions->collisions, collisions->count, sizeof(Collision), compare_collisions);
  }

  bool ground_handled = false;

  // First pass: Handle ground collision
  for (int i = 0; i < collisions->count; i++) {
      Collision* collision = &collisions->collisions[i];

      if (!point_in_player_bounding_cylinder(collision->contactPoint, player)) {
          continue;
      }

      if (collision->overlap < 0.01f || Vector3Length(collision->normal) < 0.1f) {
          continue;
      }

      // Handle ground collision
      if (collision->onGround && collision->normal.y > 0.5f && !ground_handled) {
          player->onGround = true;
          player->velocity.y = 0.0f;
          float block_top = collision->block->position.y + (collision->block->size.y * 0.5f);
          float target_y = block_top + (player->height * 0.5f);
          if (player->position.y < target_y) {
              player->position.y = target_y;
          }
          ground_handled = true;
          printf("Resolved ground: Block at (%.1f, %.1f, %.1f), Player y=%.2f, Overlap: %.2f\n",
                 collision->block->position.x, collision->block->position.y, collision->block->position.z,
                 player->position.y, collision->overlap);
          continue;
      }

      // Handle wall collision
      if (!collision->onGround && collision->overlap >= 0.01f) {
          Vector3 delta_position = Vector3Scale(collision->normal, collision->overlap);
          player->position = Vector3Add(player->position, delta_position);

          float dot = Vector3DotProduct(player->velocity, collision->normal);
          if (dot < -0.01f) {
              player->velocity = slide_velocity(player->velocity, collision->normal);
              if (fabsf(collision->normal.x) > 0.5f) {
                  player->velocity.x = 0.0f;
              }
              if (fabsf(collision->normal.z) > 0.5f) {
                  player->velocity.z = 0.0f;
              }
          }

          printf("Wall collision: Normal (%.1f, %.1f, %.1f), Overlap: %.2f, Vel: (%.2f, %.2f, %.2f), Pos: (%.2f, %.2f, %.2f)\n",
                 collision->normal.x, collision->normal.y, collision->normal.z,
                 collision->overlap,
                 player->velocity.x, player->velocity.y, player->velocity.z,
                 player->position.x, player->position.y, player->position.z);
      }
  }

  // Fallback: Check for ground proximity if no ground collision
  if (!ground_handled) {
      float player_bottom = player->position.y - (player->height * 0.5f);
      ObjBlock* block_below = get_block(world, (int)player->position.x, player_bottom, (int)player->position.z); // Fixed: pass world
      if (block_below) {
          float block_top = block_below->position.y + (block_below->size.y * 0.5f);
          if (fabsf(player_bottom - block_top) < 0.2f && player->velocity.y <= 0.0f) {
              player->onGround = true;
              player->velocity.y = 0.0f;
              player->position.y = block_top + (player->height * 0.5f);
              printf("Fallback ground correction: Block at (%.1f, %.1f, %.1f), Player y=%.2f\n",
                     block_below->position.x, block_below->position.y, block_below->position.z,
                     player->position.y);
          } else if (player->velocity.y > 0.0f || player->position.y > block_top + 0.2f) {
              player->onGround = false;
              printf("Reset onGround: Vel.y=%.2f, Pos.y=%.2f, Block top=%.2f\n",
                     player->velocity.y, player->position.y, block_top);
          }
      } else {
          printf("Fallback: No block below at x=%d, z=%d, player_bottom=%.2f\n",
                 (int)player->position.x, (int)player->position.z, player_bottom);
      }
  }

  printf("Post-resolve: Pos (%.2f, %.2f, %.2f), Vel (%.2f, %.2f, %.2f), onGround=%d\n",
         player->position.x, player->position.y, player->position.z,
         player->velocity.x, player->velocity.y, player->velocity.z, player->onGround);
}


// Narrow-phase collision detection
Collisions narrow_phase(Candidates* candidates, ObjPlayer* player) {
  Collisions collisions;
  init_collisions(&collisions, candidates->count ? candidates->count : 1);

  for (int i = 0; i < candidates->count; i++) {
      ObjBlock* block = candidates->blocks[i];

      Vector3 cylinder_center = {player->position.x, player->position.y - player->height / 2.0f, player->position.z};
      Vector3 closest_point = {
          fmaxf(block->position.x - 0.5f, fminf(player->position.x, block->position.x + 0.5f)),
          fmaxf(block->position.y - 0.5f, fminf(cylinder_center.y, block->position.y + 0.5f)),
          fmaxf(block->position.z - 0.5f, fminf(player->position.z, block->position.z + 0.5f))
      };

      float dx = closest_point.x - player->position.x;
      float dy = closest_point.y - cylinder_center.y;
      float dz = closest_point.z - player->position.z;

      float block_top = block->position.y + (block->size.y * 0.5f);
      float player_bottom = player->position.y - (player->height * 0.5f);

      if (point_in_player_bounding_cylinder(closest_point, player)) {
          float overlap_y = (player->height / 2.0f) - fabsf(dy);
          float overlap_xz = player->radius - sqrtf(dx * dx + dz * dz);

          Vector3 normal;
          float overlap;
          bool on_ground = false;

          // Ground detection: Increased threshold
          float ground_threshold = 0.3f; // Relaxed to handle fast falls
          if (player_bottom <= block_top + ground_threshold && player_bottom >= block_top - ground_threshold && player->velocity.y <= 0.0f) {
              normal = (Vector3){0.0f, 1.0f, 0.0f};
              overlap = block_top - player_bottom + 0.01f;
              if (overlap < 0.0f) overlap = 0.01f; // Ensure positive overlap
              on_ground = true;
              // player->onGround = true; // Set here for immediate effect
          } else if (overlap_xz > 0.01f) {
              // Wall collision
              Vector3 delta = {player->position.x - block->position.x, 0.0f, player->position.z - block->position.z};
              float abs_dx = fabsf(delta.x);
              float abs_dz = fabsf(delta.z);
              if (abs_dx > abs_dz + 0.3f) {
                  normal = (Vector3){delta.x > 0.0f ? 1.0f : -1.0f, 0.0f, 0.0f};
              } else {
                  normal = (Vector3){0.0f, 0.0f, delta.z > 0.0f ? 1.0f : -1.0f};
              }
              overlap = overlap_xz;
          } else {
              continue; // Skip invalid collisions
          }

          Collision collision = {
              .block = block,
              .contactPoint = closest_point,
              .normal = normal,
              .overlap = overlap,
              .onGround = on_ground
          };
          append_collision(&collisions, collision);
          printf("Narrow-phase collision: Block (%.1f, %.1f, %.1f), Normal (%.1f, %.1f, %.1f), Overlap: %.2f, onGround=%d\n",
                 block->position.x, block->position.y, block->position.z,
                 normal.x, normal.y, normal.z, overlap, on_ground);
      }
  }

  printf("Narrow-phase: %d collisions\n", collisions.count);
  return collisions;
}

// Visualize contact point
void add_contact_pointer_helper(Vector3 contact_point) {
    DrawSphere(contact_point, 0.1f, YELLOW);
}


Candidates broad_phase(ObjWorld* world, ObjPlayer* player) {
  Candidates candidates;
  init_candidates(&candidates, 16);

  // Bounding box with larger downward extension
  BoundingBox player_box = {
      .min = {player->position.x - player->radius - 0.1f, player->position.y - player->height / 2.0f - 0.5f, player->position.z - player->radius - 0.1f},
      .max = {player->position.x + player->radius + 0.1f, player->position.y + player->height / 2.0f + 0.1f, player->position.z + player->radius + 0.1f}
  };

  printf("Player box: Min (%.2f, %.2f, %.2f), Max (%.2f, %.2f, %.2f)\n",
         player_box.min.x, player_box.min.y, player_box.min.z,
         player_box.max.x, player_box.max.y, player_box.max.z);

  for (int i = 0; i < world->count; i++) {
      ObjBlock* block = &world->blocks[i];
      BoundingBox block_box = {
          .min = Vector3Subtract(block->position, Vector3Scale(block->size, 0.5f)),
          .max = Vector3Add(block->position, Vector3Scale(block->size, 0.5f))
      };

      if (CheckCollisionBoxes(player_box, block_box)) {
          append_candidate(&candidates, block);
          printf("Broad-phase candidate: (%.1f, %.1f, %.1f), Player y=%.2f\n",
                 block->position.x, block->position.y, block->position.z, player->position.y);
      }
  }

  printf("Broad-phase: %d candidates\n", candidates.count);
  return candidates;
}


// Update player input
void update_input_player(float dt, ObjPlayer* player) {
  player->input = (Vector3){0.0f, 0.0f, 0.0f};

  if (IsKeyDown(KEY_W)) player->input.z -= 1.0f;
  if (IsKeyDown(KEY_S)) player->input.z += 1.0f;
  if (IsKeyDown(KEY_A)) player->input.x -= 1.0f;
  if (IsKeyDown(KEY_D)) player->input.x += 1.0f;

  printf("Input: (%.1f, %.1f, %.1f)\n", player->input.x, player->input.y, player->input.z);

  if (IsKeyPressed(KEY_SPACE)) {
      if (player->onGround) {
          player->velocity.y = 6.0f;
          player->onGround = false;
          printf("Jump triggered! Pos: (%.2f, %.2f, %.2f), Vel.y=%.2f\n",
                 player->position.x, player->position.y, player->position.z, player->velocity.y);
      } else {
          printf("Jump failed: Not on ground (onGround=%d), Pos: (%.2f, %.2f, %.2f)\n",
                 player->onGround, player->position.x, player->position.y, player->position.z);
      }
  }

  float mag = Vector3Length(player->input);
  if (mag > 1.0f) {
      player->input = Vector3Scale(player->input, 1.0f / mag);
  }

  Vector3 target_velocity = {player->input.x * player->maxSpeed, player->velocity.y, player->input.z * player->maxSpeed};
  float lerp_factor = 1.0f - powf(0.001f, dt);
  player->velocity.x = Lerp(player->velocity.x, target_velocity.x, lerp_factor);
  player->velocity.z = Lerp(player->velocity.z, target_velocity.z, lerp_factor);

  printf("Post-input velocity: (%.2f, %.2f, %.2f)\n", player->velocity.x, player->velocity.y, player->velocity.z);
}


// Physics update
void physics_update(ObjWorld* world, ObjPlayer* player, float dt) {
  static float accumulatedTime = 0.0f;
  float simulationRate = 60.0f;
  float stepSize = 1.0f / simulationRate;
  float maxStepSize = 0.0167f;
  static Vector3 last_position = {0};
  static Vector3 last_velocity = {0};
  static bool skip_collision = false;

  accumulatedTime += dt;

  printf("Physics update: dt=%.4f, accumulatedTime=%.4f, stepSize=%.4f\n", dt, accumulatedTime, stepSize);

  int steps = 0;
  while (accumulatedTime >= stepSize) {
      float currentStep = fminf(stepSize, maxStepSize);

      printf("Physics step %d: Pos (%.2f, %.2f, %.2f), Vel (%.2f, %.2f, %.2f), onGround=%d\n",
             steps, player->position.x, player->position.y, player->position.z,
             player->velocity.x, player->velocity.y, player->velocity.z, player->onGround);

      update_input_player(currentStep, player);

      if (!player->onGround) {
          player->velocity.y -= gravity * currentStep;
          if (player->velocity.y < -10.0f) {
              player->velocity.y = -10.0f;
              printf("Capped velocity.y: %.2f\n", player->velocity.y);
          }
          printf("Applied gravity: Vel.y=%.2f\n", player->velocity.y);
      }

      player->position.x += player->velocity.x * currentStep;
      player->position.y += player->velocity.y * currentStep;
      player->position.z += player->velocity.z * currentStep;
      printf("Updated position: Pos (%.2f, %.2f, %.2f)\n",
             player->position.x, player->position.y, player->position.z);

      if (Vector3Distance(player->position, last_position) < 0.01f &&
          Vector3Distance(player->velocity, last_velocity) < 0.01f &&
          Vector3Length(player->input) < 0.01f) {
          skip_collision = true;
          printf("Skipping collision: Stationary\n");
      } else {
          skip_collision = false;
          last_position = player->position;
          last_velocity = player->velocity;
      }

      if (!skip_collision) {
          Candidates candidates = broad_phase(world, player);
          Collisions collisions = narrow_phase(&candidates, player);
          resolve_collisions(world, &collisions, player);
          free_collisions(&collisions);
          free_candidates(&candidates);
      }

      accumulatedTime -= stepSize;
      steps++;
  }

  if (accumulatedTime > 0.0f && accumulatedTime < stepSize && dt > 0.0f) {
      static float totalTime = 0.0f;
      totalTime += dt;
      if (totalTime >= stepSize) {
          printf("Forced physics step: totalTime=%.4f\n", totalTime);
          float currentStep = fminf(stepSize, maxStepSize);
          update_input_player(currentStep, player);
          if (!player->onGround) {
              player->velocity.y -= gravity * currentStep;
              if (player->velocity.y < -10.0f) player->velocity.y = -10.0f;
          }
          player->position.x += player->velocity.x * currentStep;
          player->position.y += player->velocity.y * currentStep;
          player->position.z += player->velocity.z * currentStep;

          if (Vector3Distance(player->position, last_position) < 0.01f &&
              Vector3Distance(player->velocity, last_velocity) < 0.01f &&
              Vector3Length(player->input) < 0.01f) {
              skip_collision = true;
              printf("Skipping collision (forced step): Stationary\n");
          } else {
              skip_collision = false;
              last_position = player->position;
              last_velocity = player->velocity;
          }

          if (!skip_collision) {
              Candidates candidates = broad_phase(world, player);
              Collisions collisions = narrow_phase(&candidates, player);
              resolve_collisions(world, &collisions, player);
              free_collisions(&collisions);
              free_candidates(&candidates);
          }

          totalTime -= stepSize;
          accumulatedTime = 0.0f;
      }
  }
}



// Render the world
void render_world(ObjWorld* world, ObjPlayer* player, DebugCubes* debug_cubes, DebugCollisions* debug_collisions) {
    for (int i = 0; i < world->count; i++) {
        ObjBlock* block = &world->blocks[i];
        DrawCubeV(block->position, block->size, block->color);
        DrawCubeWiresV(block->position, block->size, BLACK);
    }
    // DrawCubeV(player->position, player->size, player->color);
    DrawCubeWiresV(player->position, player->size, BLACK);
    for (int i = 0; i < debug_cubes->count; i++) {
        ObjBlock* block = debug_cubes->blocks[i];
        DrawCubeWiresV(block->position, block->size, YELLOW);
    }
    for (int i = 0; i < debug_collisions->count; i++) {
        Collision* collision = &debug_collisions->collisions[i];
        add_contact_pointer_helper(collision->contactPoint);
        Vector3 normal_end = Vector3Add(collision->contactPoint, Vector3Scale(collision->normal, 1.0f));
        DrawLine3D(collision->contactPoint, normal_end, RED);
    }
}

int main(void) {
    const int screenWidth = 800;
    const int screenHeight = 600;

    InitWindow(screenWidth, screenHeight, "Collision Demo");
    SetTargetFPS(60);
    DisableCursor();

    Camera3D camera = { 0 };
    camera.up = (Vector3){0.0f, 1.0f, 0.0f};
    camera.fovy = 45.0f;
    camera.projection = CAMERA_PERSPECTIVE;

    ObjPlayer player = {
      .maxSpeed = 3.0f,
      .size = {1.0f, 1.85f, 1.0f},
      .height = 1.85f,
      .radius = 0.4f,
      .position = {4.5f, 3.85f, 4.5f}, // Centered in 8x8 world
      .color = {0, 255, 0, 255},
      .velocity = {0.0f, 0.0f, 0.0f},
      .input = {0.0f, 0.0f, 0.0f},
      .jumpGround = 2.0f,
      .onGround = true
    };

    ObjWorld world;
    DebugCubes debug_cubes;
    DebugCollisions debug_collisions;
    int world_size = t_width;
    generate_world(&world, world_size);
    init_debug_cubes(&debug_cubes, 4);
    init_debug_collisions(&debug_collisions, 4);

    printf("World contains %d blocks:\n", world.count);
    for (int i = 0; i < world.count; i++) {
        ObjBlock* block = &world.blocks[i];
        printf("Block %d: Position (%.1f, %.1f, %.1f), Color (%d, %d, %d, %d)\n",
               i, block->position.x, block->position.y, block->position.z,
               block->color.r, block->color.g, block->color.b, block->color.a);
    }

    float accumulator = 0.0f;
    while (!WindowShouldClose()) {

        if (IsKeyPressed(KEY_R)) {
          player.position.x = 0.5f; // Slightly off wall to avoid x=0 collision
          player.position.y = 2.425f; // Ground top (1.5) + player->height/2 (0.925)
          player.position.z = 4.0f;
          player.velocity = (Vector3){0.0f, 0.0f, 0.0f};
          player.onGround = true;
          printf("Reset to ground: Pos (%.2f, %.2f, %.2f)\n", 
                 player.position.x, player.position.y, player.position.z);
        }
        if (IsKeyPressed(KEY_T)) {
            player.position.x = 0.5f; // Slightly off wall
            player.position.y = 2.425f; // Ground top (1.5) + player->height/2 (0.925)
            player.position.z = 4.0f;
            player.velocity = (Vector3){0.0f, 0.0f, 0.0f};
            player.onGround = true;
            printf("Reset to ground: Pos (%.2f, %.2f, %.2f)\n",
                   player.position.x, player.position.y, player.position.z);
        }

        float dt = GetFrameTime();
        // accumulator += dt;
        // // Clamp accumulator to prevent spiral-of-death
        // if (accumulator > 0.2f) accumulator = 0.2f;
        // while (accumulator >= stepSize) {
        //     physics_update(&world, &player, stepSize);
        //     accumulator -= stepSize;
        // }
        physics_update(&world, &player, dt);

        camera.position = Vector3Add(player.position, (Vector3){0.0f, 3.0f, 5.0f});
        camera.target = player.position;

        BeginDrawing();
        ClearBackground(RAYWHITE);

        BeginMode3D(camera);
        render_world(&world, &player, &debug_cubes, &debug_collisions);
        DrawGrid(world_size, 1.0f);
        EndMode3D();

        DrawFPS(10, 10);
        float player_base_y = player.position.y - player.height * 0.5f;
        ObjBlock* block_below = get_block(&world, (int)player.position.x, player_base_y, (int)player.position.z); // Fixed: pass &world
        float block_height = block_below ? block_below->position.y : -1.0f;
        DrawText(TextFormat("OnGround: %d, Collisions: %d, BlockY: %.1f, Vel: (%.1f, %.1f, %.1f), Acc: %.4f",
                            player.onGround, debug_collisions.count, block_height,
                            player.velocity.x, player.velocity.y, player.velocity.z, accumulator),
                 10, 30, 20, BLACK);

        EndDrawing();
    }

    free_world(&world);
    free_debug_cubes(&debug_cubes);
    free_debug_collisions(&debug_collisions);
    CloseWindow();
    return 0;
}