c raylib collision3d test

README.md

Ported to mineccraft clone base on javascript. https://www.youtube.com/watch?v=_aK-1L-GC6I&list=PLtzt35QOXmkKALLv9RzT8oGwN5qwmRjTo&index=5

simple collision detect but still need work. move around but input bugged a bit.

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>

// 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 = 32.0f; // Feet/s²

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

// 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);

    for (int x = 0; x < size; x++) {
        for (int z = 0; z < size; z++) {
            float nx = (float)x / size * t_scale + t_offset;
            float nz = (float)z / size * t_scale + t_offset;
            float height = simple_noise(nx, nz) * t_magnitude * 4.0f;
            height = height < 0 ? 0 : height;
            height = roundf(height * 2.0f) / 2.0f; // Round to 0.5f

            Color color = (Color){
                (unsigned char)(100 + height * 50),
                (unsigned char)(150 - height * 30),
                50,
                255
            };

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

// Get block at (x, y, z) with range check for Y
ObjBlock* get_block(ObjWorld* world, int x, float y, int z) {
    for (int i = 0; i < world->count; i++) {
        ObjBlock* block = &world->blocks[i];
        if ((int)block->position.x == x &&
            fabsf(block->position.y - y) <= 0.5f &&
            (int)block->position.z == z) {
            return block;
        }
    }
    return NULL;
}

// 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;
}

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

    for (int i = 0; i < collisions->count; i++) {
        Collision* collision = &collisions->collisions[i];

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

        Vector3 delta_position = Vector3Scale(collision->normal, collision->overlap);
        player->position = Vector3Add(player->position, delta_position);

        float magnitude = Vector3DotProduct(player->velocity, collision->normal);
        if (magnitude > 0.0f) { // Only adjust if moving toward the surface
            Vector3 velocity_adjustment = Vector3Scale(collision->normal, magnitude);
            player->velocity = Vector3Subtract(player->velocity, velocity_adjustment);
        }

        if (collision->onGround) {
            player->onGround = true;
        }
    }
}

// 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;

        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;

            if (overlap_y < overlap_xz) {
                normal = (Vector3){0.0f, dy < 0 ? 1.0f : -1.0f, 0.0f};
                overlap = overlap_y;
                on_ground = true;
            } else {
                normal = Vector3Normalize((Vector3){-dx, 0.0f, -dz});
                overlap = overlap_xz;
            }

            Collision collision = {
                .block = block,
                .contactPoint = closest_point,
                .normal = normal,
                .overlap = overlap,
                .onGround = on_ground
            };
            append_collision(&collisions, collision);
        }
    }

    return collisions;
}

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

// Broad-phase collision detection
Candidates broad_phase(ObjWorld* world, ObjPlayer* player) {
    Candidates candidates;
    init_candidates(&candidates, 4);

    BoundingBox player_box = {
        .min = Vector3Subtract(player->position, Vector3Scale(player->size, 0.5f)),
        .max = Vector3Add(player->position, Vector3Scale(player->size, 0.5f))
    };
    player_box.min.y -= player->height * 0.5f;
    player_box.max.y += player->height * 0.5f;

    for (int x = (int)player_box.min.x - 1; x <= (int)player_box.max.x + 1; x++) {
        for (int y = (int)player_box.min.y - 1; y <= (int)player_box.max.y + 1; y++) {
            for (int z = (int)player_box.min.z - 1; z <= (int)player_box.max.z + 1; z++) {
                ObjBlock* block = get_block(world, x, (float)y, z);
                if (block) {
                    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);
                    }
                }
            }
        }
    }

    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;

    if (IsKeyPressed(KEY_SPACE)) {
        if (player->onGround) {
            player->velocity.y = player->jumpGround;
            player->onGround = false;
        } else {
            printf("Jump failed: onGround = false\n");
        }
    }

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

    player->velocity.x = player->input.x * player->maxSpeed;
    player->velocity.z = player->input.z * player->maxSpeed;
}

// Physics update
void physics_update(float dt, ObjWorld* world, ObjPlayer* player, DebugCubes* debug_cubes, DebugCollisions* debug_collisions) {
    // Apply gravity if not on ground
    if (!player->onGround) {
        player->velocity.y -= gravity * dt;
    } else {
        player->velocity.y = 0.0f;
    }

    update_input_player(dt, player);

    Candidates candidates = broad_phase(world, player);

    debug_cubes->count = 0;
    for (int i = 0; i < candidates.count; i++) {
        append_debug_cube(debug_cubes, candidates.blocks[i]);
    }

    Collisions collisions = narrow_phase(&candidates, player);

    debug_collisions->count = 0;
    for (int i = 0; i < collisions.count; i++) {
        append_debug_collision(debug_collisions, collisions.collisions[i]);
    }

    player->onGround = false; // Reset before resolving
    if (collisions.count > 0) {
        resolve_collisions(&collisions, player);
    }

    free_collisions(&collisions);
    free_candidates(&candidates);

    player->position = Vector3Add(player->position, Vector3Scale(player->velocity, dt));

    // Snap to ground if falling and close to a block
    if (!player->onGround && player->velocity.y <= 0.0f) {
        float player_base_y = player->position.y - player->height * 0.5f;
        for (float y_offset = -1.0f; y_offset <= 0.0f; y_offset += 0.5f) {
            ObjBlock* block_below = get_block(world, (int)player->position.x, player_base_y + y_offset, (int)player->position.z);
            if (block_below) {
                float ground_y = block_below->position.y + block_below->size.y * 0.5f + player->height * 0.5f;
                if (player->position.y - ground_y <= 0.3f) {
                    player->position.y = ground_y;
                    player->onGround = true;
                    player->velocity.y = 0.0f;
                    break;
                }
            }
        }
    }
}

// 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 = 10.0f,
        .size = {1.0f, 1.75f, 1.0f},
        .height = 1.75f,
        .radius = 0.5f,
        .position = {4.0f, 5.0f, 4.0f},
        .color = {0, 255, 0, 255},
        .velocity = {0.0f, 0.0f, 0.0f},
        .input = {0.0f, 0.0f, 0.0f},
        .jumpGround = 10.0f,
        .onGround = false
    };

    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()) {
        float dt = GetFrameTime();
        accumulator += dt;
        // Clamp accumulator to prevent spiral-of-death
        if (accumulator > 0.2f) accumulator = 0.2f;

        while (accumulator >= stepSize) {
            physics_update(stepSize, &world, &player, &debug_cubes, &debug_collisions);
            accumulator -= stepSize;
        }

        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);
        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;
}