collide-mesh 0.2.0

use ga3::Vector;
use inner_space::{DotProduct, InnerSpace};

const DEGENERATE_AREA: f32 = 1e-12;
const DEGENERATE_DENOMINATOR: f32 = 1e-10;
const BARYCENTRIC_TOLERANCE: f32 = 1e-4;
const PARALLEL_DETERMINANT: f32 = 1e-8;
const MINIMUM_RAY_DISTANCE: f32 = 1e-4;

pub fn face_normal(triangle: &[Vector<f32>; 3]) -> Option<Vector<f32>> {
    let [a, b, c] = *triangle;
    let normal = (b - a).cross(c - a);
    let area2 = normal.magnitude2();
    (area2 > DEGENERATE_AREA).then(|| normal / area2.sqrt())
}

pub fn ground_distance_at(
    triangle: &[Vector<f32>; 3],
    position: Vector<f32>,
    up: Vector<f32>,
) -> Option<f32> {
    let [a, b, c] = *triangle;
    let edge_ab = b - a;
    let edge_ac = c - a;

    let perpendicular = up.cross(edge_ac);
    let determinant = edge_ab.dot(&perpendicular);
    if determinant.abs() < DEGENERATE_DENOMINATOR {
        return None;
    }
    let inverse = 1.0 / determinant;

    let from_a = position - a;
    let u = from_a.dot(&perpendicular) * inverse;
    let cross_ab = from_a.cross(edge_ab);
    let v = up.dot(&cross_ab) * inverse;

    let inside = u >= -BARYCENTRIC_TOLERANCE
        && v >= -BARYCENTRIC_TOLERANCE
        && u + v <= 1.0 + BARYCENTRIC_TOLERANCE;
    inside.then(|| edge_ac.dot(&cross_ab) * inverse)
}

pub fn capsule_push(
    segment_bottom: Vector<f32>,
    segment_top: Vector<f32>,
    radius: f32,
    triangle: &[Vector<f32>; 3],
) -> Option<Vector<f32>> {
    let on_triangle = closest_point(triangle, segment_bottom);
    let on_segment = closest_point_on_segment(segment_bottom, segment_top, on_triangle);
    let on_triangle = closest_point(triangle, on_segment);

    let offset = on_segment - on_triangle;
    let distance2 = offset.magnitude2();
    if distance2 >= radius * radius || distance2 < DEGENERATE_DENOMINATOR {
        return None;
    }

    let distance = distance2.sqrt();
    Some(offset * ((radius - distance) / distance))
}

pub fn ray_intersection(
    origin: Vector<f32>,
    direction: Vector<f32>,
    triangle: &[Vector<f32>; 3],
) -> Option<f32> {
    let [a, b, c] = *triangle;
    let edge_ab = b - a;
    let edge_ac = c - a;

    let perpendicular = direction.cross(edge_ac);
    let determinant = edge_ab.dot(&perpendicular);
    if determinant.abs() < PARALLEL_DETERMINANT {
        return None;
    }
    let inverse = 1.0 / determinant;

    let from_a = origin - a;
    let u = from_a.dot(&perpendicular) * inverse;
    if !(0.0..=1.0).contains(&u) {
        return None;
    }

    let cross_ab = from_a.cross(edge_ab);
    let v = direction.dot(&cross_ab) * inverse;
    if v < 0.0 || u + v > 1.0 {
        return None;
    }

    let distance = edge_ac.dot(&cross_ab) * inverse;
    (distance > MINIMUM_RAY_DISTANCE).then_some(distance)
}

fn closest_point(triangle: &[Vector<f32>; 3], point: Vector<f32>) -> Vector<f32> {
    let [a, b, c] = *triangle;
    let ab = b - a;
    let ac = c - a;

    let to_point_a = point - a;
    let ab_a = ab.dot(&to_point_a);
    let ac_a = ac.dot(&to_point_a);
    if ab_a <= 0.0 && ac_a <= 0.0 {
        return a;
    }

    let to_point_b = point - b;
    let ab_b = ab.dot(&to_point_b);
    let ac_b = ac.dot(&to_point_b);
    if ab_b >= 0.0 && ac_b <= ab_b {
        return b;
    }

    let region_ab = ab_a * ac_b - ab_b * ac_a;
    if region_ab <= 0.0 && ab_a >= 0.0 && ab_b <= 0.0 {
        return a + ab * (ab_a / (ab_a - ab_b));
    }

    let to_point_c = point - c;
    let ab_c = ab.dot(&to_point_c);
    let ac_c = ac.dot(&to_point_c);
    if ac_c >= 0.0 && ab_c <= ac_c {
        return c;
    }

    let region_ac = ab_c * ac_a - ab_a * ac_c;
    if region_ac <= 0.0 && ac_a >= 0.0 && ac_c <= 0.0 {
        return a + ac * (ac_a / (ac_a - ac_c));
    }

    let region_bc = ab_b * ac_c - ab_c * ac_b;
    if region_bc <= 0.0 && ac_b - ab_b >= 0.0 && ab_c - ac_c >= 0.0 {
        return b + (c - b) * ((ac_b - ab_b) / ((ac_b - ab_b) + (ab_c - ac_c)));
    }

    let inverse = 1.0 / (region_bc + region_ac + region_ab);
    a + ab * (region_ac * inverse) + ac * (region_ab * inverse)
}

fn closest_point_on_segment(
    start: Vector<f32>,
    end: Vector<f32>,
    point: Vector<f32>,
) -> Vector<f32> {
    let segment = end - start;
    let length2 = segment.magnitude2();
    if length2 < DEGENERATE_DENOMINATOR {
        return start;
    }
    let parameter = ((point - start).dot(&segment) / length2).clamp(0.0, 1.0);
    start + segment * parameter
}