axiolid_evaluate/station/
path_bound.rs

1//! Curve-path piece bounds given as a native parameter or a point (#304,
2//! ADR 0082 amendment).
3//!
4//! A [`PathBound`] names where a piece starts or ends without stating its
5//! distance in the curve's station measure. A parameter is read as it is;
6//! the distance to it is [`StationCurve`]'s own measure at that parameter,
7//! the one [`super::StationPiece::between_parameters`] measures by, so a
8//! bounded piece resolves to the piece between those parameters bitwise.
9//! This module resolves a POINT to the one parameter naming it.
10//!
11//! # Points
12//!
13//! A point is given in the curve's own coordinates, before any placement,
14//! and lifted to `z = 0` on a 2D curve. It must lie on the curve within the
15//! joint tolerance, [`super::JOINT_TOLERANCE`]` * max(1, |p|)`, `|p|` the
16//! point's largest coordinate magnitude, the tolerance within which two
17//! pieces meet and within which a graph compiler accepts a trim point:
18//!
19//! - A line, a circle or an ellipse is inverted in closed form (the
20//!   parameter `axiolid-evaluate`'s `invert2`/`invert3` reports, a closed
21//!   conic's angle in `[0, 2 pi)`), then checked against the curve. The
22//!   angle runs round, so a closed conic's seam is one parameter, not two.
23//! - A polyline is projected onto each segment; a B-spline is sampled
24//!   (64 samples per control point, and every knot), each local minimum
25//!   of the distance refined by Newton on `|C(t) - p|^2` within its
26//!   neighbouring samples. Every parameter that comes within tolerance is
27//!   kept.
28//! - Two parameters name the same place when the curve between them stays
29//!   within tolerance of the point (every vertex between them on a
30//!   polyline; every sample between them and their midpoint on a
31//!   B-spline): one place is one parameter, the nearest of them.
32//!
33//! No parameter within tolerance is refused naming [`PATH_POINT_OFF_CURVE`]
34//! (the point is never projected onto the curve); several places naming
35//! [`PATH_POINT_AMBIGUOUS`]: a curve passing through the point more than
36//! once, and the seam of a closed polyline or B-spline, whose two domain
37//! ends both name it. Every other family (an intrinsic curve, a chain, an
38//! elevated or banked curve, an offset) is refused naming
39//! [`POINT_BOUND_UNSUPPORTED`]; a caller states its parameter instead.
40
41use axiolid_contracts::{BackendId, GeomError, GeomResult, Operation};
42use axiolid_core::{Point2, Point3, Scalar, Tolerance, Vec3};
43use axiolid_curve::{Curve2, Curve3, PathBound};
44pub use axiolid_curve_evaluate_contract::{PATH_POINT_AMBIGUOUS, PATH_POINT_OFF_CURVE};
45
46use super::composite::{StationCurve, JOINT_TOLERANCE};
47use crate::curve::{
48    derivative2, derivative3, domain2, domain3, evaluate2, evaluate3, invert2, invert3,
49    second_derivative2, second_derivative3,
50};
51
52/// The input a point bound on a curve this evaluator does not invert is
53/// refused for.
54pub const POINT_BOUND_UNSUPPORTED: &str =
55    "a curve path piece bounded by a point on a curve this evaluator does not invert (an \
56     intrinsic curve, a chain, an elevated or banked curve, an offset, or a family other than a \
57     line, a circle, an ellipse, a polyline or a B-spline): state its parameter instead";
58
59/// Samples per control point a B-spline is scanned at for a point.
60const SAMPLES_PER_CONTROL_POINT: usize = 64;
61
62/// Newton steps refining one candidate parameter.
63const NEWTON_STEPS: usize = 60;
64
65fn refused(input: &'static str) -> GeomError {
66    GeomError::UnsupportedInput {
67        backend: BackendId::new("axiolid-evaluate"),
68        operation: Operation::CurveEvaluation,
69        input,
70    }
71}
72
73/// How far from its curve a point bound may lie: the joint tolerance at
74/// the point.
75#[must_use]
76pub fn point_bound_tolerance(point: Point3) -> Scalar {
77    JOINT_TOLERANCE * point.abs().max_element().max(1.0)
78}
79
80/// The native parameter `bound` names on `curve`: a parameter as it is
81/// (finite), a point resolved by the
82/// [module documentation](self#points)'s rule.
83///
84/// # Errors
85///
86/// A parameter that is not finite, a point that is not finite, off the
87/// curve ([`PATH_POINT_OFF_CURVE`]), named by several parameters
88/// ([`PATH_POINT_AMBIGUOUS`]) or on a curve this evaluator does not invert
89/// ([`POINT_BOUND_UNSUPPORTED`]), and a bound of a kind it does not know,
90/// by name.
91pub(super) fn bound_parameter(curve: StationCurve<'_>, bound: PathBound) -> GeomResult<Scalar> {
92    match bound {
93        PathBound::Parameter(t) if t.is_finite() => Ok(t),
94        PathBound::Parameter(t) => Err(GeomError::Degenerate(format!(
95            "station: a curve path piece is bounded at parameter {t}, which is not finite"
96        ))),
97        PathBound::Point(point) => point_parameter(curve, point),
98        _ => Err(refused(
99            "a curve path piece bound of a kind this evaluator does not know",
100        )),
101    }
102}
103
104/// The one parameter of `curve` naming `point`; see the
105/// [module documentation](self#points).
106fn point_parameter(curve: StationCurve<'_>, point: Point3) -> GeomResult<Scalar> {
107    if !(point.x.is_finite() && point.y.is_finite() && point.z.is_finite()) {
108        return Err(GeomError::InvalidInput(format!(
109            "station: a curve path piece is bounded at point {point:?}, which is not finite"
110        )));
111    }
112    let traced = Traced::of(curve)?;
113    let found = traced.parameters(point, point_bound_tolerance(point))?;
114    match found.as_slice() {
115        [] => Err(refused(PATH_POINT_OFF_CURVE)),
116        [t] => Ok(*t),
117        _ => Err(refused(PATH_POINT_AMBIGUOUS)),
118    }
119}
120
121/// A curve whose points this module inverts, 2D lifted to `z = 0`.
122#[derive(Clone, Copy)]
123enum Traced<'c> {
124    Two(&'c Curve2),
125    Three(&'c Curve3),
126}
127
128impl<'c> Traced<'c> {
129    fn of(curve: StationCurve<'c>) -> GeomResult<Self> {
130        match curve {
131            StationCurve::Two(
132                curve @ (Curve2::Line(_)
133                | Curve2::Circle(_)
134                | Curve2::Ellipse(_)
135                | Curve2::Polyline(_)
136                | Curve2::BSpline(_)),
137            ) => Ok(Self::Two(curve)),
138            StationCurve::Three(
139                curve @ (Curve3::Line(_)
140                | Curve3::Circle(_)
141                | Curve3::Ellipse(_)
142                | Curve3::Polyline(_)
143                | Curve3::BSpline(_)),
144            ) => Ok(Self::Three(curve)),
145            _ => Err(refused(POINT_BOUND_UNSUPPORTED)),
146        }
147    }
148
149    fn point(self, t: Scalar) -> GeomResult<Point3> {
150        match self {
151            Self::Two(curve) => {
152                let p = evaluate2(curve, t)?;
153                Ok(Point3::new(p.x, p.y, 0.0))
154            }
155            Self::Three(curve) => evaluate3(curve, t),
156        }
157    }
158
159    fn derivatives(self, t: Scalar) -> GeomResult<(Vec3, Vec3)> {
160        match self {
161            Self::Two(curve) => {
162                let (d, dd) = (derivative2(curve, t)?, second_derivative2(curve, t)?);
163                Ok((Vec3::new(d.x, d.y, 0.0), Vec3::new(dd.x, dd.y, 0.0)))
164            }
165            Self::Three(curve) => Ok((derivative3(curve, t)?, second_derivative3(curve, t)?)),
166        }
167    }
168
169    fn distance(self, t: Scalar, point: Point3) -> GeomResult<Scalar> {
170        Ok((self.point(t)? - point).length())
171    }
172
173    /// Every place of the curve within `tolerance` of `point`, one
174    /// parameter each, in increasing order.
175    fn parameters(self, point: Point3, tolerance: Scalar) -> GeomResult<Vec<Scalar>> {
176        match self {
177            Self::Two(Curve2::Polyline(polyline)) => {
178                let points: Vec<Point3> = polyline
179                    .points
180                    .iter()
181                    .map(|p| Point3::new(p.x, p.y, 0.0))
182                    .collect();
183                Ok(polyline_parameters(
184                    &points,
185                    polyline.closed,
186                    point,
187                    tolerance,
188                ))
189            }
190            Self::Three(Curve3::Polyline(polyline)) => Ok(polyline_parameters(
191                &polyline.points,
192                polyline.closed,
193                point,
194                tolerance,
195            )),
196            Self::Two(Curve2::BSpline(_)) | Self::Three(Curve3::BSpline(_)) => {
197                self.spline_parameters(point, tolerance)
198            }
199            _ => {
200                // Closed form: the parameter the inversion reports, at any
201                // distance, then checked here so that a point off the curve
202                // is refused by name.
203                let anywhere = Tolerance::new(Scalar::MAX, 0.0).map_err(|_| {
204                    GeomError::InvalidInput("station: no inversion tolerance".into())
205                })?;
206                let t = match self {
207                    Self::Two(curve) => invert2(curve, Point2::new(point.x, point.y), anywhere)?,
208                    Self::Three(curve) => invert3(curve, point, anywhere)?,
209                };
210                Ok(if self.distance(t, point)? <= tolerance {
211                    vec![t]
212                } else {
213                    Vec::new()
214                })
215            }
216        }
217    }
218
219    /// A B-spline's places within `tolerance` of `point`: every local
220    /// minimum of the sampled distance, refined between its neighbours.
221    fn spline_parameters(self, point: Point3, tolerance: Scalar) -> GeomResult<Vec<Scalar>> {
222        let (domain, count, knots) = match self {
223            Self::Two(curve @ Curve2::BSpline(spline)) => (
224                domain2(curve),
225                spline.control_points.len(),
226                spline.knots.as_slice(),
227            ),
228            Self::Three(curve @ Curve3::BSpline(spline)) => (
229                domain3(curve),
230                spline.control_points.len(),
231                spline.knots.as_slice(),
232            ),
233            _ => return Err(refused(POINT_BOUND_UNSUPPORTED)),
234        };
235        let (lo, hi) = (domain.start.min(domain.end), domain.start.max(domain.end));
236        if !(lo.is_finite() && hi.is_finite() && hi > lo) {
237            return Err(GeomError::Degenerate(
238                "station: a B-spline with no parameter domain bounds a curve path piece".into(),
239            ));
240        }
241        let samples = count.max(2) * SAMPLES_PER_CONTROL_POINT;
242        let mut at: Vec<Scalar> = (0..=samples)
243            .map(|i| lo + (hi - lo) * i as Scalar / samples as Scalar)
244            .chain(knots.iter().copied().filter(|&k| k > lo && k < hi))
245            .collect();
246        at.sort_by(Scalar::total_cmp);
247        at.dedup();
248        let off = at
249            .iter()
250            .map(|&t| self.distance(t, point))
251            .collect::<GeomResult<Vec<_>>>()?;
252        let last = at.len() - 1;
253        let mut found = Vec::new();
254        for i in 0..=last {
255            let falls = i == 0 || off[i] <= off[i - 1];
256            let rises = i == last || off[i] <= off[i + 1];
257            if !(falls && rises) {
258                continue;
259            }
260            let (a, b) = (at[i.saturating_sub(1)], at[(i + 1).min(last)]);
261            let refined = self.refine(at[i], a, b, point)?;
262            let near = self.distance(refined, point)?;
263            let (t, distance) = if near <= off[i] {
264                (refined, near)
265            } else {
266                (at[i], off[i])
267            };
268            if distance <= tolerance {
269                found.push((t, distance));
270            }
271        }
272        found.sort_by(|x, y| x.0.total_cmp(&y.0));
273        places(found, |ta, tb| {
274            let between = at
275                .iter()
276                .zip(&off)
277                .filter(|(t, _)| **t > ta && **t < tb)
278                .all(|(_, d)| *d <= tolerance);
279            Ok(between && self.distance(0.5 * (ta + tb), point)? <= tolerance)
280        })
281    }
282
283    /// Newton on `(C(t) - p) . C'(t)` from `t`, kept within `[a, b]`.
284    fn refine(self, mut t: Scalar, a: Scalar, b: Scalar, point: Point3) -> GeomResult<Scalar> {
285        for _ in 0..NEWTON_STEPS {
286            let (d, dd) = self.derivatives(t)?;
287            let r = self.point(t)? - point;
288            let (g, h) = (r.dot(d), d.dot(d) + r.dot(dd));
289            if !(h.is_finite() && h > 0.0) {
290                break;
291            }
292            let next = (t - g / h).clamp(a, b);
293            let moved = (next - t).abs();
294            t = next;
295            if moved <= 4.0 * Scalar::EPSILON * (1.0 + t.abs()) {
296                break;
297            }
298        }
299        Ok(t)
300    }
301}
302
303/// A polyline's places within `tolerance` of `point`: each segment's
304/// nearest parameter (its index plus the fraction along it), one per
305/// place.
306fn polyline_parameters(
307    points: &[Point3],
308    closed: bool,
309    point: Point3,
310    tolerance: Scalar,
311) -> Vec<Scalar> {
312    let n = points.len();
313    let segments = if closed { n } else { n.saturating_sub(1) };
314    let mut found = Vec::new();
315    for i in 0..segments {
316        let (a, b) = (points[i], points[(i + 1) % n]);
317        let d = b - a;
318        let dd = d.dot(d);
319        let u = if dd > 0.0 {
320            ((point - a).dot(d) / dd).clamp(0.0, 1.0)
321        } else {
322            0.0
323        };
324        let distance = (a + d * u - point).length();
325        if distance <= tolerance {
326            found.push((i as Scalar + u, distance));
327        }
328    }
329    let vertex = |k: usize| points[k % n.max(1)];
330    places(found, |ta, tb| {
331        // The vertices strictly between: the segments joining them stay
332        // within tolerance exactly when every vertex does.
333        let first = ta.floor() as usize + 1;
334        let last = tb.ceil() as usize;
335        Ok((first..last)
336            .filter(|&k| (k as Scalar) > ta && (k as Scalar) < tb)
337            .all(|k| (vertex(k) - point).length() <= tolerance))
338    })
339    .unwrap_or_default()
340}
341
342/// The candidates `(parameter, distance)`, in increasing parameter, merged
343/// into places: consecutive candidates are one place when `same` says the
344/// curve stays within tolerance between them. Each place is its nearest
345/// candidate (the first on a tie).
346fn places(
347    found: Vec<(Scalar, Scalar)>,
348    same: impl Fn(Scalar, Scalar) -> GeomResult<bool>,
349) -> GeomResult<Vec<Scalar>> {
350    let mut out: Vec<(Scalar, Scalar)> = Vec::new();
351    let mut previous: Option<Scalar> = None;
352    for (t, distance) in found {
353        let joins = match previous {
354            Some(before) => same(before, t)?,
355            None => false,
356        };
357        previous = Some(t);
358        match out.last_mut() {
359            Some(place) if joins => {
360                if distance < place.1 {
361                    *place = (t, distance);
362                }
363            }
364            _ => out.push((t, distance)),
365        }
366    }
367    Ok(out.into_iter().map(|(t, _)| t).collect())
368}
axiolid_evaluate/station/path_bound.rs

axiolid_evaluate/station/
path_bound.rs
