geop_core_geometry/nurb_curve/
closest.rs

1//! [`NurbCurve::closest_point`]: the point of a 3-D curve nearest a given
2//! point.
3
4use geop_core_math::{
5    geop_error::GeopResult, scalars::Scalar, solvers::minimize::descend, vector::Vector3,
6};
7
8use super::NurbCurve;
9use crate::{
10    nearest::{CurveSpan, PointPiece, least_distance},
11    search::{MAX_NODES, NEAREST_HANDOFF},
12};
13
14/// Newton steps from a seed. Bounds effort only: what is returned is a
15/// point of the curve, however many steps it took.
16const NEWTON_STEPS: usize = 30;
17
18/// How often a step that does not bring the point nearer is halved before
19/// the search stops.
20const HALVINGS: usize = 10;
21
22impl NurbCurve<4> {
23    /// The point of the curve nearest `target`, with its parameter.
24    ///
25    /// Subdivision isolates, Newton refines: [`least_distance`] bounds the
26    /// distance over every span of the curve, so no dip of it is missed,
27    /// and Newton on `(C(t) - target) · C'(t) = 0` refines each span left
28    /// at the handoff, kept inside the domain — which also reaches an end
29    /// of the curve, where the nearest point is one. The parameter is a
30    /// free choice of which point of the curve is returned, so every
31    /// iterate is sharpened: the point returned lies on the curve, and its
32    /// distance to `target` is attained.
33    pub fn closest_point(&self, target: &Vector3) -> GeopResult<(Scalar, Vector3)> {
34        let nearest = least_distance(
35            vec![PointPiece(*target)],
36            vec![CurveSpan(self.clone())],
37            self.size()?.mul(Scalar::from_f64(NEAREST_HANDOFF)),
38            MAX_NODES,
39        )?;
40        let distance = |p: &Vector3| p.sub(target).norm_sq().to_f64();
41        let (mut t, mut p) = nearest.b;
42        for ((), seed) in nearest.candidates {
43            let refined = self.closest_parameter_from(target, seed)?;
44            let q = self.evaluate(refined)?;
45            if distance(&q) < distance(&p) {
46                (t, p) = (refined, q);
47            }
48        }
49        Ok((t, p))
50    }
51
52    /// The parameter of the point of the curve nearest `target` around the
53    /// seed `t`, sharp: the local search [`NurbCurve::closest_point`]
54    /// refines each of its seeds with, for a caller that already knows
55    /// where to look.
56    ///
57    /// Damped Newton on `f(t) = (C(t) - target) · C'(t)` from `t`: the full
58    /// derivative `f'` where it is definitely positive — near the nearest
59    /// point — and `|C'|²` where not (Gauss–Newton, a descent direction
60    /// wherever the curve is regular). A step that does not bring the point
61    /// nearer is halved, and the search stops once halving does not help.
62    /// Every iterate is sharpened — which point is returned is a free
63    /// choice — and clamped into the domain.
64    pub fn closest_parameter_from(&self, target: &Vector3, t: Scalar) -> GeopResult<Scalar> {
65        let (t0, t1) = self.domain();
66        descend(
67            t,
68            NEWTON_STEPS,
69            HALVINGS,
70            |&t| -> GeopResult<f64> { Ok(self.evaluate(t)?.sub(target).norm_sq().to_f64()) },
71            |&t| -> GeopResult<_> {
72                let r = self.evaluate(t)?.sub(target);
73                let d1 = self.tangent(t)?;
74                let d2 = self.second_derivative(t)?;
75                let f = r.prod_dot(&d1);
76                let speed = d1.prod_dot(&d1);
77                let full = speed.add(r.prod_dot(&d2));
78                let slope = if full.definitely_greater(Scalar::ZERO) {
79                    full
80                } else if speed.definitely_greater(Scalar::ZERO) {
81                    speed
82                } else {
83                    return Ok(None);
84                };
85                let step = f.div(slope)?;
86                Ok(Some(move |scale: Scalar| {
87                    t.sub(step.mul(scale)).sharpen().clamped(t0, t1)
88                }))
89            },
90        )
91    }
92}
93
94#[cfg(test)]
95mod tests {
96    use geop_core_math::{
97        scalars::Scalar,
98        vector::{Vector3, Vector4},
99    };
100
101    use crate::nurb_curve::NurbCurve;
102
103    /// A quarter circle of radius 2 about the origin, in the xy plane.
104    fn quarter() -> NurbCurve<4> {
105        let w = Scalar::from_f64(std::f64::consts::FRAC_1_SQRT_2);
106        let p = |x: f64, y: f64, w: Scalar| {
107            Vector4::from_array([
108                Scalar::from_f64(x).mul(w),
109                Scalar::from_f64(y).mul(w),
110                Scalar::ZERO,
111                w,
112            ])
113        };
114        NurbCurve::try_new(
115            2,
116            vec![
117                p(2.0, 0.0, Scalar::ONE),
118                p(2.0, 2.0, w),
119                p(0.0, 2.0, Scalar::ONE),
120            ],
121            vec![
122                Scalar::ZERO,
123                Scalar::ZERO,
124                Scalar::ZERO,
125                Scalar::ONE,
126                Scalar::ONE,
127                Scalar::ONE,
128            ],
129        )
130        .unwrap()
131    }
132
133    /// The nearest point of an arc to a point off it lies on the ray from
134    /// the centre through that point; beyond the arc's end, it is the end.
135    #[test]
136    fn nearest_points_of_an_arc() {
137        let arc = quarter();
138        let target = Vector3::from_array([3.0, 3.0, 1.0].map(Scalar::from_f64));
139        let (_, p) = arc.closest_point(&target).unwrap();
140        let expected = Vector3::from_array([2f64.sqrt(), 2f64.sqrt(), 0.0].map(Scalar::from_f64));
141        assert!(p.sub(&expected).norm().to_f64() < 1e-12, "{p:?}");
142
143        let beyond = Vector3::from_array([3.0, -1.0, 0.0].map(Scalar::from_f64));
144        let (t, p) = arc.closest_point(&beyond).unwrap();
145        assert!(t.could_be_equal(Scalar::ZERO));
146        assert!(p.could_be_equal(&Vector3::from_array([2.0, 0.0, 0.0].map(Scalar::from_f64))));
147    }
148}
geop_core_geometry/nurb_curve/closest.rs

geop_core_geometry/nurb_curve/
closest.rs
