Skip to main content

mathverse_geometry/
mesh3d.rs

1//! 3D extensions: triangle mesh, AABB tree, ray-triangle (Möller-Trumbore), plane-plane intersection, line-solid intersections.
2
3use super::shapes3d::{Line3, Plane, Point3};
4use super::spatial::AABB3;
5
6// ---------------------------------------------------------------------------
7// Triangle3
8// ---------------------------------------------------------------------------
9
10/// 3D triangle defined by three vertices.
11#[derive(Debug, Clone, Copy, PartialEq)]
12pub struct Triangle3 {
13    pub a: Point3,
14    pub b: Point3,
15    pub c: Point3,
16}
17
18impl Triangle3 {
19    pub fn new(a: Point3, b: Point3, c: Point3) -> Self {
20        Self { a, b, c }
21    }
22    pub fn normal(self) -> Point3 {
23        let ab = Point3::new(self.b.x - self.a.x, self.b.y - self.a.y, self.b.z - self.a.z);
24        let ac = Point3::new(self.c.x - self.a.x, self.c.y - self.a.y, self.c.z - self.a.z);
25        cross(ab, ac)
26    }
27    pub fn area(self) -> f64 {
28        norm(self.normal()) / 2.0
29    }
30    pub fn centroid(self) -> Point3 {
31        Point3::new(
32            (self.a.x + self.b.x + self.c.x) / 3.0,
33            (self.a.y + self.b.y + self.c.y) / 3.0,
34            (self.a.z + self.b.z + self.c.z) / 3.0,
35        )
36    }
37}
38
39// ---------------------------------------------------------------------------
40// TriangleMesh
41// ---------------------------------------------------------------------------
42
43/// Simple triangle mesh (indexed).
44#[derive(Debug, Clone)]
45pub struct TriangleMesh {
46    pub vertices: Vec<Point3>,
47    pub indices: Vec<(usize, usize, usize)>,
48}
49
50impl TriangleMesh {
51    pub fn new(vertices: Vec<Point3>, indices: Vec<(usize, usize, usize)>) -> Self {
52        Self { vertices, indices }
53    }
54    pub fn triangle(&self, i: usize) -> Triangle3 {
55        let (a, b, c) = self.indices[i];
56        Triangle3::new(self.vertices[a], self.vertices[b], self.vertices[c])
57    }
58    pub fn num_triangles(&self) -> usize {
59        self.indices.len()
60    }
61    pub fn aabb(&self) -> Option<AABB3> {
62        AABB3::from_points(&self.vertices)
63    }
64    /// Compute normals for each triangle.
65    pub fn normals(&self) -> Vec<Point3> {
66        (0..self.indices.len())
67            .map(|i| self.triangle(i).normal())
68            .collect()
69    }
70}
71
72// ---------------------------------------------------------------------------
73// AABB Tree
74// ---------------------------------------------------------------------------
75
76/// AABB tree node for fast ray intersection.
77pub struct AABBTreeNode {
78    pub bounds: AABB3,
79    pub triangle_index: Option<usize>,
80    pub left: Option<Box<AABBTreeNode>>,
81    pub right: Option<Box<AABBTreeNode>>,
82}
83
84impl AABBTreeNode {
85    pub fn leaf(bounds: AABB3, triangle_index: usize) -> Self {
86        Self {
87            bounds,
88            triangle_index: Some(triangle_index),
89            left: None,
90            right: None,
91        }
92    }
93    pub fn internal(bounds: AABB3, left: AABBTreeNode, right: AABBTreeNode) -> Self {
94        Self {
95            bounds,
96            triangle_index: None,
97            left: Some(Box::new(left)),
98            right: Some(Box::new(right)),
99        }
100    }
101    /// Find the closest triangle hit by a ray. Returns (triangle_index, t).
102    pub fn ray_intersect(&self, mesh: &TriangleMesh, origin: Point3, dir: Point3) -> Option<(usize, f64)> {
103        if !aabb_ray_intersect(self.bounds, origin, dir) {
104            return None;
105        }
106        if let Some(idx) = self.triangle_index {
107            let tri = mesh.triangle(idx);
108            if let Some(t) = ray_triangle_intersect(origin, dir, tri) {
109                return Some((idx, t));
110            }
111            return None;
112        }
113        let mut closest: Option<(usize, f64)> = None;
114        if let Some(ref left) = self.left {
115            if let Some(hit) = left.ray_intersect(mesh, origin, dir) {
116                closest = Some(hit);
117            }
118        }
119        if let Some(ref right) = self.right {
120            if let Some(hit) = right.ray_intersect(mesh, origin, dir) {
121                if closest.is_none() || hit.1 < closest.unwrap().1 {
122                    closest = Some(hit);
123                }
124            }
125        }
126        closest
127    }
128}
129
130/// Build a simple AABB tree from a mesh (median split on longest axis).
131pub fn build_aabb_tree(mesh: &TriangleMesh) -> Option<AABBTreeNode> {
132    if mesh.indices.is_empty() {
133        return None;
134    }
135    let mut tris: Vec<(usize, AABB3)> = (0..mesh.indices.len())
136        .map(|i| {
137            let tri = mesh.triangle(i);
138            let pts = [tri.a, tri.b, tri.c];
139            (i, AABB3::from_points(&pts).unwrap())
140        })
141        .collect();
142    build_tree_recursive(&mut tris)
143}
144
145fn build_tree_recursive(tris: &mut [(usize, AABB3)]) -> Option<AABBTreeNode> {
146    if tris.is_empty() {
147        return None;
148    }
149    if tris.len() == 1 {
150        let (idx, bounds) = tris[0];
151        return Some(AABBTreeNode::leaf(bounds, idx));
152    }
153    // Merge all bounds
154    let bounds = tris.iter().map(|(_, b)| *b).reduce(|a, b| a.merge(b)).unwrap();
155    // Split on longest axis
156    let ext = [
157        bounds.max.x - bounds.min.x,
158        bounds.max.y - bounds.min.y,
159        bounds.max.z - bounds.min.z,
160    ];
161    let axis = if ext[0] >= ext[1] && ext[0] >= ext[2] { 0 }
162        else if ext[1] >= ext[2] { 1 } else { 2 };
163    tris.sort_by(|a, b| {
164        let ca = match axis {
165            0 => (a.1.min.x + a.1.max.x) / 2.0,
166            1 => (a.1.min.y + a.1.max.y) / 2.0,
167            _ => (a.1.min.z + a.1.max.z) / 2.0,
168        };
169        let cb = match axis {
170            0 => (b.1.min.x + b.1.max.x) / 2.0,
171            1 => (b.1.min.y + b.1.max.y) / 2.0,
172            _ => (b.1.min.z + b.1.max.z) / 2.0,
173        };
174        ca.partial_cmp(&cb).unwrap()
175    });
176    let mid = tris.len() / 2;
177    let right = build_tree_recursive(&mut tris[mid..])?;
178    let left = build_tree_recursive(&mut tris[..mid])?;
179    Some(AABBTreeNode::internal(bounds, left, right))
180}
181
182// ---------------------------------------------------------------------------
183// Ray-triangle intersection (Möller-Trumbore)
184// ---------------------------------------------------------------------------
185
186/// Möller-Trumbore ray-triangle intersection. Returns `t` if hit (t > 0).
187///
188/// The test is **two-sided**: rays intersect the triangle from either side
189/// (no back-face culling), which is the expected behavior for volume
190/// rendering and collision queries. Callers that need culling should check
191/// `dot(dir, tri.normal())` themselves before trusting the hit.
192pub fn ray_triangle_intersect(origin: Point3, dir: Point3, tri: Triangle3) -> Option<f64> {
193    let eps = 1e-7;
194    let ab = Point3::new(tri.b.x - tri.a.x, tri.b.y - tri.a.y, tri.b.z - tri.a.z);
195    let ac = Point3::new(tri.c.x - tri.a.x, tri.c.y - tri.a.y, tri.c.z - tri.a.z);
196    let h = cross(dir, ac);
197    let a = dot(ab, h);
198    if a > -eps && a < eps {
199        return None;
200    }
201    let f = 1.0 / a;
202    let s = Point3::new(origin.x - tri.a.x, origin.y - tri.a.y, origin.z - tri.a.z);
203    let u = f * dot(s, h);
204    if u < 0.0 || u > 1.0 {
205        return None;
206    }
207    let q = cross(s, ab);
208    let v = f * dot(dir, q);
209    if v < 0.0 || u + v > 1.0 {
210        return None;
211    }
212    let t = f * dot(ac, q);
213    if t > eps {
214        Some(t)
215    } else {
216        None
217    }
218}
219
220/// AABB-ray intersection (Slab method). Returns `t_min` if hit.
221pub fn aabb_ray_intersect(bb: AABB3, origin: Point3, dir: Point3) -> bool {
222    let mut tmin = f64::NEG_INFINITY;
223    let mut tmax = f64::INFINITY;
224    let orig = [origin.x, origin.y, origin.z];
225    let dir = [dir.x, dir.y, dir.z];
226    let min = [bb.min.x, bb.min.y, bb.min.z];
227    let max = [bb.max.x, bb.max.y, bb.max.z];
228    for i in 0..3 {
229        if dir[i].abs() < 1e-30 {
230            if orig[i] < min[i] || orig[i] > max[i] {
231                return false;
232            }
233        } else {
234            let inv = 1.0 / dir[i];
235            let mut t1 = (min[i] - orig[i]) * inv;
236            let mut t2 = (max[i] - orig[i]) * inv;
237            if t1 > t2 {
238                std::mem::swap(&mut t1, &mut t2);
239            }
240            if t1 > tmin { tmin = t1; }
241            if t2 < tmax { tmax = t2; }
242            if tmin > tmax {
243                return false;
244            }
245        }
246    }
247    tmax >= 0.0
248}
249
250// ---------------------------------------------------------------------------
251// Plane-plane intersection
252// ---------------------------------------------------------------------------
253
254/// Intersection of two planes: returns a line if they intersect, or None if parallel.
255pub fn plane_plane_intersection(p1: Plane, p2: Plane) -> Option<Line3> {
256    let n1 = p1.normal;
257    let n2 = p2.normal;
258    let dir = cross(n1, n2);
259    if norm(dir) < 1e-10 {
260        return None; // parallel
261    }
262    // Find a point on the intersection line
263    // Solve: n1·p + d1 = 0, n2·p + d2 = 0, set one coord to 0
264    let a = [
265        [n1.y, n1.z, -p1.d],
266        [n2.y, n2.z, -p2.d],
267    ];
268    let det = a[0][0] * a[1][1] - a[0][1] * a[1][0];
269    if det.abs() > 1e-10 {
270        let y = (a[0][2] * a[1][1] - a[0][1] * a[1][2]) / det;
271        let z = (a[0][0] * a[1][2] - a[0][2] * a[1][0]) / det;
272        return Some(Line3::new(Point3::new(0.0, y, z), dir));
273    }
274    // Try setting y = 0
275    let a = [
276        [n1.x, n1.z, -p1.d],
277        [n2.x, n2.z, -p2.d],
278    ];
279    let det = a[0][0] * a[1][1] - a[0][1] * a[1][0];
280    if det.abs() > 1e-10 {
281        let x = (a[0][2] * a[1][1] - a[0][1] * a[1][2]) / det;
282        let z = (a[0][0] * a[1][2] - a[0][2] * a[1][0]) / det;
283        return Some(Line3::new(Point3::new(x, 0.0, z), dir));
284    }
285    // Try setting z = 0
286    let a = [
287        [n1.x, n1.y, -p1.d],
288        [n2.x, n2.y, -p2.d],
289    ];
290    let det = a[0][0] * a[1][1] - a[0][1] * a[1][0];
291    if det.abs() > 1e-10 {
292        let x = (a[0][2] * a[1][1] - a[0][1] * a[1][2]) / det;
293        let y = (a[0][0] * a[1][2] - a[0][2] * a[1][0]) / det;
294        return Some(Line3::new(Point3::new(x, y, 0.0), dir));
295    }
296    None
297}
298
299// ---------------------------------------------------------------------------
300// Line-solid intersections
301// ---------------------------------------------------------------------------
302
303/// Ray-sphere intersection. Returns closest `t > 0` or None.
304pub fn ray_sphere_intersect(origin: Point3, dir: Point3, center: Point3, radius: f64) -> Option<f64> {
305    let oc = Point3::new(origin.x - center.x, origin.y - center.y, origin.z - center.z);
306    let a = dot(dir, dir);
307    let b = 2.0 * dot(oc, dir);
308    let c = dot(oc, oc) - radius * radius;
309    let disc = b * b - 4.0 * a * c;
310    if disc < 0.0 {
311        return None;
312    }
313    let sqrt_disc = disc.sqrt();
314    let t1 = (-b - sqrt_disc) / (2.0 * a);
315    let t2 = (-b + sqrt_disc) / (2.0 * a);
316    if t1 > 1e-7 {
317        Some(t1)
318    } else if t2 > 1e-7 {
319        Some(t2)
320    } else {
321        None
322    }
323}
324
325/// Ray-AABB intersection. Returns closest `t > 0` or None.
326pub fn ray_aabb_intersect(origin: Point3, dir: Point3, bb: AABB3) -> Option<f64> {
327    let mut tmin = f64::NEG_INFINITY;
328    let mut tmax = f64::INFINITY;
329    let orig = [origin.x, origin.y, origin.z];
330    let dir_arr = [dir.x, dir.y, dir.z];
331    let min = [bb.min.x, bb.min.y, bb.min.z];
332    let max = [bb.max.x, bb.max.y, bb.max.z];
333    for i in 0..3 {
334        if dir_arr[i].abs() < 1e-30 {
335            if orig[i] < min[i] || orig[i] > max[i] {
336                return None;
337            }
338        } else {
339            let inv = 1.0 / dir_arr[i];
340            let mut t1 = (min[i] - orig[i]) * inv;
341            let mut t2 = (max[i] - orig[i]) * inv;
342            if t1 > t2 {
343                std::mem::swap(&mut t1, &mut t2);
344            }
345            if t1 > tmin { tmin = t1; }
346            if t2 < tmax { tmax = t2; }
347            if tmin > tmax {
348                return None;
349            }
350        }
351    }
352    if tmax < 1e-7 {
353        return None;
354    }
355    Some(if tmin > 1e-7 { tmin } else { tmax })
356}
357
358/// Ray-cylinder intersection (infinite cylinder along `axis` through `center`).
359pub fn ray_cylinder_intersect(origin: Point3, dir: Point3, center: Point3, axis: Point3, radius: f64) -> Option<f64> {
360    let oc = Point3::new(origin.x - center.x, origin.y - center.y, origin.z - center.z);
361    let d_dot_ax = dot(dir, axis);
362    let oc_dot_ax = dot(oc, axis);
363    let proj_d = Point3::new(dir.x - d_dot_ax * axis.x, dir.y - d_dot_ax * axis.y, dir.z - d_dot_ax * axis.z);
364    let proj_oc = Point3::new(oc.x - oc_dot_ax * axis.x, oc.y - oc_dot_ax * axis.y, oc.z - oc_dot_ax * axis.z);
365    let a = dot(proj_d, proj_d);
366    let b = 2.0 * dot(proj_oc, proj_d);
367    let c = dot(proj_oc, proj_oc) - radius * radius;
368    let disc = b * b - 4.0 * a * c;
369    if disc < 0.0 {
370        return None;
371    }
372    let sqrt_disc = disc.sqrt();
373    let t1 = (-b - sqrt_disc) / (2.0 * a);
374    let t2 = (-b + sqrt_disc) / (2.0 * a);
375    if t1 > 1e-7 {
376        Some(t1)
377    } else if t2 > 1e-7 {
378        Some(t2)
379    } else {
380        None
381    }
382}
383
384// ---------------------------------------------------------------------------
385// Helpers
386// ---------------------------------------------------------------------------
387
388fn dot(a: Point3, b: Point3) -> f64 {
389    a.x * b.x + a.y * b.y + a.z * b.z
390}
391
392fn cross(a: Point3, b: Point3) -> Point3 {
393    Point3::new(
394        a.y * b.z - a.z * b.y,
395        a.z * b.x - a.x * b.z,
396        a.x * b.y - a.y * b.x,
397    )
398}
399
400fn norm(v: Point3) -> f64 {
401    dot(v, v).sqrt()
402}
403
404#[cfg(test)]
405mod tests {
406    use super::*;
407    use super::super::shapes3d::{Point3, Plane};
408
409    fn pt3(x: f64, y: f64, z: f64) -> Point3 { Point3::new(x, y, z) }
410
411    #[test]
412    fn triangle_normal() {
413        let tri = Triangle3::new(pt3(0.0, 0.0, 0.0), pt3(1.0, 0.0, 0.0), pt3(0.0, 1.0, 0.0));
414        let n = tri.normal();
415        assert!((n.z - 1.0).abs() < 1e-12);
416    }
417
418    #[test]
419    fn ray_triangle_hit() {
420        // Triangle in the xy-plane at z=0
421        let tri = Triangle3::new(pt3(-1.0, -1.0, 0.0), pt3(1.0, -1.0, 0.0), pt3(0.0, 1.0, 0.0));
422        let t = ray_triangle_intersect(pt3(0.0, 0.0, -5.0), pt3(0.0, 0.0, 1.0), tri);
423        assert!(t.is_some());
424        assert!((t.unwrap() - 5.0).abs() < 1e-6);
425    }
426
427    #[test]
428    fn ray_triangle_miss() {
429        let tri = Triangle3::new(pt3(-1.0, -1.0, 0.0), pt3(1.0, -1.0, 0.0), pt3(0.0, 1.0, 0.0));
430        let t = ray_triangle_intersect(pt3(5.0, 5.0, -5.0), pt3(0.0, 0.0, 1.0), tri);
431        assert!(t.is_none());
432    }
433
434    #[test]
435    fn ray_triangle_two_sided() {
436        // Hit from behind (opposite face) still registers — two-sided test.
437        let tri = Triangle3::new(pt3(-1.0, -1.0, 0.0), pt3(1.0, -1.0, 0.0), pt3(0.0, 1.0, 0.0));
438        let t = ray_triangle_intersect(pt3(0.0, 0.0, 5.0), pt3(0.0, 0.0, -1.0), tri);
439        assert!(t.is_some());
440        assert!((t.unwrap() - 5.0).abs() < 1e-6);
441    }
442
443    #[test]
444    fn ray_triangle_edge_on_parallel() {
445        // Ray parallel to the triangle plane → no hit.
446        let tri = Triangle3::new(pt3(-1.0, -1.0, 0.0), pt3(1.0, -1.0, 0.0), pt3(0.0, 1.0, 0.0));
447        let t = ray_triangle_intersect(pt3(0.0, 0.0, 5.0), pt3(1.0, 0.0, 0.0), tri);
448        assert!(t.is_none());
449    }
450
451    #[test]
452    fn ray_sphere_hit() {
453        let t = ray_sphere_intersect(pt3(0.0, 0.0, -5.0), pt3(0.0, 0.0, 1.0), pt3(0.0, 0.0, 0.0), 1.0);
454        assert!(t.is_some());
455        assert!((t.unwrap() - 4.0).abs() < 1e-6);
456    }
457
458    #[test]
459    fn ray_sphere_miss() {
460        let t = ray_sphere_intersect(pt3(5.0, 5.0, -5.0), pt3(0.0, 0.0, 1.0), pt3(0.0, 0.0, 0.0), 1.0);
461        assert!(t.is_none());
462    }
463
464    #[test]
465    fn plane_plane_intersect() {
466        let p1 = Plane::new(pt3(0.0, 0.0, 1.0), 0.0); // z = 0
467        let p2 = Plane::new(pt3(0.0, 1.0, 0.0), 0.0); // y = 0
468        let line = plane_plane_intersection(p1, p2).unwrap();
469        // Intersection is the x-axis
470        assert!((line.dir.x.abs() - 1.0).abs() < 1e-6);
471    }
472
473    #[test]
474    fn aabb_tree_build() {
475        let mesh = TriangleMesh::new(
476            vec![pt3(0.0, 0.0, 0.0), pt3(1.0, 0.0, 0.0), pt3(0.0, 1.0, 0.0), pt3(0.0, 0.0, 1.0)],
477            vec![(0, 1, 2), (0, 1, 3)],
478        );
479        let tree = build_aabb_tree(&mesh);
480        assert!(tree.is_some());
481    }
482}