collide-mesh 0.2.0

#![expect(clippy::expect_used)]

use collide_capsule::Capsule;
use collide_mesh::{CollisionWorld, TriangleMesh};
use collide_ray::Ray;
use ga3::Vector;

const RADIUS: f32 = 0.4;
const HEIGHT: f32 = 1.8;
fn up() -> Vector<f32> {
    Vector::new(0.0, 1.0, 0.0)
}

fn capsule_at(position: Vector<f32>) -> Capsule<Vector<f32>> {
    Capsule::new(
        RADIUS,
        position + Vector::y(RADIUS),
        position + Vector::y(HEIGHT - RADIUS),
    )
}

fn floor_world() -> CollisionWorld {
    let positions = [
        [-10.0, 0.0, -10.0],
        [10.0, 0.0, -10.0],
        [10.0, 0.0, 10.0],
        [-10.0, 0.0, 10.0],
    ];
    let indices = [0, 2, 1, 0, 3, 2];
    CollisionWorld::new(vec![TriangleMesh::from_vertices(&positions, &indices)])
}

fn wall_world() -> CollisionWorld {
    let positions = [
        [2.0, -1.0, -10.0],
        [2.0, -1.0, 10.0],
        [2.0, 5.0, 10.0],
        [2.0, 5.0, -10.0],
    ];
    let indices = [0, 1, 2, 0, 2, 3];
    CollisionWorld::new(vec![TriangleMesh::from_vertices(&positions, &indices)])
}

#[test]
fn capsule_rests_on_floor() {
    let world = floor_world();
    let result = world.collide_capsule(
        &capsule_at(Vector::new(0.0, 0.0, 0.0)),
        up(),
        Vector::new(0.0, 0.0, 0.0),
    );
    let ground = result.ground.expect("should be grounded");
    assert!(ground.distance.abs() < 1e-4);
    assert!((ground.normal.y - 1.0).abs() < 1e-4);
}

#[test]
fn capsule_above_floor_is_airborne() {
    let world = floor_world();
    let result = world.collide_capsule(
        &capsule_at(Vector::new(0.0, 2.0, 0.0)),
        up(),
        Vector::new(0.0, 0.0, 0.0),
    );
    assert!(result.ground.is_none());
}

#[test]
fn falling_capsule_lands_on_floor() {
    let world = floor_world();
    let result = world.collide_capsule(
        &capsule_at(Vector::new(0.0, -0.1, 0.0)),
        up(),
        Vector::new(0.0, -5.0, 0.0),
    );
    let ground = result.ground.expect("should be grounded");
    assert!((ground.distance - 0.1).abs() < 1e-4);
}

#[test]
fn wall_pushes_capsule_out() {
    let world = wall_world();
    let result = world.collide_capsule(
        &capsule_at(Vector::new(1.8, 0.0, 0.0)),
        up(),
        Vector::new(0.0, 0.0, 0.0),
    );
    assert!(result.ground.is_none());
    assert!(result.push.x < -1e-4);
    assert!(result.push.y.abs() < 1e-4);
}

#[test]
fn gentle_slope_counts_as_ground() {
    let positions = [
        [-10.0, 0.0, -10.0],
        [10.0, 4.0, -10.0],
        [10.0, 4.0, 10.0],
        [-10.0, 0.0, 10.0],
    ];
    let indices = [0, 2, 1, 0, 3, 2];
    let world = CollisionWorld::new(vec![TriangleMesh::from_vertices(&positions, &indices)]);
    let expected_height = 2.0;
    let result = world.collide_capsule(
        &capsule_at(Vector::new(0.0, expected_height - 0.05, 0.0)),
        up(),
        Vector::new(0.0, -1.0, 0.0),
    );
    let ground = result.ground.expect("should be grounded");
    let landed = expected_height - 0.05 + ground.distance;
    assert!((landed - expected_height).abs() < 0.05);
}

#[test]
fn steep_slope_pushes_instead_of_grounding() {
    let positions = [
        [-10.0, -20.0, -10.0],
        [10.0, 20.0, -10.0],
        [10.0, 20.0, 10.0],
        [-10.0, -20.0, 10.0],
    ];
    let indices = [0, 2, 1, 0, 3, 2];
    let world = CollisionWorld::new(vec![TriangleMesh::from_vertices(&positions, &indices)]);
    let result = world.collide_capsule(
        &capsule_at(Vector::new(0.0, -0.2, 0.0)),
        up(),
        Vector::new(0.0, 0.0, 0.0),
    );
    assert!(result.ground.is_none());
    assert!(result.push != Vector::new(0.0, 0.0, 0.0));
}

