axiolid_evaluate/bound/
chain_positions.rs

1//! Certified positions of an arc-length chain (#438).
2//!
3//! The evaluator reads a chain ([`Chain2`]) piece by piece: an intrinsic
4//! piece by quadrature, a parametric piece at the parameter of an
5//! arc-length inverse whose `1e-12` tolerance is an estimate, and every
6//! piece is placed at the end of the one before it, so each error is carried
7//! on. [`ChainPositionTable`] reads the *exact* chain with a certified bound
8//! on the distance of every point, tangent and curvature vector it returns,
9//! so a chord bound can charge the evaluated ends against it
10//! ([`chain_chord_bound`](crate::chain::chain_chord_bound), #414's rule for
11//! chains).
12//!
13//! # The exact chain
14//!
15//! It is the representation's chain, nothing is re-fitted:
16//! `placement_0` is the identity and `placement_{k+1} = placement_k o
17//! (q_k(L_k), unit q_k'(L_k))`, with `q_k` the exact local curve of piece
18//! `k`: the integral of `e^(i theta)` for an intrinsic piece, `c(u*(m))`
19//! with `lambda(u*) = m` measured from `start` for a parametric one. A
20//! parametric curve may start up to `PIECE_FRAME_TOLERANCE max(1, L)` off its
21//! local origin, so the exact chain may jump by that at a join: a read
22//! carries a [`SeamSide`], and `Incoming` at a join reads the piece before
23//! it at its end.
24//!
25//! # A local read
26//!
27//! A piece at local distance `m` gives a point `p~` within `e` of the exact
28//! `p*`, a unit tangent `t~` within `phi` and a curvature vector `w~` (`p''`
29//! in arc length) within `omega`:
30//!
31//! - **Intrinsic.** [`IntrinsicPositionTable`] of the piece in its identity
32//!   frame, built at the piece's share of the budget; the tangent is the
33//!   closed-form heading, off by its `heading_rounding`; `w~ = k(m)
34//!   perp(t~)` with `k` the law's value, off by `K (Phi_k + rho)`, `K` the
35//!   law's supremum over the piece and `Phi_k` the turn error of the
36//!   placement.
37//! - **Line.** `u = start + m / |d|`: a distance off by `delta = |(u -
38//!   start) |d| - m|` plus a few roundings, which moves a point of a unit
39//!   speed curve by at most `delta`. Straight: no curvature.
40//! - **Circle, ellipse, B-spline.** The parameter is *enclosed*, not
41//!   trusted: [`CurveLengths`](crate::station::CurveLengths) bounds
42//!   `lambda(u)` between `lo` and `hi` by chord panels, the evaluator's
43//!   inverse is only a guess, and a guess whose enclosure misses `m` is
44//!   bisected until it holds `m` (a piece longer than its curve is `None`).
45//!   `delta = max(m - lo, hi - m, 0)` bounds `|lambda(u) - m|`, which is the
46//!   arc between `u` and the exact `u*`, which bounds `|c(u) - c(u*)|` and
47//!   turns the unit tangent by at most `K delta`, `K` the curvature supremum
48//!   over the panels the piece reaches; the curvature vector moves by `K'
49//!   delta`, `K'` bounding `|p'''|`. A read whose `delta` exceeds the
50//!   piece's share is `None`, so `max_error` is a true supremum by
51//!   refusal.
52//!
53//! # Carried errors
54//!
55//! With `E_k` the position error and `Phi_k` the turn error of piece `k`'s
56//! placement (`E_0 = Phi_0 = 0`): a point `P = O_k + X_k p~` is off by `E_k +
57//! e + Phi_k (|p~| + e)` plus the rounding of the sum, because `|R(X) p~ -
58//! R(X*) p*| <= |p~ - p*| + |X - X*| |p*|`; and the next placement carries
59//! `E_{k+1} = E_k + e_end + Phi_k (|p~_end| + e_end) + 8 eps (|O_k| +
60//! |p~_end|)`, `Phi_{k+1} = Phi_k + phi_end + 8 eps`, the angles of
61//! successive turns adding. The start frame maps all of it, stretching by at
62//! most [`frame_stretch2`]. Each piece's share of the budget is `error / (2
63//! sigma n)`, `n` the pieces up to the end, so the sum of the pieces'
64//! own errors is half the request; what the carried turns add on top is
65//! rounding.
66//!
67//! `max_error`, `max_tangent_error` and `max_curvature_error` are suprema
68//! over every read: a read never reports more (it refuses first).
69
70use axiolid_core::{Frame2, Point2, Scalar, Vec2};
71use axiolid_curve::{Chain2, ChainPiece2, Curve2, Intrinsic2};
72
73use super::interval::Iv;
74use super::{frame_stretch2, inflate, IntrinsicPositionTable};
75use crate::arc_length::intrinsic_tangent;
76use crate::arc_parameter::{parameter_at_arc_length2, ARC_LENGTH_TOLERANCE};
77use crate::chain::{
78    check_local_start, check_well_formed, curvature_bound, local_frame, parametric_bend,
79    parametric_plan_jet, unit, Placement,
80};
81use crate::curve::{derivative2, domain2, evaluate2};
82use crate::station::{CurveLengths, POINT_ROUNDING};
83
84/// The relative rounding of a computed derivative against the curve's
85/// derivative bound, as [`POINT_ROUNDING`] is against coordinates.
86const DERIVATIVE_ROUNDING: Scalar = 1e-14;
87
88/// Most halvings the search for the enclosed parameter makes.
89const BISECTIONS: usize = 128;
90
91/// The rounding of one placement step, in units of epsilon.
92const PLACE: Scalar = 8.0 * Scalar::EPSILON;
93
94/// Which piece a read at a join belongs to.
95#[derive(Debug, Clone, Copy, PartialEq, Eq)]
96pub enum SeamSide {
97    /// The piece that starts at the join (what the evaluator reads).
98    Outgoing,
99    /// The piece that ends at the join: its end, before any jump a
100    /// parametric piece's start makes.
101    Incoming,
102}
103
104/// One local read of a piece.
105#[derive(Debug, Clone, Copy)]
106struct Local {
107    p: Vec2,
108    e: Scalar,
109    t: Vec2,
110    phi: Scalar,
111    w: Vec2,
112    /// The curvature vector's error before the placement's turn is added.
113    omega: Scalar,
114}
115
116/// How a piece is read.
117#[derive(Debug, Clone)]
118enum Kind {
119    Intrinsic {
120        local: Intrinsic2,
121        table: IntrinsicPositionTable,
122    },
123    Parametric {
124        curve: Curve2,
125        start: Scalar,
126        /// The parameter the evaluator reads at the piece end.
127        end: Scalar,
128        /// A line's `|d|`; `None` for a curve measured by its panels.
129        rate: Option<Scalar>,
130        lengths: Option<CurveLengths>,
131        high: Scalar,
132        /// `K'`, bounding `|p'''|`.
133        jerk: Scalar,
134        /// A parameter magnitude the line's rounding is relative to.
135        reach: Scalar,
136    },
137}
138
139/// A piece with its placement and the errors carried into it.
140#[derive(Debug, Clone)]
141struct Placed {
142    length: Scalar,
143    placement: Placement,
144    position: Scalar,
145    turn: Scalar,
146    share: Scalar,
147    /// `K`, bounding `|p''|` (the curvature) over the piece.
148    curvature: Scalar,
149    /// The tangent's rounding.
150    rho: Scalar,
151    /// The largest error of a local point.
152    emax: Scalar,
153    kind: Kind,
154}
155
156/// Certified positions, unit tangents and curvature vectors of an
157/// arc-length chain over `[0, end]`, read in any order; see the module
158/// documentation.
159#[derive(Debug, Clone)]
160pub struct ChainPositionTable {
161    start: Frame2,
162    stretch: Scalar,
163    total: Scalar,
164    end: Scalar,
165    /// Where each built piece starts, summed as `Chain2::piece_at` sums.
166    begins: Vec<Scalar>,
167    pieces: Vec<Placed>,
168    max_point: Scalar,
169    max_tangent: Scalar,
170    max_curvature: Scalar,
171}
172
173/// The world read of a piece at a local distance.
174struct Read {
175    point: Point2,
176    point_error: Scalar,
177    tangent: Vec2,
178    tangent_error: Scalar,
179    curvature: Vec2,
180    curvature_error: Scalar,
181}
182
183impl ChainPositionTable {
184    /// The table over `[0, end]`, its points within about `error`; `None`
185    /// for a malformed chain (or a parametric piece off its local frame), an
186    /// error or end that is not finite and positive, an end past the chain,
187    /// an intrinsic piece whose law has no closed-form bound of `k` to
188    /// `k'''`, a parametric piece of a family other than a line, a circle,
189    /// an ellipse or a B-spline, a piece longer than its curve, a curve
190    /// without a positive speed floor on a panel, or a budget exhausted.
191    #[must_use]
192    pub fn new(chain: &Chain2, error: Scalar, end: Scalar) -> Option<Self> {
193        if !(error.is_finite() && error > 0.0 && end.is_finite() && end > 0.0) {
194            return None;
195        }
196        check_well_formed(chain).ok()?;
197        let total = chain.length()?;
198        if end > total + ARC_LENGTH_TOLERANCE * total.max(1.0) {
199            return None;
200        }
201        let end = end.min(total);
202        let stretch = frame_stretch2(&chain.start);
203        let mut begins = vec![0.0];
204        for piece in &chain.pieces {
205            let last = *begins.last()?;
206            begins.push(last + piece.length());
207        }
208        let built = begins[..chain.pieces.len()]
209            .iter()
210            .take_while(|&&begin| begin <= end)
211            .count()
212            .max(1);
213        begins.truncate(built);
214        let budget = 0.5 * error / (stretch * built as Scalar);
215        let mut pieces: Vec<Placed> = Vec::with_capacity(built);
216        let mut placement = Placement::IDENTITY;
217        let (mut position, mut turn) = (0.0, 0.0);
218        for (index, piece) in chain.pieces[..built].iter().enumerate() {
219            let placed = Placed::new(index, piece, budget, placement, position, turn)?;
220            // The end of the piece: where the next one is placed.
221            let local = placed.read(placed.length)?;
222            let end_turn = turn + local.phi + PLACE;
223            position = position
224                + local.e
225                + turn * (local.p.length() + local.e)
226                + PLACE * (placement.origin.abs().max_element() + local.p.length());
227            turn = end_turn;
228            placement = Placement {
229                origin: placement.place(local.p),
230                x: unit(placement.turn(local.t), "chain join").ok()?,
231            };
232            pieces.push(placed);
233        }
234        let mut table = Self {
235            start: chain.start,
236            stretch,
237            total,
238            end,
239            begins,
240            pieces,
241            max_point: 0.0,
242            max_tangent: 0.0,
243            max_curvature: 0.0,
244        };
245        table.bound_suprema();
246        let finite = table.max_point.is_finite()
247            && table.max_tangent.is_finite()
248            && table.max_curvature.is_finite();
249        finite.then_some(table)
250    }
251
252    /// The suprema of the three errors over every read.
253    fn bound_suprema(&mut self) {
254        let origin = self.start.origin.abs().max_element();
255        let mut worst = (0.0, 0.0, 0.0);
256        for piece in &self.pieces {
257            let size = piece.length * (1.0 + PIECE_SLACK) + PIECE_SLACK + piece.emax;
258            let corner = piece.placement.origin.abs().max_element();
259            let local = piece.position
260                + piece.emax
261                + piece.turn * (size + piece.emax)
262                + PLACE * (corner + size);
263            let placed = (piece.placement.origin.length() + size).max(1.0);
264            let point = self.stretch * local + PLACE * (origin + self.stretch * placed);
265            let tangent = self.stretch * (piece.turn + piece.phi_max() + PLACE);
266            let omega = piece.omega_max() + piece.curvature * piece.turn;
267            let curvature = self.stretch * (omega + PLACE * piece.curvature);
268            worst.0 = Scalar::max(worst.0, point);
269            worst.1 = Scalar::max(worst.1, tangent);
270            worst.2 = Scalar::max(worst.2, curvature);
271        }
272        self.max_point = inflate(worst.0);
273        self.max_tangent = inflate(worst.1);
274        self.max_curvature = inflate(worst.2);
275    }
276
277    /// The piece holding `s` on `side`, and the local distance in it.
278    fn locate(&self, s: Scalar, side: SeamSide) -> Option<(&Placed, Scalar)> {
279        let slack = ARC_LENGTH_TOLERANCE * self.total.max(1.0);
280        // A distance a rounding step past the end is the end, as the
281        // evaluator reads it.
282        let s = if s > self.total && s <= self.total + slack {
283            self.total
284        } else {
285            s
286        };
287        if !(s.is_finite() && s >= 0.0 && s <= self.end) {
288            return None;
289        }
290        let index = self
291            .begins
292            .partition_point(|&begin| begin <= s)
293            .saturating_sub(1)
294            .min(self.pieces.len() - 1);
295        if side == SeamSide::Incoming && index > 0 && s == self.begins[index] {
296            let piece = &self.pieces[index - 1];
297            return Some((piece, piece.length));
298        }
299        let piece = &self.pieces[index];
300        Some((piece, (s - self.begins[index]).clamp(0.0, piece.length)))
301    }
302
303    fn read(&self, s: Scalar, side: SeamSide) -> Option<Read> {
304        let (piece, m) = self.locate(s, side)?;
305        let local = piece.read(m)?;
306        let origin = self.start.origin.abs().max_element();
307        let turned = piece.placement.turn(local.p);
308        let p = piece.placement.origin + turned;
309        let size = local.p.length();
310        let at = piece.position
311            + local.e
312            + piece.turn * (size + local.e)
313            + PLACE * (piece.placement.origin.abs().max_element() + size);
314        let world = |v: Vec2| {
315            let x = self.start.x * v.x + self.start.y * v.y;
316            Vec2::new(x.x, x.y)
317        };
318        let w = world(p);
319        let point_error =
320            inflate(self.stretch * at + PLACE * (origin + self.stretch * p.length().max(1.0)));
321        let omega = local.omega + piece.curvature * piece.turn;
322        let out = Read {
323            point: Point2::new(self.start.origin.x + w.x, self.start.origin.y + w.y),
324            point_error,
325            tangent: world(piece.placement.turn(local.t)),
326            tangent_error: inflate(self.stretch * (piece.turn + local.phi + PLACE)),
327            curvature: world(piece.placement.turn(local.w)),
328            curvature_error: inflate(self.stretch * (omega + PLACE * piece.curvature)),
329        };
330        (point_error <= self.max_point
331            && out.tangent_error <= self.max_tangent
332            && out.curvature_error <= self.max_curvature)
333            .then_some(out)
334    }
335
336    /// The point at arc length `s` as the evaluator reads it (a join belongs
337    /// to the piece that starts there), and a certified bound on its
338    /// distance from the exact point. `None` outside `[0, end]`, or where a
339    /// piece refuses the read.
340    #[must_use]
341    pub fn point2(&self, s: Scalar) -> Option<(Point2, Scalar)> {
342        self.point2_on(s, SeamSide::Outgoing)
343    }
344
345    /// [`Self::point2`] on `side` of a join.
346    #[must_use]
347    pub fn point2_on(&self, s: Scalar, side: SeamSide) -> Option<(Point2, Scalar)> {
348        self.read(s, side).map(|r| (r.point, r.point_error))
349    }
350
351    /// The tangent at `s` on `side`, and a certified bound on its distance
352    /// from the exact tangent.
353    #[must_use]
354    pub fn tangent2_on(&self, s: Scalar, side: SeamSide) -> Option<(Vec2, Scalar)> {
355        self.read(s, side).map(|r| (r.tangent, r.tangent_error))
356    }
357
358    /// `p''(s)` in arc length (`k n`) on `side`, and a certified bound on its
359    /// distance from the exact one. Where a piece's curvature law has a seam
360    /// inside, the value is the left one, as the evaluator reads it.
361    #[must_use]
362    pub fn curvature2_on(&self, s: Scalar, side: SeamSide) -> Option<(Vec2, Scalar)> {
363        self.read(s, side).map(|r| (r.curvature, r.curvature_error))
364    }
365
366    /// A certified bound on the error of every point the table returns.
367    #[must_use]
368    pub fn max_error(&self) -> Scalar {
369        self.max_point
370    }
371
372    /// A certified bound on the error of every tangent the table returns.
373    #[must_use]
374    pub fn max_tangent_error(&self) -> Scalar {
375        self.max_tangent
376    }
377
378    /// A certified bound on the error of every curvature vector the table
379    /// returns.
380    #[must_use]
381    pub fn max_curvature_error(&self) -> Scalar {
382        self.max_curvature
383    }
384}
385
386/// Certified bound on the distance from a chain over `[a, b]` (inside one
387/// piece) to the chord of its evaluated ends; see
388/// [`chain_chord_bound`](crate::chain::chain_chord_bound).
389///
390/// The exact-chord bound plus the larger distance of an evaluated end from
391/// the table's point (and that point's error): the lower end read as the
392/// piece that starts there, the upper end as the piece that ends there. The
393/// table is built at an eighth of the exact bound, or a few ulps of the
394/// placement if more.
395pub(crate) fn evaluated_chain_chord_bound(chain: &Chain2, a: Scalar, b: Scalar) -> Option<Scalar> {
396    let (lo, hi) = super::ordered(a, b);
397    let law = crate::chain::exact_chain_chord_bound(chain, lo, hi)?;
398    let total = chain.length()?;
399    let stretch = frame_stretch2(&chain.start);
400    let origin = chain.start.origin.abs().max_element();
401    let floor = 64.0 * Scalar::EPSILON * (1.0 + origin + stretch * hi);
402    let table = ChainPositionTable::new(chain, (law / 8.0).max(floor), hi.min(total))?;
403    let gap = |s: Scalar, side: SeamSide| {
404        let (exact, error) = table.point2_on(s.min(total), side)?;
405        let read = crate::chain::chain_point(chain, s).ok()?;
406        let d = [read.x - exact.x, read.y - exact.y]
407            .map(|c| Iv::point(c).sq())
408            .into_iter()
409            .fold(Iv::point(0.0), Iv::add)
410            .sqrt();
411        Some(Iv::point(d.hi).add(Iv::point(error)).hi)
412    };
413    let g = gap(lo, SeamSide::Outgoing)?.max(gap(hi, SeamSide::Incoming)?);
414    let out = inflate(Iv::point(law).add(Iv::point(g)).hi);
415    out.is_finite().then_some(out)
416}
417
418/// How far a parametric piece's curve may sit from its local origin, per
419/// `max(1, length)`: [`crate::chain::PIECE_FRAME_TOLERANCE`].
420const PIECE_SLACK: Scalar = crate::chain::PIECE_FRAME_TOLERANCE;
421
422enum Side {
423    Below,
424    Inside(Scalar, Scalar),
425    Above,
426}
427
428impl Placed {
429    fn new(
430        index: usize,
431        piece: &ChainPiece2,
432        budget: Scalar,
433        placement: Placement,
434        position: Scalar,
435        turn: Scalar,
436    ) -> Option<Self> {
437        let length = piece.length();
438        match piece {
439            ChainPiece2::Intrinsic { curvature, .. } => {
440                let local = Intrinsic2::new(local_frame(), curvature.clone(), length);
441                let table = IntrinsicPositionTable::new2(&local, budget, length)?;
442                let k = curvature_bound(curvature, 0.0, length)?;
443                Some(Self {
444                    length,
445                    placement,
446                    position,
447                    turn,
448                    share: budget,
449                    curvature: k,
450                    rho: table.heading_rounding(),
451                    emax: table.max_error(),
452                    kind: Kind::Intrinsic { local, table },
453                })
454            }
455            ChainPiece2::Parametric {
456                curve,
457                start,
458                length,
459            } => {
460                check_local_start(index, curve, *start, *length).ok()?;
461                // At least what the evaluator's own inverse is good for.
462                let share = budget.max(64.0 * ARC_LENGTH_TOLERANCE * length.max(1.0));
463                let end = parameter_at_arc_length2(curve, *start, *length).ok()?;
464                match curve {
465                    Curve2::Line(line) => {
466                        let rate = line.direction.length();
467                        if !(rate > 0.0 && rate.is_finite()) {
468                            return None;
469                        }
470                        let o = line.origin.abs().max_element();
471                        let d = line.direction.abs().max_element();
472                        let reach = (o + d * (start.abs() + end.abs())).max(1.0);
473                        Some(Self {
474                            length: *length,
475                            placement,
476                            position,
477                            turn,
478                            share,
479                            curvature: 0.0,
480                            rho: 2.0 * DERIVATIVE_ROUNDING,
481                            emax: share + POINT_ROUNDING * reach,
482                            kind: Kind::Parametric {
483                                curve: curve.clone(),
484                                start: *start,
485                                end,
486                                rate: Some(rate),
487                                lengths: None,
488                                high: end,
489                                jerk: 0.0,
490                                reach,
491                            },
492                        })
493                    }
494                    Curve2::Circle(_) | Curve2::Ellipse(_) | Curve2::BSpline(_) => {
495                        let high = match curve {
496                            Curve2::BSpline(_) => {
497                                let domain = domain2(curve);
498                                domain.start.max(domain.end)
499                            }
500                            _ => start + core::f64::consts::TAU,
501                        };
502                        if high.partial_cmp(start) != Some(core::cmp::Ordering::Greater) {
503                            return None;
504                        }
505                        let lengths = CurveLengths::new(curve, *start, high, 0.25 * share).ok()?;
506                        // Even its least length falls short of the piece:
507                        // the piece is longer than its curve.
508                        if lengths.total().1 < *length {
509                            return None;
510                        }
511                        let (mut k, mut jerk, mut speed, mut first): (
512                            Scalar,
513                            Scalar,
514                            Scalar,
515                            Scalar,
516                        ) = (0.0, 0.0, Scalar::INFINITY, 0.0);
517                        for (a, b) in lengths.panels() {
518                            if lengths.at(a).ok()?.0 > *length + share {
519                                break;
520                            }
521                            let (p2, p3, sigma) = parametric_plan_jet(curve, a, b)?;
522                            let bounds = crate::bound::curve_derivative_bounds2(curve, a, b)?;
523                            k = k.max(p2);
524                            jerk = jerk.max(p3);
525                            speed = speed.min(sigma);
526                            first = first.max(bounds.first);
527                        }
528                        if !(k.is_finite() && jerk.is_finite() && speed > 0.0 && first > 0.0) {
529                            return None;
530                        }
531                        let rho = 2.0 * DERIVATIVE_ROUNDING * first / speed;
532                        let extent = lengths.extent();
533                        Some(Self {
534                            length: *length,
535                            placement,
536                            position,
537                            turn,
538                            share,
539                            curvature: k,
540                            rho,
541                            emax: share + POINT_ROUNDING * extent,
542                            kind: Kind::Parametric {
543                                curve: curve.clone(),
544                                start: *start,
545                                end,
546                                rate: None,
547                                lengths: Some(lengths),
548                                high,
549                                jerk,
550                                reach: extent,
551                            },
552                        })
553                    }
554                    _ => None,
555                }
556            }
557            _ => None,
558        }
559    }
560
561    /// The tangent's largest error in a local read.
562    fn phi_max(&self) -> Scalar {
563        match &self.kind {
564            Kind::Intrinsic { .. } => self.rho,
565            Kind::Parametric { rate: Some(_), .. } => self.rho,
566            Kind::Parametric { .. } => self.curvature * self.share + self.rho,
567        }
568    }
569
570    /// The curvature vector's largest error in a local read, before the
571    /// placement's turn.
572    fn omega_max(&self) -> Scalar {
573        match &self.kind {
574            Kind::Intrinsic { .. } => self.curvature * self.rho,
575            Kind::Parametric { rate: Some(_), .. } => 0.0,
576            Kind::Parametric { jerk, .. } => {
577                self.curvature * self.rho
578                    + jerk * self.share
579                    + 4.0 * DERIVATIVE_ROUNDING * self.curvature
580            }
581        }
582    }
583
584    /// The local read at distance `m` of the piece.
585    fn read(&self, m: Scalar) -> Option<Local> {
586        if !(m.is_finite() && (0.0..=self.length).contains(&m)) {
587            return None;
588        }
589        match &self.kind {
590            Kind::Intrinsic { local, table } => {
591                let (p, e) = table.point2(m)?;
592                let t = intrinsic_tangent(local, m).ok()?;
593                let kappa = crate::frenet::law_value(&local.curvature, m)?;
594                let out = Local {
595                    p: Vec2::new(p.x, p.y),
596                    e,
597                    t,
598                    phi: self.rho,
599                    w: Vec2::new(-t.y, t.x) * kappa,
600                    omega: self.curvature * self.rho,
601                };
602                (e <= self.emax && out.w.is_finite()).then_some(out)
603            }
604            Kind::Parametric {
605                curve,
606                start,
607                end,
608                rate,
609                lengths,
610                high,
611                reach,
612                ..
613            } => {
614                let (u, delta) = match (rate, lengths) {
615                    (Some(rate), _) => {
616                        let u = if m == self.length {
617                            *end
618                        } else {
619                            start + m / rate
620                        };
621                        let delta = ((u - start) * rate - m).abs()
622                            + 4.0 * Scalar::EPSILON * (rate * (u.abs() + start.abs()) + m);
623                        (u, delta)
624                    }
625                    (None, Some(lengths)) => {
626                        let guess = if m == self.length {
627                            *end
628                        } else {
629                            parameter_at_arc_length2(curve, *start, m).ok()?
630                        };
631                        solve(lengths, *start, *high, m, guess)?
632                    }
633                    (None, None) => return None,
634                };
635                if !(delta.is_finite() && delta <= self.share && u.is_finite()) {
636                    return None;
637                }
638                let p = evaluate2(curve, u).ok()?;
639                let t = unit(derivative2(curve, u).ok()?, "chain piece curve").ok()?;
640                let rounding = POINT_ROUNDING * reach;
641                let e = delta + rounding;
642                let (w, omega) = if rate.is_some() {
643                    (Vec2::ZERO, 0.0)
644                } else {
645                    (
646                        parametric_bend(curve, u).ok()?,
647                        self.curvature * self.rho
648                            + self.jerk() * delta
649                            + 4.0 * DERIVATIVE_ROUNDING * self.curvature,
650                    )
651                };
652                let out = Local {
653                    p: Vec2::new(p.x, p.y),
654                    e,
655                    t,
656                    phi: if rate.is_some() {
657                        self.rho
658                    } else {
659                        self.curvature * delta + self.rho
660                    },
661                    w,
662                    omega,
663                };
664                (e <= self.emax && out.p.is_finite() && out.w.is_finite()).then_some(out)
665            }
666        }
667    }
668
669    fn jerk(&self) -> Scalar {
670        match &self.kind {
671            Kind::Parametric { jerk, .. } => *jerk,
672            Kind::Intrinsic { .. } => 0.0,
673        }
674    }
675}
676
677/// The parameter `u` with `lambda(u)` enclosing `m`, and the width `delta`
678/// of the enclosure about it; `None` where the piece is longer than the
679/// curve or no enclosure is found.
680///
681/// `guess` is the evaluator's parameter, tried first and never trusted.
682fn solve(
683    lengths: &CurveLengths,
684    start: Scalar,
685    high: Scalar,
686    m: Scalar,
687    guess: Scalar,
688) -> Option<(Scalar, Scalar)> {
689    let classify = |u: Scalar| -> Option<Side> {
690        let (lo, hi) = lengths.at(u).ok()?;
691        Some(if hi < m {
692            Side::Below
693        } else if lo > m {
694            Side::Above
695        } else {
696            Side::Inside(lo, hi)
697        })
698    };
699    let inside = |lo: Scalar, hi: Scalar| (m - lo).max(hi - m).max(0.0);
700    let guess = guess.clamp(start, high);
701    let (mut below, mut above) = match classify(guess)? {
702        Side::Inside(lo, hi) => return Some((guess, inside(lo, hi))),
703        Side::Below => match classify(high)? {
704            Side::Below => return None,
705            Side::Inside(lo, hi) => return Some((high, inside(lo, hi))),
706            Side::Above => (guess, high),
707        },
708        Side::Above => match classify(start)? {
709            Side::Above => return None,
710            Side::Inside(lo, hi) => return Some((start, inside(lo, hi))),
711            Side::Below => (start, guess),
712        },
713    };
714    for _ in 0..BISECTIONS {
715        let mid = 0.5 * (below + above);
716        if !(mid > below && mid < above) {
717            return None;
718        }
719        match classify(mid)? {
720            Side::Inside(lo, hi) => return Some((mid, inside(lo, hi))),
721            Side::Below => below = mid,
722            Side::Above => above = mid,
723        }
724    }
725    None
726}
727
728#[cfg(test)]
729mod tests {
730    use super::*;
731    use axiolid_core::Point2;
732    use axiolid_curve::{BSplineCurve2, KnotSpec};
733
734    /// The cubic `y = x^3 / (6 R X)` of radius 300 over 60 m, as a Bezier.
735    fn cubic() -> Curve2 {
736        let (reach, a) = (60.0, 1.0 / (6.0 * 300.0 * 60.0));
737        Curve2::BSpline(BSplineCurve2 {
738            degree: 3,
739            control_points: vec![
740                Point2::new(0.0, 0.0),
741                Point2::new(reach / 3.0, 0.0),
742                Point2::new(2.0 * reach / 3.0, 0.0),
743                Point2::new(reach, a * reach * reach * reach),
744            ],
745            knots: vec![0.0, 1.0],
746            multiplicities: vec![4, 4],
747            weights: None,
748            closed: false,
749            self_intersect: None,
750            knot_spec: KnotSpec::PiecewiseBezier,
751        })
752    }
753
754    #[test]
755    fn a_parametric_read_off_its_distance_is_charged() {
756        // A coarse enclosure holds both the guess's distance and `m`, so the
757        // read is charged at least the gap between them.
758        let curve = cubic();
759        let lengths = CurveLengths::new(&curve, 0.0, 1.0, 1e-3).unwrap();
760        let m = 30.0;
761        let off = parameter_at_arc_length2(&curve, 0.0, m + 1e-6).unwrap();
762        let (u, delta) = solve(&lengths, 0.0, 1.0, m, off).unwrap();
763        assert_eq!(u, off);
764        assert!(delta >= 1e-6 - 1e-9, "{delta}");
765        let (lo, hi) = lengths.at(u).unwrap();
766        assert!(lo <= m && m <= hi);
767    }
768
769    #[test]
770    fn a_bad_guess_is_bisected() {
771        let curve = cubic();
772        let lengths = CurveLengths::new(&curve, 0.0, 1.0, 1e-9).unwrap();
773        let m = 30.0;
774        for far in [m + 1e-3, m + 10.0, m - 10.0, 59.0, 0.5] {
775            let guess = parameter_at_arc_length2(&curve, 0.0, far).unwrap();
776            let (u, delta) = solve(&lengths, 0.0, 1.0, m, guess).unwrap();
777            let (lo, hi) = lengths.at(u).unwrap();
778            assert!(lo <= m && m <= hi, "guess for {far}");
779            assert!(delta <= 1e-9, "{delta}");
780        }
781        // Past the curve: no parameter has that length.
782        assert!(solve(&lengths, 0.0, 1.0, 61.0, 1.0).is_none());
783    }
784}
axiolid_evaluate/bound/chain_positions.rs

axiolid_evaluate/bound/
chain_positions.rs
