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).
187pub fn ray_triangle_intersect(origin: Point3, dir: Point3, tri: Triangle3) -> Option<f64> {
188    let eps = 1e-7;
189    let ab = Point3::new(tri.b.x - tri.a.x, tri.b.y - tri.a.y, tri.b.z - tri.a.z);
190    let ac = Point3::new(tri.c.x - tri.a.x, tri.c.y - tri.a.y, tri.c.z - tri.a.z);
191    let h = cross(dir, ac);
192    let a = dot(ab, h);
193    if a > -eps && a < eps {
194        return None;
195    }
196    let f = 1.0 / a;
197    let s = Point3::new(origin.x - tri.a.x, origin.y - tri.a.y, origin.z - tri.a.z);
198    let u = f * dot(s, h);
199    if u < 0.0 || u > 1.0 {
200        return None;
201    }
202    let q = cross(s, ab);
203    let v = f * dot(dir, q);
204    if v < 0.0 || u + v > 1.0 {
205        return None;
206    }
207    let t = f * dot(ac, q);
208    if t > eps {
209        Some(t)
210    } else {
211        None
212    }
213}
214
215/// AABB-ray intersection (Slab method). Returns `t_min` if hit.
216pub fn aabb_ray_intersect(bb: AABB3, origin: Point3, dir: Point3) -> bool {
217    let mut tmin = f64::NEG_INFINITY;
218    let mut tmax = f64::INFINITY;
219    let orig = [origin.x, origin.y, origin.z];
220    let dir = [dir.x, dir.y, dir.z];
221    let min = [bb.min.x, bb.min.y, bb.min.z];
222    let max = [bb.max.x, bb.max.y, bb.max.z];
223    for i in 0..3 {
224        if dir[i].abs() < 1e-30 {
225            if orig[i] < min[i] || orig[i] > max[i] {
226                return false;
227            }
228        } else {
229            let inv = 1.0 / dir[i];
230            let mut t1 = (min[i] - orig[i]) * inv;
231            let mut t2 = (max[i] - orig[i]) * inv;
232            if t1 > t2 {
233                std::mem::swap(&mut t1, &mut t2);
234            }
235            if t1 > tmin { tmin = t1; }
236            if t2 < tmax { tmax = t2; }
237            if tmin > tmax {
238                return false;
239            }
240        }
241    }
242    tmax >= 0.0
243}
244
245// ---------------------------------------------------------------------------
246// Plane-plane intersection
247// ---------------------------------------------------------------------------
248
249/// Intersection of two planes: returns a line if they intersect, or None if parallel.
250pub fn plane_plane_intersection(p1: Plane, p2: Plane) -> Option<Line3> {
251    let n1 = p1.normal;
252    let n2 = p2.normal;
253    let dir = cross(n1, n2);
254    if norm(dir) < 1e-10 {
255        return None; // parallel
256    }
257    // Find a point on the intersection line
258    // Solve: n1·p + d1 = 0, n2·p + d2 = 0, set one coord to 0
259    let a = [
260        [n1.y, n1.z, -p1.d],
261        [n2.y, n2.z, -p2.d],
262    ];
263    let det = a[0][0] * a[1][1] - a[0][1] * a[1][0];
264    if det.abs() > 1e-10 {
265        let y = (a[0][2] * a[1][1] - a[0][1] * a[1][2]) / det;
266        let z = (a[0][0] * a[1][2] - a[0][2] * a[1][0]) / det;
267        return Some(Line3::new(Point3::new(0.0, y, z), dir));
268    }
269    // Try setting y = 0
270    let a = [
271        [n1.x, n1.z, -p1.d],
272        [n2.x, n2.z, -p2.d],
273    ];
274    let det = a[0][0] * a[1][1] - a[0][1] * a[1][0];
275    if det.abs() > 1e-10 {
276        let x = (a[0][2] * a[1][1] - a[0][1] * a[1][2]) / det;
277        let z = (a[0][0] * a[1][2] - a[0][2] * a[1][0]) / det;
278        return Some(Line3::new(Point3::new(x, 0.0, z), dir));
279    }
280    // Try setting z = 0
281    let a = [
282        [n1.x, n1.y, -p1.d],
283        [n2.x, n2.y, -p2.d],
284    ];
285    let det = a[0][0] * a[1][1] - a[0][1] * a[1][0];
286    if det.abs() > 1e-10 {
287        let x = (a[0][2] * a[1][1] - a[0][1] * a[1][2]) / det;
288        let y = (a[0][0] * a[1][2] - a[0][2] * a[1][0]) / det;
289        return Some(Line3::new(Point3::new(x, y, 0.0), dir));
290    }
291    None
292}
293
294// ---------------------------------------------------------------------------
295// Line-solid intersections
296// ---------------------------------------------------------------------------
297
298/// Ray-sphere intersection. Returns closest `t > 0` or None.
299pub fn ray_sphere_intersect(origin: Point3, dir: Point3, center: Point3, radius: f64) -> Option<f64> {
300    let oc = Point3::new(origin.x - center.x, origin.y - center.y, origin.z - center.z);
301    let a = dot(dir, dir);
302    let b = 2.0 * dot(oc, dir);
303    let c = dot(oc, oc) - radius * radius;
304    let disc = b * b - 4.0 * a * c;
305    if disc < 0.0 {
306        return None;
307    }
308    let sqrt_disc = disc.sqrt();
309    let t1 = (-b - sqrt_disc) / (2.0 * a);
310    let t2 = (-b + sqrt_disc) / (2.0 * a);
311    if t1 > 1e-7 {
312        Some(t1)
313    } else if t2 > 1e-7 {
314        Some(t2)
315    } else {
316        None
317    }
318}
319
320/// Ray-AABB intersection. Returns closest `t > 0` or None.
321pub fn ray_aabb_intersect(origin: Point3, dir: Point3, bb: AABB3) -> Option<f64> {
322    let mut tmin = f64::NEG_INFINITY;
323    let mut tmax = f64::INFINITY;
324    let orig = [origin.x, origin.y, origin.z];
325    let dir_arr = [dir.x, dir.y, dir.z];
326    let min = [bb.min.x, bb.min.y, bb.min.z];
327    let max = [bb.max.x, bb.max.y, bb.max.z];
328    for i in 0..3 {
329        if dir_arr[i].abs() < 1e-30 {
330            if orig[i] < min[i] || orig[i] > max[i] {
331                return None;
332            }
333        } else {
334            let inv = 1.0 / dir_arr[i];
335            let mut t1 = (min[i] - orig[i]) * inv;
336            let mut t2 = (max[i] - orig[i]) * inv;
337            if t1 > t2 {
338                std::mem::swap(&mut t1, &mut t2);
339            }
340            if t1 > tmin { tmin = t1; }
341            if t2 < tmax { tmax = t2; }
342            if tmin > tmax {
343                return None;
344            }
345        }
346    }
347    if tmax < 1e-7 {
348        return None;
349    }
350    Some(if tmin > 1e-7 { tmin } else { tmax })
351}
352
353/// Ray-cylinder intersection (infinite cylinder along `axis` through `center`).
354pub fn ray_cylinder_intersect(origin: Point3, dir: Point3, center: Point3, axis: Point3, radius: f64) -> Option<f64> {
355    let oc = Point3::new(origin.x - center.x, origin.y - center.y, origin.z - center.z);
356    let d_dot_ax = dot(dir, axis);
357    let oc_dot_ax = dot(oc, axis);
358    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);
359    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);
360    let a = dot(proj_d, proj_d);
361    let b = 2.0 * dot(proj_oc, proj_d);
362    let c = dot(proj_oc, proj_oc) - radius * radius;
363    let disc = b * b - 4.0 * a * c;
364    if disc < 0.0 {
365        return None;
366    }
367    let sqrt_disc = disc.sqrt();
368    let t1 = (-b - sqrt_disc) / (2.0 * a);
369    let t2 = (-b + sqrt_disc) / (2.0 * a);
370    if t1 > 1e-7 {
371        Some(t1)
372    } else if t2 > 1e-7 {
373        Some(t2)
374    } else {
375        None
376    }
377}
378
379// ---------------------------------------------------------------------------
380// Helpers
381// ---------------------------------------------------------------------------
382
383fn dot(a: Point3, b: Point3) -> f64 {
384    a.x * b.x + a.y * b.y + a.z * b.z
385}
386
387fn cross(a: Point3, b: Point3) -> Point3 {
388    Point3::new(
389        a.y * b.z - a.z * b.y,
390        a.z * b.x - a.x * b.z,
391        a.x * b.y - a.y * b.x,
392    )
393}
394
395fn norm(v: Point3) -> f64 {
396    dot(v, v).sqrt()
397}
398
399#[cfg(test)]
400mod tests {
401    use super::*;
402    use super::super::shapes3d::{Point3, Plane};
403
404    fn pt3(x: f64, y: f64, z: f64) -> Point3 { Point3::new(x, y, z) }
405
406    #[test]
407    fn triangle_normal() {
408        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));
409        let n = tri.normal();
410        assert!((n.z - 1.0).abs() < 1e-12);
411    }
412
413    #[test]
414    fn ray_triangle_hit() {
415        // Triangle in the xy-plane at z=0
416        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));
417        let t = ray_triangle_intersect(pt3(0.0, 0.0, -5.0), pt3(0.0, 0.0, 1.0), tri);
418        assert!(t.is_some());
419        assert!((t.unwrap() - 5.0).abs() < 1e-6);
420    }
421
422    #[test]
423    fn ray_triangle_miss() {
424        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));
425        let t = ray_triangle_intersect(pt3(5.0, 5.0, -5.0), pt3(0.0, 0.0, 1.0), tri);
426        assert!(t.is_none());
427    }
428
429    #[test]
430    fn ray_sphere_hit() {
431        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);
432        assert!(t.is_some());
433        assert!((t.unwrap() - 4.0).abs() < 1e-6);
434    }
435
436    #[test]
437    fn ray_sphere_miss() {
438        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);
439        assert!(t.is_none());
440    }
441
442    #[test]
443    fn plane_plane_intersect() {
444        let p1 = Plane::new(pt3(0.0, 0.0, 1.0), 0.0); // z = 0
445        let p2 = Plane::new(pt3(0.0, 1.0, 0.0), 0.0); // y = 0
446        let line = plane_plane_intersection(p1, p2).unwrap();
447        // Intersection is the x-axis
448        assert!((line.dir.x.abs() - 1.0).abs() < 1e-6);
449    }
450
451    #[test]
452    fn aabb_tree_build() {
453        let mesh = TriangleMesh::new(
454            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)],
455            vec![(0, 1, 2), (0, 1, 3)],
456        );
457        let tree = build_aabb_tree(&mesh);
458        assert!(tree.is_some());
459    }
460}