1use std::ops::{Add, Div, Mul, Sub};
6
7pub trait Real:
9 Copy
10 + PartialOrd
11 + Add<Output = Self>
12 + Sub<Output = Self>
13 + Mul<Output = Self>
14 + Div<Output = Self>
15 + sealed::Sealed
16{
17 const ZERO: Self;
18 const ONE: Self;
19}
20
21impl Real for f32 {
22 const ZERO: Self = 0.0;
23 const ONE: Self = 1.0;
24}
25
26impl Real for f64 {
27 const ZERO: Self = 0.0;
28 const ONE: Self = 1.0;
29}
30
31mod sealed {
32 pub trait Sealed {}
33 impl Sealed for f32 {}
34 impl Sealed for f64 {}
35}
36
37pub fn segment_param<S: Real, const N: usize>(p: [S; N], a: [S; N], b: [S; N]) -> S {
41 let ab = sub(b, a);
42 let length_sq = dot(ab, ab);
43 let t = if length_sq > S::ZERO {
45 dot(sub(p, a), ab) / length_sq
46 } else {
47 S::ZERO
48 };
49 if t < S::ZERO {
50 S::ZERO
51 } else if t > S::ONE {
52 S::ONE
53 } else {
54 t
55 }
56}
57
58pub fn nearest_on_segment<S: Real, const N: usize>(p: [S; N], a: [S; N], b: [S; N]) -> [S; N] {
61 along(a, sub(b, a), segment_param(p, a, b))
62}
63
64pub fn nearest_on_triangle<S: Real>(p: [S; 3], a: [S; 3], b: [S; 3], c: [S; 3]) -> [S; 3] {
67 let ab = sub(b, a);
68 let ac = sub(c, a);
69 let n = cross(ab, ac);
70 if dot(n, n) == S::ZERO {
71 return nearest_on_sides(p, a, b, c);
72 }
73 let ap = sub(p, a);
74 let d1 = dot(ab, ap);
75 let d2 = dot(ac, ap);
76 if d1 <= S::ZERO && d2 <= S::ZERO {
77 return a;
78 }
79 let bp = sub(p, b);
80 let d3 = dot(ab, bp);
81 let d4 = dot(ac, bp);
82 if d3 >= S::ZERO && d4 <= d3 {
83 return b;
84 }
85 let vc = d1 * d4 - d3 * d2;
86 if vc <= S::ZERO && d1 >= S::ZERO && d3 <= S::ZERO {
87 return along(a, ab, d1 / (d1 - d3));
88 }
89 let cp = sub(p, c);
90 let d5 = dot(ab, cp);
91 let d6 = dot(ac, cp);
92 if d6 >= S::ZERO && d5 <= d6 {
93 return c;
94 }
95 let vb = d5 * d2 - d1 * d6;
96 if vb <= S::ZERO && d2 >= S::ZERO && d6 <= S::ZERO {
97 return along(a, ac, d2 / (d2 - d6));
98 }
99 let va = d3 * d6 - d5 * d4;
100 if va <= S::ZERO && (d4 - d3) >= S::ZERO && (d5 - d6) >= S::ZERO {
101 return along(b, sub(c, b), (d4 - d3) / ((d4 - d3) + (d5 - d6)));
102 }
103 let denom = va + vb + vc;
104 if denom <= S::ZERO {
105 return nearest_on_sides(p, a, b, c);
106 }
107 along(along(a, ab, vb / denom), ac, vc / denom)
108}
109
110fn nearest_on_sides<S: Real>(p: [S; 3], a: [S; 3], b: [S; 3], c: [S; 3]) -> [S; 3] {
111 let mut best = nearest_on_segment(p, a, b);
112 for (u, v) in [(b, c), (c, a)] {
113 let q = nearest_on_segment(p, u, v);
114 let (dq, db) = (sub(q, p), sub(best, p));
115 if dot(dq, dq) < dot(db, db) {
116 best = q;
117 }
118 }
119 best
120}
121
122pub(crate) fn sub<S: Real, const N: usize>(a: [S; N], b: [S; N]) -> [S; N] {
123 std::array::from_fn(|i| a[i] - b[i])
124}
125
126pub(crate) fn dot<S: Real, const N: usize>(a: [S; N], b: [S; N]) -> S {
127 (1..N).fold(a[0] * b[0], |s, i| s + a[i] * b[i])
128}
129
130fn along<S: Real, const N: usize>(from: [S; N], by: [S; N], t: S) -> [S; N] {
132 std::array::from_fn(|i| from[i] + by[i] * t)
133}
134
135pub(crate) fn cross<S: Real>(a: [S; 3], b: [S; 3]) -> [S; 3] {
136 [
137 a[1] * b[2] - a[2] * b[1],
138 a[2] * b[0] - a[0] * b[2],
139 a[0] * b[1] - a[1] * b[0],
140 ]
141}
142
143#[cfg(test)]
144mod tests {
145 use super::*;
146
147 fn near<const N: usize>(got: [f64; N], want: [f64; N]) -> bool {
148 got.iter().zip(want).all(|(g, w)| (g - w).abs() < 1e-12)
149 }
150
151 #[test]
152 fn a_segment_clamps_at_both_ends_and_a_point_segment_answers_its_start() {
153 let (a, b) = ([1.0, 1.0], [3.0, 1.0]);
154 for (p, t, want) in [
155 ([2.5, 4.0], 0.75, [2.5, 1.0]),
156 ([-5.0, 0.0], 0.0, a),
157 ([9.0, -2.0], 1.0, b),
158 ] {
159 assert_eq!(segment_param(p, a, b), t);
160 assert!(near(nearest_on_segment(p, a, b), want));
161 }
162 let p = [0.0, 0.0, 7.0];
163 assert_eq!(segment_param(p, [2.0; 3], [2.0; 3]), 0.0);
164 assert_eq!(nearest_on_segment(p, [2.0; 3], [2.0; 3]), [2.0; 3]);
165 assert_eq!(
166 nearest_on_segment([0.5f32, 9.0], [0.0; 2], [1.0, 0.0]),
167 [0.5, 0.0]
168 );
169 }
170
171 #[test]
172 fn a_triangle_answers_its_face_sides_and_corners_and_a_flat_one_its_sides() {
173 let (a, b, c) = ([0.0; 3], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]);
174 for (p, [a, b, c], want) in [
175 ([0.25, 0.25, 2.0], [a, b, c], [0.25, 0.25, 0.0]),
176 ([2.0, -1.0, 0.0], [a, b, c], b),
177 ([1.0, 1.0, 0.0], [a, b, c], [0.5, 0.5, 0.0]),
178 ([0.5, 1.0, 0.0], [a, a, b], [0.5, 0.0, 0.0]),
179 ([0.5, 1.0, 0.0], [a, b, [2.0, 0.0, 0.0]], [0.5, 0.0, 0.0]),
180 (c, [a, a, a], a),
181 ] {
182 assert!(near(nearest_on_triangle(p, a, b, c), want), "{p:?}");
183 }
184 }
185}