collide-mesh 0.1.0

use ga3::Vector;

use crate::{Aabb, bvh::Bvh, triangle};

const BVH_THRESHOLD: usize = 16;
const WALKABLE_NORMAL_Y: f32 = 0.5;
const GROUND_BELOW_TOLERANCE: f32 = 0.2;
const GROUND_REST_TOLERANCE: f32 = 0.05;
const GROUND_OVERLAP_RATIO: f32 = 0.6;

/// A static triangle mesh prepared for capsule collision and raycasts.
pub struct TriangleMesh {
    triangles: Vec<[Vector<f32>; 3]>,
    normals: Vec<Vector<f32>>,
    bounds: Aabb,
    bvh: Option<Bvh>,
}

/// The result of resolving a capsule against a single mesh.
pub struct TriangleHit {
    /// Whether the capsule is supported by walkable ground of this mesh.
    pub grounded: bool,
    /// The height of the supporting ground.
    pub ground_y: f32,
    /// The face normal of the supporting ground.
    pub ground_normal: Vector<f32>,
    /// The accumulated displacement out of steep geometry.
    pub push: Vector<f32>,
}

impl TriangleMesh {
    /// Builds a mesh from raw vertex positions and triangle indices.
    ///
    /// Degenerate triangles and out-of-range indices are skipped.
    /// A bounding volume hierarchy is built once the mesh exceeds 16 triangles.
    pub fn from_vertices(positions: &[[f32; 3]], indices: &[u32]) -> Self {
        let mut triangles = Vec::with_capacity(indices.len() / 3);
        let mut normals = Vec::with_capacity(indices.len() / 3);
        let mut bounds = Aabb::EMPTY;

        for chunk in indices.chunks_exact(3) {
            let &[index_a, index_b, index_c] = chunk else {
                continue;
            };
            let (Some(&a), Some(&b), Some(&c)) = (
                positions.get(index_a as usize),
                positions.get(index_b as usize),
                positions.get(index_c as usize),
            ) else {
                continue;
            };

            let corners = [Vector::from(a), Vector::from(b), Vector::from(c)];
            let Some(normal) = triangle::face_normal(&corners) else {
                continue;
            };

            for corner in &corners {
                bounds.include((*corner).into());
            }
            triangles.push(corners);
            normals.push(normal);
        }

        let bvh = (triangles.len() > BVH_THRESHOLD).then(|| {
            let triangle_bounds: Vec<Aabb> = triangles.iter().map(triangle_bounds).collect();
            Bvh::build(&triangle_bounds)
        });

        Self {
            triangles,
            normals,
            bounds,
            bvh,
        }
    }

    /// Returns the bounding box of the whole mesh.
    pub const fn bounds(&self) -> &Aabb {
        &self.bounds
    }

    /// Resolves a vertical capsule against this mesh.
    ///
    /// `position` is the bottom of the capsule, `height` its total height.
    /// Returns `None` when nothing touches.
    pub fn collide_capsule(
        &self,
        position: Vector<f32>,
        velocity_y: f32,
        radius: f32,
        height: f32,
    ) -> Option<TriangleHit> {
        let capsule_bounds = Aabb {
            min: [position.x - radius, position.y, position.z - radius],
            max: [
                position.x + radius,
                position.y + height,
                position.z + radius,
            ],
        };
        if !self.bounds.overlaps(&capsule_bounds) {
            return None;
        }

        let segment_bottom = position + Vector::y(radius);
        let segment_top = position + Vector::y(height - radius);

        let feet = position.y;
        let falling = velocity_y <= 0.0;

        let mut grounded = false;
        let mut best_ground_y = f32::NEG_INFINITY;
        let mut best_ground_normal = Vector::y(1.0);
        let mut push = Vector::new(0.0, 0.0, 0.0);

        for index in self.overlapping_triangles(&capsule_bounds) {
            let (Some(corners), Some(&normal)) =
                (self.triangles.get(index), self.normals.get(index))
            else {
                continue;
            };

            if normal.y > WALKABLE_NORMAL_Y {
                let Some(ground_y) = triangle::ground_height_at(corners, position.x, position.z)
                else {
                    continue;
                };
                let overlap = ground_y - feet;
                if overlap < -GROUND_BELOW_TOLERANCE || overlap > height * GROUND_OVERLAP_RATIO {
                    continue;
                }
                let supported = (falling && overlap > 0.0)
                    || (-GROUND_BELOW_TOLERANCE..=GROUND_REST_TOLERANCE).contains(&overlap);
                if supported && ground_y > best_ground_y {
                    grounded = true;
                    best_ground_y = ground_y;
                    best_ground_normal = normal;
                }
            } else if let Some(correction) =
                triangle::capsule_push(segment_bottom, segment_top, radius, corners)
            {
                push += correction;
            }
        }

        if !grounded && push == Vector::new(0.0, 0.0, 0.0) {
            return None;
        }

        Some(TriangleHit {
            grounded,
            ground_y: best_ground_y,
            ground_normal: best_ground_normal,
            push,
        })
    }

    /// Returns the distance to the closest triangle hit by the ray, if any.
    pub fn raycast(
        &self,
        origin: Vector<f32>,
        direction: Vector<f32>,
        max_distance: f32,
    ) -> Option<f32> {
        let origin_array: [f32; 3] = origin.into();
        let inverse_direction = <[f32; 3]>::from(direction).map(f32::recip);

        let candidates = self.bvh.as_ref().map_or_else(
            || (0..self.triangles.len()).collect(),
            |bvh| bvh.ray_overlapping(origin_array, inverse_direction, max_distance),
        );

        let mut closest: Option<f32> = None;
        for index in candidates {
            let Some(corners) = self.triangles.get(index) else {
                continue;
            };
            if let Some(distance) = triangle::ray_intersection(origin, direction, corners)
                && distance <= max_distance
                && closest.is_none_or(|best| distance < best)
            {
                closest = Some(distance);
            }
        }
        closest
    }

    fn overlapping_triangles(&self, query: &Aabb) -> Vec<usize> {
        self.bvh.as_ref().map_or_else(
            || (0..self.triangles.len()).collect(),
            |bvh| bvh.overlapping(query),
        )
    }
}

fn triangle_bounds(corners: &[Vector<f32>; 3]) -> Aabb {
    let mut bounds = Aabb::EMPTY;
    for corner in corners {
        bounds.include((*corner).into());
    }
    bounds
}