1use super::shapes3d::{Line3, Plane, Point3};
4use super::spatial::AABB3;
5
6#[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#[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 pub fn normals(&self) -> Vec<Point3> {
66 (0..self.indices.len())
67 .map(|i| self.triangle(i).normal())
68 .collect()
69 }
70}
71
72pub 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 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
130pub 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 let bounds = tris.iter().map(|(_, b)| *b).reduce(|a, b| a.merge(b)).unwrap();
155 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
182pub 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
220pub 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
250pub 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; }
262 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 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 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
299pub 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
325pub 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
358pub 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
384fn 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 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 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 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); let p2 = Plane::new(pt3(0.0, 1.0, 0.0), 0.0); let line = plane_plane_intersection(p1, p2).unwrap();
469 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}