collide-mesh 0.2.0

use ga3::Vector;
use inner_space::DotProduct;

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

const BVH_THRESHOLD: usize = 16;
const WALKABLE_NORMAL_THRESHOLD: 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 {
    /// The walkable ground of this mesh supporting the capsule, if any.
    pub ground: Option<Ground>,
    /// 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.
    #[must_use]
    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.
    #[must_use]
    pub const fn bounds(&self) -> &Aabb {
        &self.bounds
    }

    /// Resolves a capsule aligned with `up` against this mesh.
    ///
    /// `segment_bottom`/`segment_top` are the capsule's inner segment
    /// endpoints (sphere centers), `up` the unit vector walkable ground is
    /// measured against. Returns `None` when nothing touches.
    #[must_use]
    pub fn collide_capsule(
        &self,
        segment_bottom: Vector<f32>,
        segment_top: Vector<f32>,
        radius: f32,
        up: Vector<f32>,
        motion: Motion,
    ) -> Option<TriangleHit> {
        let pad = Vector::new(radius, radius, radius);
        let mut capsule_bounds = Aabb::EMPTY;
        capsule_bounds.include((segment_bottom - pad).into());
        capsule_bounds.include((segment_bottom + pad).into());
        capsule_bounds.include((segment_top - pad).into());
        capsule_bounds.include((segment_top + pad).into());
        if !self.bounds.overlaps(&capsule_bounds) {
            return None;
        }

        let feet = segment_bottom - up * radius;
        let height = (segment_top - segment_bottom).dot(&up) + 2.0 * radius;

        let mut ground: Option<Ground> = None;
        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.dot(&up) > WALKABLE_NORMAL_THRESHOLD {
                let Some(distance) = triangle::ground_distance_at(corners, feet, up) else {
                    continue;
                };
                if distance < -GROUND_BELOW_TOLERANCE || distance > height * GROUND_OVERLAP_RATIO {
                    continue;
                }
                let supported = (motion == Motion::Falling && distance > 0.0)
                    || (-GROUND_BELOW_TOLERANCE..=GROUND_REST_TOLERANCE).contains(&distance);
                if supported && ground.is_none_or(|best| distance > best.distance) {
                    ground = Some(Ground { distance, normal });
                }
            } else if let Some(correction) =
                triangle::capsule_push(segment_bottom, segment_top, radius, corners)
            {
                push += correction;
            }
        }

        if ground.is_none() && push == Vector::new(0.0, 0.0, 0.0) {
            return None;
        }

        Some(TriangleHit { ground, 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
}