rustsim_geometry/
triangle.rs

1//! 3-D triangles with closest-point and Möller–Trumbore ray intersection.
2
3use crate::ray::Ray3;
4use crate::vec3::{self, Vec3};
5
6/// 3-D triangle with three vertices.
7#[derive(Debug, Clone, Copy, PartialEq)]
8pub struct Triangle3 {
9    /// First vertex.
10    pub a: Vec3,
11    /// Second vertex.
12    pub b: Vec3,
13    /// Third vertex.
14    pub c: Vec3,
15}
16
17impl Triangle3 {
18    /// (Non-unit) normal via the right-hand rule `(b-a) × (c-a)`.
19    #[inline]
20    pub fn raw_normal(&self) -> Vec3 {
21        vec3::cross(vec3::sub(self.b, self.a), vec3::sub(self.c, self.a))
22    }
23
24    /// Unit normal. Returns `[0, 0, 0]` for degenerate triangles.
25    #[inline]
26    pub fn normal(&self) -> Vec3 {
27        vec3::normalize(self.raw_normal())
28    }
29
30    /// Möller–Trumbore ray-triangle intersection. Returns non-negative `t`.
31    pub fn intersect_ray(&self, ray: &Ray3) -> Option<f64> {
32        let eps = 1e-12;
33        let e1 = vec3::sub(self.b, self.a);
34        let e2 = vec3::sub(self.c, self.a);
35        let h = vec3::cross(ray.dir, e2);
36        let det = vec3::dot(e1, h);
37        if det.abs() < eps {
38            return None;
39        }
40        let inv_det = 1.0 / det;
41        let s = vec3::sub(ray.origin, self.a);
42        let u = vec3::dot(s, h) * inv_det;
43        if !(0.0..=1.0).contains(&u) {
44            return None;
45        }
46        let q = vec3::cross(s, e1);
47        let v = vec3::dot(ray.dir, q) * inv_det;
48        if v < 0.0 || u + v > 1.0 {
49            return None;
50        }
51        let t = vec3::dot(e2, q) * inv_det;
52        if t >= 0.0 {
53            Some(t)
54        } else {
55            None
56        }
57    }
58
59    /// Closest point on the triangle to `p` (Ericson, *Real-Time Collision
60    /// Detection*, §5.1.5).
61    pub fn closest_point(&self, p: Vec3) -> Vec3 {
62        let ab = vec3::sub(self.b, self.a);
63        let ac = vec3::sub(self.c, self.a);
64        let ap = vec3::sub(p, self.a);
65
66        let d1 = vec3::dot(ab, ap);
67        let d2 = vec3::dot(ac, ap);
68        if d1 <= 0.0 && d2 <= 0.0 {
69            return self.a;
70        }
71
72        let bp = vec3::sub(p, self.b);
73        let d3 = vec3::dot(ab, bp);
74        let d4 = vec3::dot(ac, bp);
75        if d3 >= 0.0 && d4 <= d3 {
76            return self.b;
77        }
78
79        let vc = d1 * d4 - d3 * d2;
80        if vc <= 0.0 && d1 >= 0.0 && d3 <= 0.0 {
81            let v = d1 / (d1 - d3);
82            return vec3::add(self.a, vec3::scale(ab, v));
83        }
84
85        let cp = vec3::sub(p, self.c);
86        let d5 = vec3::dot(ab, cp);
87        let d6 = vec3::dot(ac, cp);
88        if d6 >= 0.0 && d5 <= d6 {
89            return self.c;
90        }
91
92        let vb = d5 * d2 - d1 * d6;
93        if vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0 {
94            let w = d2 / (d2 - d6);
95            return vec3::add(self.a, vec3::scale(ac, w));
96        }
97
98        let va = d3 * d6 - d5 * d4;
99        if va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0 {
100            let w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
101            return vec3::add(self.b, vec3::scale(vec3::sub(self.c, self.b), w));
102        }
103
104        let denom = 1.0 / (va + vb + vc);
105        let v = vb * denom;
106        let w = vc * denom;
107        vec3::add(self.a, vec3::add(vec3::scale(ab, v), vec3::scale(ac, w)))
108    }
109}
110
111#[cfg(test)]
112mod tests {
113    use super::*;
114
115    fn tri() -> Triangle3 {
116        Triangle3 {
117            a: [0.0, 0.0, 0.0],
118            b: [1.0, 0.0, 0.0],
119            c: [0.0, 1.0, 0.0],
120        }
121    }
122
123    #[test]
124    fn normal_of_xy_triangle_is_plus_z() {
125        let n = tri().normal();
126        assert!((n[2] - 1.0).abs() < 1e-12);
127    }
128
129    #[test]
130    fn ray_through_center_hits() {
131        let t = tri();
132        let r = Ray3 {
133            origin: [0.2, 0.2, 5.0],
134            dir: [0.0, 0.0, -1.0],
135        };
136        let hit = t.intersect_ray(&r).unwrap();
137        assert!((hit - 5.0).abs() < 1e-12);
138    }
139
140    #[test]
141    fn ray_missing_triangle_returns_none() {
142        let t = tri();
143        let r = Ray3 {
144            origin: [5.0, 5.0, 5.0],
145            dir: [0.0, 0.0, -1.0],
146        };
147        assert!(t.intersect_ray(&r).is_none());
148    }
149
150    #[test]
151    fn closest_point_to_vertex_and_edge() {
152        let t = tri();
153        assert_eq!(t.closest_point([-1.0, -1.0, 0.0]), [0.0, 0.0, 0.0]);
154        let cp = t.closest_point([0.5, -1.0, 0.0]);
155        assert!((cp[0] - 0.5).abs() < 1e-9);
156        assert!(cp[1].abs() < 1e-9);
157    }
158}
rustsim_geometry/triangle.rs

rustsim_geometry/
triangle.rs
