geop_ops_step/import/
midpoints.rs

1//! Curves and surfaces in `f64`: every control point, weight and knot at the
2//! middle of its enclosure. For the work whose answer is a free choice or a
3//! measurement in `f64` — where to sample, a seed for a search, how far a
4//! point lies off a surface — many times cheaper than evaluating the
5//! enclosures, and off them only by roundings.
6//! The kernel itself stays in intervals: these serve the importer only,
7//! never an answer the kernel relies on (see `Scalar::sharpen`).
8
9use geop_core_geometry::{nurb_curve::NurbCurve3D, nurb_surface::NurbSurface3D};
10use std::convert::Infallible;
11
12use geop_core_math::solvers::iterate::{Repeat, Step, iterate};
13
14use geop_core_math::plain::{P3, dot, norm, solve2, sub};
15
16/// A curve's midpoints in `f64`: its control points, weights and knots each
17/// at the middle of its enclosure. For searches whose answer is a free
18/// choice — which parameter to measure a gap at, where to sample — many
19/// times cheaper than evaluating the enclosure, and off it only by
20/// roundings.
21pub struct CurveMidpoints {
22    degree: usize,
23    knots: Vec<f64>,
24    /// Homogeneous: `(w x, w y, w z, w)`.
25    points: Vec<[f64; 4]>,
26}
27
28impl CurveMidpoints {
29    pub fn of(curve: &NurbCurve3D) -> Self {
30        Self {
31            degree: curve.degree,
32            knots: curve.knot_vector.iter().map(|k| k.to_f64()).collect(),
33            points: curve
34                .control_points
35                .iter()
36                .map(|p| std::array::from_fn(|c| p[c].to_f64()))
37                .collect(),
38        }
39    }
40
41    pub fn domain(&self) -> (f64, f64) {
42        (self.knots[self.degree], self.knots[self.points.len()])
43    }
44
45    /// The point at `t`, clamped into the domain: de Boor's algorithm.
46    pub fn point(&self, t: f64) -> P3 {
47        let (lo, hi) = self.domain();
48        let t = t.clamp(lo, hi);
49        let k = span(self.degree, &self.knots, self.points.len(), t);
50        let (h, _) = de_boor(
51            self.degree,
52            &self.knots,
53            &self.points[k - self.degree..=k],
54            k,
55            t,
56        );
57        cartesian(h)
58    }
59}
60
61/// A surface's midpoints in `f64` (see [`CurveMidpoints`]): for the searches on
62/// a surface whose answer is a free choice or a measurement in `f64` — a
63/// seed for a projection, how far a point lies off the surface, where to
64/// draw it.
65pub struct SurfaceMidpoints {
66    degree_u: usize,
67    degree_v: usize,
68    knots_u: Vec<f64>,
69    knots_v: Vec<f64>,
70    num_u: usize,
71    num_v: usize,
72    /// Homogeneous, `u` index major, as the surface's own.
73    points: Vec<[f64; 4]>,
74}
75
76/// Newton steps of a foot point search on [`SurfaceMidpoints`].
77const PROJECT_ITERATIONS: usize = 30;
78
79impl SurfaceMidpoints {
80    pub fn of(surface: &NurbSurface3D) -> Self {
81        Self {
82            degree_u: surface.degree_u,
83            degree_v: surface.degree_v,
84            knots_u: surface.knot_vector_u.iter().map(|k| k.to_f64()).collect(),
85            knots_v: surface.knot_vector_v.iter().map(|k| k.to_f64()).collect(),
86            num_u: surface.num_u,
87            num_v: surface.num_v,
88            points: surface
89                .control_points
90                .iter()
91                .map(|p| std::array::from_fn(|c| p[c].to_f64()))
92                .collect(),
93        }
94    }
95
96    pub fn domain_u(&self) -> (f64, f64) {
97        (self.knots_u[self.degree_u], self.knots_u[self.num_u])
98    }
99
100    pub fn domain_v(&self) -> (f64, f64) {
101        (self.knots_v[self.degree_v], self.knots_v[self.num_v])
102    }
103
104    /// The point at `(u, v)`, clamped into the domain, and its partial
105    /// derivatives there: `(Scalar, S_u, S_v)`.
106    pub fn partials(&self, u: f64, v: f64) -> (P3, P3, P3) {
107        let (p, q) = (self.degree_u, self.degree_v);
108        let (u0, u1) = self.domain_u();
109        let (v0, v1) = self.domain_v();
110        let (u, v) = (u.clamp(u0, u1), v.clamp(v0, v1));
111        let ku = span(p, &self.knots_u, self.num_u, u);
112        let kv = span(q, &self.knots_v, self.num_v, v);
113        // Each column of the control points acting there at `u`, then
114        // those at `v`.
115        let (columns, columns_u): (Vec<[f64; 4]>, Vec<[f64; 4]>) = (kv - q..=kv)
116            .map(|j| {
117                let column: Vec<[f64; 4]> = (ku - p..=ku)
118                    .map(|i| self.points[i * self.num_v + j])
119                    .collect();
120                de_boor(p, &self.knots_u, &column, ku, u)
121            })
122            .unzip();
123        let (a, a_v) = de_boor(q, &self.knots_v, &columns, kv, v);
124        let (a_u, _) = de_boor(q, &self.knots_v, &columns_u, kv, v);
125        let point = cartesian(a);
126        // The quotient rule: `Scalar' = (A' - w' Scalar) / w`.
127        let rational =
128            |d: [f64; 4]| -> P3 { std::array::from_fn(|c| (d[c] - d[3] * point[c]) / a[3]) };
129        (point, rational(a_u), rational(a_v))
130    }
131
132    /// The point at `(u, v)`, clamped into the domain.
133    pub fn point(&self, u: f64, v: f64) -> P3 {
134        self.partials(u, v).0
135    }
136
137    /// The foot point of `target` on the surface by Newton from `(u, v)`,
138    /// on the first fundamental form, in the domain. A step is taken only
139    /// where it makes progress: brings the point nearer `target`, or, no
140    /// further from it, nearer meeting the foot point's conditions
141    /// `r · S_u = r · S_v = 0` (`r` from the point to `target`) — near the
142    /// foot point the distance changes too little to tell in `f64`, the
143    /// conditions do not; but alone they also lead to points furthest from
144    /// `target`. Where
145    /// the full step makes none — at a pole, where one derivative vanishes
146    /// up to the roundings of the midpoints and the system is as good as
147    /// singular — the step along either derivative alone; and where
148    /// neither does, the step along a derivative from the other parameter
149    /// at its domain's ends or middle, which at a pole is the same point on
150    /// another meridian (as `NurbSurface::project` leaves a pole). Where
151    /// none does, the search has arrived.
152    pub fn project(&self, target: P3, u: f64, v: f64) -> (f64, f64) {
153        let (u0, u1) = self.domain_u();
154        let (v0, v1) = self.domain_v();
155        // How far from `target` the point with partials `(p, su, sv)` is,
156        // and how far from meeting the foot point's conditions.
157        let progress = |(p, su, sv): (P3, P3, P3)| {
158            let r = sub(target, p);
159            (norm(r), dot(su, r).powi(2) + dot(sv, r).powi(2))
160        };
161        // The partials at the seed, carried from the step that led there.
162        let mut now = self.partials(u, v);
163        let iterated = iterate(
164            (u, v),
165            (u, v),
166            PROJECT_ITERATIONS,
167            Repeat::Never,
168            |&next| (next, next),
169            |&(u, v)| -> Result<Step<(f64, f64)>, Infallible> {
170                let (distance_now, residual_now) = progress(now);
171                // Where a step from `(u, v)`, with the partials there, leads:
172                // the full one, or along `S_u` or `S_v` alone.
173                let step = |(u, v): (f64, f64), (p, su, sv): (P3, P3, P3), along: Option<bool>| {
174                    let r = sub(target, p);
175                    let (a11, a12, a22) = (dot(su, su), dot(su, sv), dot(sv, sv));
176                    let (b1, b2) = (dot(su, r), dot(sv, r));
177                    let (du, dv) = match along {
178                        None => solve2([[a11, a12], [a12, a22]], [b1, b2]).into(),
179                        Some(true) => (b1 / a11, 0.0),
180                        Some(false) => (0.0, b2 / a22),
181                    };
182                    ((u + du).clamp(u0, u1), (v + dv).clamp(v0, v1))
183                };
184                let here = [None, Some(true), Some(false)]
185                    .into_iter()
186                    .map(|along| step((u, v), now, along));
187                let elsewhere = [(u, v0), (u, (v0 + v1) / 2.0), (u, v1)]
188                    .into_iter()
189                    .map(|from| (from, Some(true)))
190                    .chain(
191                        [(u0, v), ((u0 + u1) / 2.0, v), (u1, v)]
192                            .into_iter()
193                            .map(|from| (from, Some(false))),
194                    )
195                    .map(|(from, along)| step(from, self.partials(from.0, from.1), along));
196                let Some((next, partials)) = here.chain(elsewhere).find_map(|next| {
197                    if !(next.0.is_finite() && next.1.is_finite()) {
198                        return None;
199                    }
200                    let partials = self.partials(next.0, next.1);
201                    let (distance, residual) = progress(partials);
202                    let nearer = distance < distance_now;
203                    let closer = distance <= distance_now && residual < residual_now;
204                    (nearer || closer).then_some((next, partials))
205                }) else {
206                    return Ok(Step::Stuck);
207                };
208                now = partials;
209                Ok(Step::Next(next))
210            },
211        );
212        let Ok(iterated) = iterated;
213        iterated.seed
214    }
215}
216
217/// The span `k` of `knots` with `knots[k] <= t < knots[k + 1]`, for a
218/// spline of `degree` with `n` control points: the last one at the domain's
219/// end.
220fn span(degree: usize, knots: &[f64], n: usize, t: f64) -> usize {
221    (degree..n)
222        .rev()
223        .find(|&k| knots[k] <= t && knots[k] < knots[k + 1])
224        .unwrap_or(degree)
225}
226
227/// De Boor's algorithm on the homogeneous control points `local` acting on
228/// span `k` of `knots`: the point at `t` and its derivative, which is the
229/// difference of the last two points before the last step, over the span
230/// (`degree` times).
231fn de_boor(
232    degree: usize,
233    knots: &[f64],
234    local: &[[f64; 4]],
235    k: usize,
236    t: f64,
237) -> ([f64; 4], [f64; 4]) {
238    let p = degree;
239    let mut d = local.to_vec();
240    let mut derivative = [0.0; 4];
241    for r in 1..=p {
242        if r == p {
243            let width = knots[k + 1] - knots[k];
244            derivative = std::array::from_fn(|c| p as f64 * (d[p][c] - d[p - 1][c]) / width);
245        }
246        for j in (r..=p).rev() {
247            let i = j + k - p;
248            let width = knots[i + p + 1 - r] - knots[i];
249            let a = if width == 0.0 {
250                0.0
251            } else {
252                (t - knots[i]) / width
253            };
254            let before = d[j - 1];
255            for (x, y) in d[j].iter_mut().zip(before) {
256                *x = (1.0 - a) * y + a * *x;
257            }
258        }
259    }
260    (d[p], derivative)
261}
262
263/// The Cartesian point of the homogeneous `h`.
264fn cartesian(h: [f64; 4]) -> P3 {
265    [h[0] / h[3], h[1] / h[3], h[2] / h[3]]
266}
267
268#[cfg(test)]
269mod tests {
270    use geop_core_geometry::{nurb_curve::NurbCurve, nurb_surface::NurbSurface3D, shape::Axis};
271    use geop_core_math::{
272        scalars::Scalar,
273        vector::{Vector3, Vector4},
274    };
275
276    use super::SurfaceMidpoints;
277    use geop_core_math::plain::distance;
278
279    fn v3(p: [f64; 3]) -> Vector3 {
280        Vector3::from_array(p.map(Scalar::from_f64))
281    }
282
283    /// An eighth of the unit sphere: rational, of degree 4 and with a
284    /// pole-free but strongly varying parametrization.
285    fn octant() -> NurbSurface3D {
286        NurbSurface3D::spherical_triangle(
287            &v3([0.0, 0.0, 0.0]),
288            Scalar::ONE,
289            [
290                v3([1.0, 0.0, 0.0]),
291                v3([0.0, 1.0, 0.0]),
292                v3([0.0, 0.0, 1.0]),
293            ],
294        )
295        .unwrap()
296    }
297
298    #[test]
299    fn surface_midpoints_are_the_surface_and_its_derivatives() {
300        let surface = octant();
301        let midpoints = SurfaceMidpoints::of(&surface);
302        let (u0, u1) = midpoints.domain_u();
303        let (v0, v1) = midpoints.domain_v();
304        for i in 0..=6 {
305            for j in 0..=6 {
306                let u = u0 + (u1 - u0) * i as f64 / 6.0;
307                let v = v0 + (v1 - v0) * j as f64 / 6.0;
308                let (p, su, sv) = midpoints.partials(u, v);
309                let (u, v) = (Scalar::from_f64(u), Scalar::from_f64(v));
310                let (eu, ev) = surface.derivatives(u, v).unwrap();
311                for (got, want) in [
312                    (p, surface.evaluate(u, v).unwrap().to_f64_array()),
313                    (su, eu.to_f64_array()),
314                    (sv, ev.to_f64_array()),
315                ] {
316                    assert!(
317                        distance(got, want) < 1e-12,
318                        "{got:?} against {want:?} at ({u:?}, {v:?})"
319                    );
320                }
321            }
322        }
323    }
324
325    #[test]
326    fn surface_midpoints_project_onto_the_sphere() {
327        let midpoints = SurfaceMidpoints::of(&octant());
328        let (u0, u1) = midpoints.domain_u();
329        let (v0, v1) = midpoints.domain_v();
330        let target = [0.9, 0.5, 0.4];
331        let (u, v) = midpoints.project(target, (u0 + u1) / 2.0, (v0 + v1) / 2.0);
332        // How far the target lies from the sphere is what the foot point
333        // measures; where along the sphere it is, `f64` pins down only to
334        // the square root of a rounding, as the distance hardly changes.
335        let r = (0.81f64 + 0.25 + 0.16).sqrt();
336        let foot = midpoints.point(u, v);
337        assert!((distance(foot, target) - (r - 1.0)).abs() < 1e-12);
338        assert!(distance(foot, target.map(|c| c / r)) < 1e-7);
339    }
340
341    /// From a seed at the centre of a revolved disc, where every grid point
342    /// of a row lies, a point on another meridian is found: leaving the
343    /// centre along the seed's own meridian leads away from it.
344    #[test]
345    fn surface_midpoints_leave_a_pole_along_the_right_meridian() {
346        let profile = NurbCurve::try_new(
347            1,
348            vec![
349                Vector4::from_array([0.0, 0.0, 0.0, 1.0].map(Scalar::from_f64)),
350                Vector4::from_array([1.0, 0.0, 0.0, 1.0].map(Scalar::from_f64)),
351            ],
352            [0.0, 0.0, 1.0, 1.0].map(Scalar::from_f64).to_vec(),
353        )
354        .unwrap();
355        let axis = Axis::try_new(v3([0.0, 0.0, 0.0]), v3([0.0, 0.0, 1.0])).unwrap();
356        let disc = NurbSurface3D::revolve(&profile, &axis, 0.0, std::f64::consts::PI).unwrap();
357        let midpoints = SurfaceMidpoints::of(&disc);
358        let target = midpoints.point(0.95, 0.04);
359        let (u, v) = midpoints.project(target, 0.0, 0.0);
360        assert!(
361            distance(midpoints.point(u, v), target) < 1e-12,
362            "found ({u}, {v})"
363        );
364    }
365}
geop_ops_step/import/midpoints.rs

geop_ops_step/import/
midpoints.rs
