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> {
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
215pub 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
245pub 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; }
257 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 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 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
294pub 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
320pub 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
353pub 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
379fn 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 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); let p2 = Plane::new(pt3(0.0, 1.0, 0.0), 0.0); let line = plane_plane_intersection(p1, p2).unwrap();
447 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}