geop_core_geometry/nurb_surface/
closest.rs

1//! [`NurbSurface::closest_point`]: the point of a surface nearest a given
2//! point.
3
4use geop_core_math::{
5    geop_error::GeopResult,
6    scalars::Scalar,
7    solvers::{linear::solve_symmetric2, minimize::descend},
8    vector::Vector3,
9};
10
11use super::NurbSurface;
12use crate::{
13    nearest::{PointPiece, SurfacePatch, least_distance},
14    search::{MAX_NODES, NEAREST_HANDOFF},
15};
16
17/// Newton steps from a seed, at most. Bounds effort only: every iterate is
18/// a point of the surface nearer than the one before.
19const NEWTON_STEPS: usize = 40;
20
21/// How often a step that does not bring the point nearer is halved before
22/// the search stops.
23const HALVINGS: usize = 10;
24
25impl NurbSurface<4> {
26    /// The point of the (untrimmed) surface nearest `target`, with its
27    /// parameters.
28    ///
29    /// Subdivision isolates, Newton refines: [`least_distance`] bounds the
30    /// distance over every patch of the surface, so no dip of it is missed,
31    /// and each patch left at the handoff is refined by damped Newton (see
32    /// [`NurbSurface::refine_closest`]). The parameters are a free choice
33    /// of which point of the surface is returned, so every iterate is
34    /// sharp: the point returned lies on the surface, and its distance to
35    /// `target` is attained.
36    pub fn closest_point(&self, target: &Vector3) -> GeopResult<(Scalar, Scalar, Vector3)> {
37        let nearest = least_distance(
38            vec![PointPiece(*target)],
39            vec![SurfacePatch(self.clone())],
40            self.size()?.mul(Scalar::from_f64(NEAREST_HANDOFF)),
41            MAX_NODES,
42        )?;
43        let distance = |p: &Vector3| p.sub(target).norm_sq().to_f64();
44        let ((mut u, mut v), mut p) = nearest.b;
45        for ((), (su, sv)) in nearest.candidates {
46            let (ru, rv) = self.refine_closest(target, su, sv)?;
47            let q = self.evaluate(ru, rv)?;
48            if distance(&q) < distance(&p) {
49                (u, v, p) = (ru, rv, q);
50            }
51        }
52        Ok((u, v, p))
53    }
54
55    /// Damped Newton on the gradient of half the squared distance to
56    /// `target`, from `(u, v)`. The step solves the full Hessian where it
57    /// is definitely positive definite — near the nearest point, where it
58    /// converges quadratically — and the first fundamental form where not
59    /// (Gauss–Newton, a descent direction wherever the parametrization is
60    /// regular); at a pole, where that is singular too, it moves along the
61    /// one parameter that still moves the point. A step that does not
62    /// bring the point nearer is halved, and the search stops once halving
63    /// does not help. Every iterate is clamped into the domain.
64    pub fn refine_closest(
65        &self,
66        target: &Vector3,
67        u: Scalar,
68        v: Scalar,
69    ) -> GeopResult<(Scalar, Scalar)> {
70        let ((u_lo, u_hi), (v_lo, v_hi)) = (self.domain_u(), self.domain_v());
71        let positive = |a: Scalar, b: Scalar, c: Scalar| {
72            a.definitely_greater(Scalar::ZERO)
73                && a.mul(c).sub(b.mul(b)).definitely_greater(Scalar::ZERO)
74        };
75        descend(
76            (u, v),
77            NEWTON_STEPS,
78            HALVINGS,
79            |&(u, v)| -> GeopResult<f64> {
80                Ok(self.evaluate(u, v)?.sub(target).norm_sq().to_f64())
81            },
82            |&(u, v)| -> GeopResult<_> {
83                let r = self.evaluate(u, v)?.sub(target);
84                let (su, sv) = self.derivatives(u, v)?;
85                let [suu, suv, svv] = self.second_derivatives(u, v)?;
86                let (gu, gv) = (r.prod_dot(&su), r.prod_dot(&sv));
87                let (a11, a12, a22) = (su.prod_dot(&su), su.prod_dot(&sv), sv.prod_dot(&sv));
88                let (h11, h12, h22) = (
89                    a11.add(r.prod_dot(&suu)),
90                    a12.add(r.prod_dot(&suv)),
91                    a22.add(r.prod_dot(&svv)),
92                );
93                let downhill = (gu.neg(), gv.neg());
94                let (du, dv) = if positive(h11, h12, h22) {
95                    solve_symmetric2((h11, h12, h22), downhill)?
96                } else if positive(a11, a12, a22) {
97                    solve_symmetric2((a11, a12, a22), downhill)?
98                } else if a11.definitely_greater(Scalar::ZERO) {
99                    (gu.neg().div(a11)?, Scalar::ZERO)
100                } else if a22.definitely_greater(Scalar::ZERO) {
101                    (Scalar::ZERO, gv.neg().div(a22)?)
102                } else {
103                    return Ok(None);
104                };
105                Ok(Some(move |scale: Scalar| {
106                    (
107                        u.add(du.mul(scale)).sharpen().clamped(u_lo, u_hi),
108                        v.add(dv.mul(scale)).sharpen().clamped(v_lo, v_hi),
109                    )
110                }))
111            },
112        )
113    }
114}
115
116#[cfg(test)]
117mod tests {
118    use geop_core_math::{
119        scalars::Scalar,
120        vector::{Vector3, Vector4},
121    };
122
123    use crate::nurb_surface::NurbSurface3D;
124
125    /// A quarter of a cylinder of radius 1 about z, from +x to +y, from
126    /// z = 0 to 2: degree 2 around, 1 along.
127    fn quarter_cylinder() -> NurbSurface3D {
128        let w = std::f64::consts::FRAC_1_SQRT_2;
129        let h = |x: f64, y: f64, z: f64, w: f64| {
130            Vector4::from_array([x * w, y * w, z * w, w].map(Scalar::from_f64))
131        };
132        let mut points = Vec::new();
133        for (x, y, wt) in [(1.0, 0.0, 1.0), (1.0, 1.0, w), (0.0, 1.0, 1.0)] {
134            for z in [0.0, 2.0] {
135                points.push(h(x, y, z, wt));
136            }
137        }
138        let knots = |k: &[f64]| k.iter().map(|&k| Scalar::from_f64(k)).collect();
139        NurbSurface3D::try_new(
140            2,
141            1,
142            points,
143            knots(&[0.0, 0.0, 0.0, 1.0, 1.0, 1.0]),
144            knots(&[0.0, 0.0, 1.0, 1.0]),
145        )
146        .unwrap()
147    }
148
149    /// The nearest point of a cylinder lies on the radius through the
150    /// target.
151    #[test]
152    fn nearest_point_of_a_cylinder() {
153        let surface = quarter_cylinder();
154        let target = Vector3::from_array([2.0, 0.5, 0.5].map(Scalar::from_f64));
155        let (_, _, p) = surface.closest_point(&target).unwrap();
156        let r = 4.25f64.sqrt();
157        let expected = Vector3::from_array([2.0 / r, 0.5 / r, 0.5].map(Scalar::from_f64));
158        assert!(p.sub(&expected).norm().to_f64() < 1e-12, "{p:?}");
159    }
160}
geop_core_geometry/nurb_surface/closest.rs

geop_core_geometry/nurb_surface/
closest.rs
