axiolid_evaluate/
arc_length.rs

1//! Arc-length evaluation of intrinsic (natural-equation) curves and of the
2//! planar-plus-elevation composition.
3//!
4//! # Why quadrature, and why that is not an approximation of the VALUE
5//!
6//! An intrinsic curve stores curvature as a function of arc length. Its
7//! heading is the integral of that law and is exact in closed form, but its
8//! POSITION is the integral of `(cos th, sin th)` and has no elementary
9//! antiderivative -- for the clothoid it is the Fresnel integral. The stored
10//! value stays exact; only reading a point out of it needs numerical work.
11//! That is the same bargain as evaluating `sin`: the curve is not approximated,
12//! its evaluation is computed to tolerance.
13//!
14//! Gauss-Legendre is used because the integrand is smooth. An 8-point rule
15//! integrates a degree-15 polynomial exactly, and against the Fresnel closed
16//! form it reproduces a 120 m clothoid to R=300 with zero error at machine
17//! precision on a single panel, versus roughly 1e-3 relative for a comparable
18//! trapezoid budget. Panels are subdivided by total turning so a tight spiral
19//! gets more of them.
20//!
21//! A panel must never straddle a seam of a piecewise law: Gauss-Legendre
22//! assumes a smooth integrand, and a joined curve was wrong by 3.0e-3 until
23//! panels were split at seams. Any new integrator splits at
24//! `CurvatureLaw::seams_within` too. Pin a change to this quadrature against
25//! an independent closed form (Fresnel for the clothoid, the elementary arc
26//! for constant curvature), never against another run of the quadrature
27//! (ADR 0060).
28
29use axiolid_contracts::{BackendId, GeomError, GeomResult, Operation};
30use axiolid_core::{Frame2, Point2, Point3, Scalar, Vec2, Vec3};
31use axiolid_curve::{Curve2, Elevated3, Intrinsic2};
32
33/// Nodes and weights of the 8-point Gauss-Legendre rule on `[-1, 1]`.
34///
35/// Exact for polynomials up to degree 15. Written out rather than computed:
36/// they are constants, and a Newton solve at startup would be slower and no
37/// more accurate.
38const GAUSS_NODES: [Scalar; 8] = [
39    -0.960_289_856_497_536_2,
40    -0.796_666_477_413_626_7,
41    -0.525_532_409_916_328_9,
42    -0.183_434_642_495_649_8,
43    0.183_434_642_495_649_8,
44    0.525_532_409_916_328_9,
45    0.796_666_477_413_626_7,
46    0.960_289_856_497_536_2,
47];
48
49/// Weights matching [`GAUSS_NODES`].
50const GAUSS_WEIGHTS: [Scalar; 8] = [
51    0.101_228_536_290_376_3,
52    0.222_381_034_453_374_5,
53    0.313_706_645_877_887_3,
54    0.362_683_783_378_361_9,
55    0.362_683_783_378_361_9,
56    0.313_706_645_877_887_3,
57    0.222_381_034_453_374_5,
58    0.101_228_536_290_376_3,
59];
60
61/// Panels per radian of total turning, above a floor of one panel.
62///
63/// A straight or gently curving run needs one panel; a spiral that turns
64/// through a radian gets four. Bounded so a pathological law cannot ask for an
65/// unbounded amount of work.
66const PANELS_PER_RADIAN: Scalar = 4.0;
67
68/// Upper bound on panels, so a malformed law refuses rather than hangs.
69const MAX_PANELS: usize = 4096;
70
71fn unsupported() -> GeomError {
72    GeomError::Unsupported {
73        backend: BackendId::new("axiolid-evaluate"),
74        operation: Operation::CurveEvaluation,
75    }
76}
77
78fn invalid(detail: &str) -> GeomError {
79    GeomError::InvalidInput(detail.to_owned())
80}
81
82/// How many panels to spend integrating `[0, s]`.
83///
84/// Budgeted from the TOTAL VARIATION of heading, not from `total_turning`.
85/// The signed integral is zero over a whole number of periods of a zero-mean
86/// oscillation, so budgeting from it would spend one panel on a curve that
87/// swings through many radians. Measured on `k(s) = 2 sin(10 s)` over
88/// `[0, pi]`: signed turning is 0, which bought one panel and left the
89/// endpoint 2.1e-1 wrong; the variation bound buys 26 panels and lands it
90/// to 4.9e-15.
91fn panel_count(curve: &Intrinsic2, s: Scalar) -> GeomResult<usize> {
92    let variation = curve
93        .turning_variation_bound(s)
94        .ok_or_else(|| invalid("curvature law does not integrate over the requested span"))?;
95    let wanted = (variation * PANELS_PER_RADIAN).ceil().max(1.0);
96    if !wanted.is_finite() || wanted > MAX_PANELS as Scalar {
97        return Err(invalid("curvature law needs an unbounded number of panels"));
98    }
99    Ok(wanted as usize)
100}
101
102/// Position on an intrinsic curve at arc length `s` from its start.
103///
104/// The heading is exact; the position is Gauss-Legendre quadrature of the unit
105/// tangent, which is where the non-elementary integral is discharged.
106pub fn intrinsic_point(curve: &Intrinsic2, s: Scalar) -> GeomResult<Point2> {
107    if !s.is_finite() {
108        return Err(invalid("arc length must be finite"));
109    }
110    // Panels must break at curvature seams: Gauss-Legendre assumes a smooth
111    // integrand across a panel, and a piecewise law is only piecewise-smooth.
112    let mut bounds = vec![0.0];
113    bounds.extend(curve.curvature.seams_within(s));
114    bounds.push(s);
115    let mut x = 0.0;
116    let mut y = 0.0;
117    for window in bounds.windows(2) {
118        let (lo, hi) = (window[0], window[1]);
119        if hi <= lo {
120            continue;
121        }
122        let span = hi - lo;
123        // Budget from variation over [0, hi]: an upper bound for this
124        // sub-span, since variation is monotone in the span. Never from
125        // `span` alone, which would ignore how far along the curve we are.
126        let panels = panel_count(curve, hi)?.max(1);
127        let step = span / panels as Scalar;
128        for panel in 0..panels {
129            let a = lo + step * panel as Scalar;
130            let half = step / 2.0;
131            let mid = a + half;
132            for (node, weight) in GAUSS_NODES.iter().zip(GAUSS_WEIGHTS.iter()) {
133                let u = mid + half * node;
134                let heading = curve
135                    .heading_at(u)
136                    .ok_or_else(|| invalid("curvature law does not integrate to the sample"))?;
137                x += weight * half * heading.cos();
138                y += weight * half * heading.sin();
139            }
140        }
141    }
142    // The integral is taken in the start frame, whose x axis is the start
143    // tangent, then placed into world coordinates by that frame.
144    Ok(place2(&curve.start, Vec2::new(x, y)))
145}
146
147/// Unit tangent of an intrinsic curve at arc length `s`.
148///
149/// Exact: this is the closed-form heading, no quadrature involved.
150pub fn intrinsic_tangent(curve: &Intrinsic2, s: Scalar) -> GeomResult<Vec2> {
151    if !s.is_finite() {
152        return Err(invalid("arc length must be finite"));
153    }
154    let heading = curve
155        .heading_at(s)
156        .ok_or_else(|| invalid("curvature law does not integrate to the requested arc length"))?;
157    let local = Vec2::new(heading.cos(), heading.sin());
158    Ok(rotate2(&curve.start, local))
159}
160
161/// Place a local offset into world coordinates through a 2D frame.
162fn place2(frame: &Frame2, local: Vec2) -> Point2 {
163    Point2::new(
164        frame.origin.x + frame.x.x * local.x + frame.y.x * local.y,
165        frame.origin.y + frame.x.y * local.x + frame.y.y * local.y,
166    )
167}
168
169/// Rotate a local direction into world coordinates through a 2D frame.
170fn rotate2(frame: &Frame2, local: Vec2) -> Vec2 {
171    Vec2::new(
172        frame.x.x * local.x + frame.y.x * local.y,
173        frame.x.y * local.x + frame.y.y * local.y,
174    )
175}
176
177/// Position on the plan at plan distance `d`.
178///
179/// Only families whose parameter IS arc length can carry an elevation law,
180/// because the law is written against distance along the plan. A line and an
181/// intrinsic curve qualify; a B-spline's parameter is not arc length, so
182/// pairing one would silently mean something else and is refused.
183fn plan_point(plan: &Curve2, d: Scalar) -> GeomResult<Point2> {
184    match plan {
185        Curve2::Line(line) => {
186            let direction = unit2(line.direction)?;
187            Ok(Point2::new(
188                line.origin.x + direction.x * d,
189                line.origin.y + direction.y * d,
190            ))
191        }
192        Curve2::Circle(circle) => {
193            // Arc length d subtends d / r, so the angle is exact.
194            if circle.radius <= 0.0 || !circle.radius.is_finite() {
195                return Err(invalid("circle radius must be positive and finite"));
196            }
197            let angle = d / circle.radius;
198            Ok(place2(
199                &circle.frame,
200                Vec2::new(circle.radius * angle.cos(), circle.radius * angle.sin()),
201            ))
202        }
203        Curve2::Intrinsic(intrinsic) => intrinsic_point(intrinsic, d),
204        _ => Err(unsupported()),
205    }
206}
207
208/// Unit tangent of the plan at plan distance `d`.
209fn plan_tangent(plan: &Curve2, d: Scalar) -> GeomResult<Vec2> {
210    match plan {
211        Curve2::Line(line) => unit2(line.direction),
212        Curve2::Circle(circle) => {
213            if circle.radius <= 0.0 || !circle.radius.is_finite() {
214                return Err(invalid("circle radius must be positive and finite"));
215            }
216            let angle = d / circle.radius;
217            Ok(rotate2(&circle.frame, Vec2::new(-angle.sin(), angle.cos())))
218        }
219        Curve2::Intrinsic(intrinsic) => intrinsic_tangent(intrinsic, d),
220        _ => Err(unsupported()),
221    }
222}
223
224fn unit2(v: Vec2) -> GeomResult<Vec2> {
225    let length = (v.x * v.x + v.y * v.y).sqrt();
226    if !length.is_finite() || length == 0.0 {
227        return Err(invalid("direction must be finite and non-zero"));
228    }
229    Ok(Vec2::new(v.x / length, v.y / length))
230}
231
232/// Position on an elevated curve at plan distance `d`.
233///
234/// The plan supplies `x`/`y`, the elevation law supplies `z`. Both halves are
235/// read at the SAME plan distance, which is the convention `ElevationLaw`
236/// documents.
237pub fn elevated_point(curve: &Elevated3, d: Scalar) -> GeomResult<Point3> {
238    if !d.is_finite() {
239        return Err(invalid("plan distance must be finite"));
240    }
241    let planar = plan_point(&curve.plan, d)?;
242    let height = curve
243        .elevation
244        .height_at(d)
245        .ok_or_else(|| invalid("elevation law has no height at that distance"))?;
246    Ok(Point3::new(planar.x, planar.y, height))
247}
248
249/// Unit tangent of an elevated curve at plan distance `d`.
250///
251/// The plan tangent is horizontal and the grade lifts it, so the 3D tangent is
252/// `(t.x, t.y, g)` normalised -- the `sqrt(1 + g^2)` factor by which 3D arc
253/// length runs ahead of plan distance.
254pub fn elevated_tangent(curve: &Elevated3, d: Scalar) -> GeomResult<Vec3> {
255    if !d.is_finite() {
256        return Err(invalid("plan distance must be finite"));
257    }
258    let planar = plan_tangent(&curve.plan, d)?;
259    let grade = curve
260        .elevation
261        .grade_at(d)
262        .ok_or_else(|| invalid("elevation law has no grade at that distance"))?;
263    let scale = (1.0 + grade * grade).sqrt();
264    if !scale.is_finite() || scale == 0.0 {
265        return Err(invalid("grade does not give a finite tangent"));
266    }
267    Ok(Vec3::new(planar.x / scale, planar.y / scale, grade / scale))
268}
axiolid_evaluate/arc_length.rs

axiolid_evaluate/
arc_length.rs