#[test]
fn raycast_hits_floor_from_above() {
    let world = floor_world();
    let ray = Ray::new(Vector::new(0.0, 5.0, 0.0), Vector::y(-1.0));
    let distance = world.raycast(&ray, 100.0);
    assert!(distance.is_some_and(|value| (value - 5.0).abs() < 1e-4));
}

#[test]
fn raycast_misses_outside_range() {
    let world = floor_world();
    let ray = Ray::new(Vector::new(0.0, 5.0, 0.0), Vector::y(-1.0));
    assert!(world.raycast(&ray, 3.0).is_none());
}

#[test]
fn raycast_ignores_backward_geometry() {
    let world = floor_world();
    let ray = Ray::new(Vector::new(0.0, 5.0, 0.0), Vector::y(1.0));
    assert!(world.raycast(&ray, 100.0).is_none());
}

#[test]
fn bvh_path_matches_small_mesh_path() {
    let mut positions = Vec::new();
    let mut indices = Vec::new();
    let mut base: u32 = 0;
    for row in 0..10u8 {
        for column in 0..10u8 {
            let x = f32::from(column) * 2.0 - 10.0;
            let z = f32::from(row) * 2.0 - 10.0;
            positions.extend_from_slice(&[
                [x, 0.0, z],
                [x + 2.0, 0.0, z],
                [x + 2.0, 0.0, z + 2.0],
                [x, 0.0, z + 2.0],
            ]);
            indices.extend_from_slice(&[base, base + 2, base + 1, base, base + 3, base + 2]);
            base += 4;
        }
    }
    let world = CollisionWorld::new(vec![TriangleMesh::from_vertices(&positions, &indices)]);

    for sample in 0..20u8 {
        let x = f32::from(sample) - 9.5;
        let result = world.collide_capsule(
            &capsule_at(Vector::new(x, 0.0, 0.5)),
            up(),
            Vector::new(0.0, 0.0, 0.0),
        );
        let Some(ground) = result.ground else {
            panic!("sample {sample} at x {x} should be grounded");
        };
        assert!(ground.distance.abs() < 1e-4);
    }
}

#[test]
fn degenerate_triangles_are_skipped() {
    let positions = [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [2.0, 0.0, 0.0]];
    let indices = [0, 1, 2, 0, 0, 0, 5, 6, 7];
    let mesh = TriangleMesh::from_vertices(&positions, &indices);
    let world = CollisionWorld::new(vec![mesh]);
    let result = world.collide_capsule(
        &capsule_at(Vector::new(0.0, 0.0, 0.0)),
        up(),
        Vector::new(0.0, 0.0, 0.0),
    );
    assert!(result.ground.is_none());
}

fn capsule_at_z(position: Vector<f32>, up: Vector<f32>) -> Capsule<Vector<f32>> {
    Capsule::new(
        RADIUS,
        position + up * RADIUS,
        position + up * (HEIGHT - RADIUS),
    )
}

#[test]
fn z_up_capsule_rests_on_a_z_up_floor() {
    let up = Vector::new(0.0, 0.0, 1.0);
    let positions = [
        [-10.0, -10.0, 0.0],
        [10.0, -10.0, 0.0],
        [10.0, 10.0, 0.0],
        [-10.0, 10.0, 0.0],
    ];
    let indices = [0, 1, 2, 0, 2, 3];
    let world = CollisionWorld::new(vec![TriangleMesh::from_vertices(&positions, &indices)]);
    let result = world.collide_capsule(
        &capsule_at_z(Vector::new(0.0, 0.0, 0.0), up),
        up,
        Vector::new(0.0, 0.0, 0.0),
    );
    let ground = result.ground.expect("should be grounded");
    assert!(ground.distance.abs() < 1e-4);
    assert!((ground.normal.z - 1.0).abs() < 1e-4);
}

#[test]
fn z_up_wall_pushes_capsule_out() {
    let up = Vector::new(0.0, 0.0, 1.0);
    let positions = [
        [2.0, -10.0, -1.0],
        [2.0, 10.0, -1.0],
        [2.0, 10.0, 5.0],
        [2.0, -10.0, 5.0],
    ];
    let indices = [0, 1, 2, 0, 2, 3];
    let world = CollisionWorld::new(vec![TriangleMesh::from_vertices(&positions, &indices)]);
    let result = world.collide_capsule(
        &capsule_at_z(Vector::new(1.8, 0.0, 0.0), up),
        up,
        Vector::new(0.0, 0.0, 0.0),
    );
    assert!(result.ground.is_none());
    assert!(result.push.x < -1e-4);
    assert!(result.push.z.abs() < 1e-4);
}