rustsim-geometry 0.0.1

//! 3-D triangles with closest-point and Möller–Trumbore ray intersection.

use crate::ray::Ray3;
use crate::vec3::{self, Vec3};

/// 3-D triangle with three vertices.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Triangle3 {
    /// First vertex.
    pub a: Vec3,
    /// Second vertex.
    pub b: Vec3,
    /// Third vertex.
    pub c: Vec3,
}

impl Triangle3 {
    /// (Non-unit) normal via the right-hand rule `(b-a) × (c-a)`.
    #[inline]
    pub fn raw_normal(&self) -> Vec3 {
        vec3::cross(vec3::sub(self.b, self.a), vec3::sub(self.c, self.a))
    }

    /// Unit normal. Returns `[0, 0, 0]` for degenerate triangles.
    #[inline]
    pub fn normal(&self) -> Vec3 {
        vec3::normalize(self.raw_normal())
    }

    /// Möller–Trumbore ray-triangle intersection. Returns non-negative `t`.
    pub fn intersect_ray(&self, ray: &Ray3) -> Option<f64> {
        let eps = 1e-12;
        let e1 = vec3::sub(self.b, self.a);
        let e2 = vec3::sub(self.c, self.a);
        let h = vec3::cross(ray.dir, e2);
        let det = vec3::dot(e1, h);
        if det.abs() < eps {
            return None;
        }
        let inv_det = 1.0 / det;
        let s = vec3::sub(ray.origin, self.a);
        let u = vec3::dot(s, h) * inv_det;
        if !(0.0..=1.0).contains(&u) {
            return None;
        }
        let q = vec3::cross(s, e1);
        let v = vec3::dot(ray.dir, q) * inv_det;
        if v < 0.0 || u + v > 1.0 {
            return None;
        }
        let t = vec3::dot(e2, q) * inv_det;
        if t >= 0.0 {
            Some(t)
        } else {
            None
        }
    }

    /// Closest point on the triangle to `p` (Ericson, *Real-Time Collision
    /// Detection*, §5.1.5).
    pub fn closest_point(&self, p: Vec3) -> Vec3 {
        let ab = vec3::sub(self.b, self.a);
        let ac = vec3::sub(self.c, self.a);
        let ap = vec3::sub(p, self.a);

        let d1 = vec3::dot(ab, ap);
        let d2 = vec3::dot(ac, ap);
        if d1 <= 0.0 && d2 <= 0.0 {
            return self.a;
        }

        let bp = vec3::sub(p, self.b);
        let d3 = vec3::dot(ab, bp);
        let d4 = vec3::dot(ac, bp);
        if d3 >= 0.0 && d4 <= d3 {
            return self.b;
        }

        let vc = d1 * d4 - d3 * d2;
        if vc <= 0.0 && d1 >= 0.0 && d3 <= 0.0 {
            let v = d1 / (d1 - d3);
            return vec3::add(self.a, vec3::scale(ab, v));
        }

        let cp = vec3::sub(p, self.c);
        let d5 = vec3::dot(ab, cp);
        let d6 = vec3::dot(ac, cp);
        if d6 >= 0.0 && d5 <= d6 {
            return self.c;
        }

        let vb = d5 * d2 - d1 * d6;
        if vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0 {
            let w = d2 / (d2 - d6);
            return vec3::add(self.a, vec3::scale(ac, w));
        }

        let va = d3 * d6 - d5 * d4;
        if va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0 {
            let w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
            return vec3::add(self.b, vec3::scale(vec3::sub(self.c, self.b), w));
        }

        let denom = 1.0 / (va + vb + vc);
        let v = vb * denom;
        let w = vc * denom;
        vec3::add(self.a, vec3::add(vec3::scale(ab, v), vec3::scale(ac, w)))
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    fn tri() -> Triangle3 {
        Triangle3 {
            a: [0.0, 0.0, 0.0],
            b: [1.0, 0.0, 0.0],
            c: [0.0, 1.0, 0.0],
        }
    }

    #[test]
    fn normal_of_xy_triangle_is_plus_z() {
        let n = tri().normal();
        assert!((n[2] - 1.0).abs() < 1e-12);
    }

    #[test]
    fn ray_through_center_hits() {
        let t = tri();
        let r = Ray3 {
            origin: [0.2, 0.2, 5.0],
            dir: [0.0, 0.0, -1.0],
        };
        let hit = t.intersect_ray(&r).unwrap();
        assert!((hit - 5.0).abs() < 1e-12);
    }

    #[test]
    fn ray_missing_triangle_returns_none() {
        let t = tri();
        let r = Ray3 {
            origin: [5.0, 5.0, 5.0],
            dir: [0.0, 0.0, -1.0],
        };
        assert!(t.intersect_ray(&r).is_none());
    }

    #[test]
    fn closest_point_to_vertex_and_edge() {
        let t = tri();
        assert_eq!(t.closest_point([-1.0, -1.0, 0.0]), [0.0, 0.0, 0.0]);
        let cp = t.closest_point([0.5, -1.0, 0.0]);
        assert!((cp[0] - 0.5).abs() < 1e-9);
        assert!(cp[1].abs() < 1e-9);
    }
}