collide-mesh 0.1.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_height_at(triangle: &[Vector<f32>; 3], x: f32, z: f32) -> Option<f32> {
    let [a, b, c] = *triangle;

    let edge_ab = [b.x - a.x, b.z - a.z];
    let edge_ac = [c.x - a.x, c.z - a.z];
    let to_point = [x - a.x, z - a.z];

    let ab_ab = dot2(edge_ab, edge_ab);
    let ab_ac = dot2(edge_ab, edge_ac);
    let ab_point = dot2(edge_ab, to_point);
    let ac_ac = dot2(edge_ac, edge_ac);
    let ac_point = dot2(edge_ac, to_point);

    let denominator = ab_ab * ac_ac - ab_ac * ab_ac;
    if denominator.abs() < DEGENERATE_DENOMINATOR {
        return None;
    }
    let inverse = 1.0 / denominator;

    let u = (ac_ac * ab_point - ab_ac * ac_point) * inverse;
    let v = (ab_ab * ac_point - ab_ac * ab_point) * inverse;

    let inside = u >= -BARYCENTRIC_TOLERANCE
        && v >= -BARYCENTRIC_TOLERANCE
        && u + v <= 1.0 + BARYCENTRIC_TOLERANCE;
    inside.then_some(a.y + u * (b.y - a.y) + v * (c.y - a.y))
}

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
}

fn dot2(a: [f32; 2], b: [f32; 2]) -> f32 {
    a[0] * b[0] + a[1] * b[1]
}