1use crate::nearest::{Real, cross, dot, sub};
6
7pub trait RayReal: Real {
9 const EDGE_SLACK: Self;
12 const FLAT: Self;
15}
16
17impl RayReal for f32 {
18 const EDGE_SLACK: Self = 1e-5;
19 const FLAT: Self = 1e-12;
20}
21
22impl RayReal for f64 {
23 const EDGE_SLACK: Self = 1e-9;
24 const FLAT: Self = 1e-12;
25}
26
27pub fn ray_triangle<S: RayReal>(
38 origin: [S; 3],
39 direction: [S; 3],
40 a: [S; 3],
41 b: [S; 3],
42 c: [S; 3],
43) -> Option<S> {
44 let (e1, e2) = (sub(b, a), sub(c, a));
45 let p = cross(direction, e2);
46 let det = dot(e1, p);
47 if det < S::FLAT && det > S::ZERO - S::FLAT {
48 return None;
49 }
50 let inv = S::ONE / det;
51 let s = sub(origin, a);
52 let u = dot(s, p) * inv;
53 let (low, high) = (S::ZERO - S::EDGE_SLACK, S::ONE + S::EDGE_SLACK);
54 if u < low || u > high {
55 return None;
56 }
57 let q = cross(s, e1);
58 let v = dot(direction, q) * inv;
59 if v < low || u + v > high {
60 return None;
61 }
62 Some(dot(e2, q) * inv)
63}
64
65#[cfg(test)]
66mod tests {
67 use super::*;
68
69 #[test]
70 fn crosses_inside_on_either_side_and_not_beside_or_along() {
71 let (a, b, c) = ([0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]);
72 let down = [0.0, 0.0, -1.0];
73 for (from, dir, want) in [
74 ([0.25, 0.25, 1.0], down, Some(1.0)),
75 ([0.25, 0.25, -2.0], down, Some(-2.0)),
76 ([0.0, 0.0, 1.0], down, Some(1.0)),
77 ([2.0, 2.0, 1.0], down, None),
78 ([0.25, 0.25, 1.0], [1.0, 0.0, 0.0], None),
79 ] {
80 assert_eq!(ray_triangle::<f64>(from, dir, a, b, c), want, "{from:?}");
81 }
82 assert_eq!(ray_triangle([0.0, 0.0, 1.0], down, a, a, a), None);
83 }
84
85 #[test]
89 fn a_ray_through_a_shared_edge_hits_a_side_of_it() {
90 fn check<S: RayReal>(of: fn(f64) -> S) {
91 let point = |p: [f64; 3]| p.map(of);
92 let (a, b) = ([0.137, 2.31, 0.71], [3.91, -1.07, 1.93]);
93 let (c, d) = ([4.2, 3.3, 0.4], [-0.6, -1.9, 1.1]);
94 let mut seed = 12345_u64;
95 let mut next = || {
96 seed = seed
97 .wrapping_mul(6364136223846793005)
98 .wrapping_add(1442695040888963407);
99 (seed >> 11) as f64 / (1_u64 << 53) as f64
100 };
101 for _ in 0..20_000 {
102 let s = next();
103 let on: [f64; 3] = std::array::from_fn(|i| a[i] + (b[i] - a[i]) * s);
104 let from = [
105 next() * 20.0 - 10.0,
106 next() * 20.0 - 10.0,
107 15.0 + next() * 10.0,
108 ];
109 let dir: [f64; 3] = std::array::from_fn(|i| on[i] - from[i]);
110 let (from, dir) = (point(from), point(dir));
111 let hits = [
112 ray_triangle(from, dir, point(a), point(b), point(c)),
113 ray_triangle(from, dir, point(b), point(a), point(d)),
114 ];
115 assert!(hits.iter().any(Option::is_some), "through {on:?}");
116 }
117 }
118 check(|x| x as f32);
119 check(|x| x);
120 }
121}