Skip to main content

kernel_api/
nearest.rs

1//! The point of a segment or a triangle nearest a given point: one
2//! implementation over plain coordinates, in single or double precision,
3//! a segment in any number of dimensions and a triangle in three.
4
5use std::ops::{Add, Div, Mul, Sub};
6
7/// A coordinate the queries work in: `f32` or `f64`.
8pub 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
37/// How far along the segment from `a` to `b` its point nearest `p` lies,
38/// from 0 at `a` to 1 at `b`. A segment with no length answers 0, its
39/// start.
40pub 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    // Also false for a NaN length, which answers the start as well.
44    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
58/// The point of the segment from `a` to `b` nearest `p`; `a` for a
59/// segment with no length.
60pub 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
64/// The point of triangle `abc` nearest `p`. A triangle with no area
65/// answers the nearest point of its sides.
66pub 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
130/// `from + by * t`.
131fn 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}