1use axiolid_contracts::{GeomError, GeomResult};
35use axiolid_core::{Frame2, Frame3, Interval, Point2, Point3, Scalar, Tolerance, Vec2, Vec3};
36use axiolid_curve::{
37 BSplineCurve, BSplineCurve2, BSplineCurve3, Circle2, Circle3, Curve2, Curve3, CurveEvaluator,
38 Ellipse2, Ellipse3, Line2, Line3, Polyline2, Polyline3,
39};
40
41use crate::nurbs::SplineAxis;
42
43#[derive(Debug, Clone, Copy, Default)]
48pub struct ScalarCurve;
49
50impl ScalarCurve {
51 #[must_use]
53 pub const fn new() -> Self {
54 Self
55 }
56}
57
58#[derive(Debug, Clone, Copy, PartialEq)]
64pub struct CurveJet<P, D> {
65 pub point: P,
67 pub first: D,
69 pub second: D,
71}
72
73#[must_use]
77pub fn domain2(curve: &Curve2) -> Interval {
78 match curve {
79 Curve2::Line(_) => Interval::UNIT,
82 Curve2::Circle(_) | Curve2::Ellipse(_) => full_turn(),
83 Curve2::Polyline(p) => polyline_domain(p.points.len(), p.closed),
84 Curve2::BSpline(b) => spline_domain(b),
85 Curve2::Intrinsic(i) if i.length.is_finite() && i.length > 0.0 => Interval {
89 start: 0.0,
90 end: i.length,
91 },
92 Curve2::Sinusoid(_) => full_turn(),
94 _ => Interval {
96 start: 0.0,
97 end: 0.0,
98 },
99 }
100}
101
102#[must_use]
104pub fn domain3(curve: &Curve3) -> Interval {
105 match curve {
106 Curve3::Line(_) => Interval::UNIT,
107 Curve3::Circle(_) | Curve3::Ellipse(_) => full_turn(),
108 Curve3::Polyline(p) => polyline_domain(p.points.len(), p.closed),
109 Curve3::BSpline(b) => spline_domain(b),
110 Curve3::Intrinsic(i) if i.length.is_finite() && i.length > 0.0 => Interval {
114 start: 0.0,
115 end: i.length,
116 },
117 _ => Interval {
118 start: 0.0,
119 end: 0.0,
120 },
121 }
122}
123
124fn full_turn() -> Interval {
125 Interval {
126 start: 0.0,
127 end: core::f64::consts::TAU,
128 }
129}
130
131fn polyline_domain(count: usize, closed: bool) -> Interval {
133 let segments = if closed {
134 count
135 } else {
136 count.saturating_sub(1)
137 };
138 Interval {
139 start: 0.0,
140 end: segments as Scalar,
141 }
142}
143
144fn spline_domain<P>(b: &BSplineCurve<P>) -> Interval {
147 SplineAxis::new(
148 &b.knots,
149 &b.multiplicities,
150 b.degree,
151 b.control_points.len(),
152 "B-spline curve",
153 )
154 .map_or(
155 Interval {
156 start: 0.0,
157 end: 0.0,
158 },
159 |axis| {
160 let (start, end) = axis.domain();
161 Interval { start, end }
162 },
163 )
164}
165
166pub fn evaluate2(curve: &Curve2, t: Scalar) -> GeomResult<Point2> {
170 finite(t)?;
171 let value = match curve {
172 Curve2::Line(l) => Ok(line_point(l.origin, l.direction, t)),
173 Curve2::Circle(c) => Ok(conic_point2(&c.frame, c.radius, c.radius, t)),
174 Curve2::Ellipse(e) => Ok(conic_point2(&e.frame, e.semi_axis_x, e.semi_axis_y, t)),
175 Curve2::Polyline(p) => polyline_point(&p.points, p.closed, t),
176 Curve2::BSpline(b) => de_boor(b, t, |p| [p.x, p.y], |c| Point2::new(c[0], c[1])),
177 Curve2::Intrinsic(i) => crate::arc_length::intrinsic_point(i, t),
181 Curve2::Sinusoid(w) => Ok(Point2::new(t, w.height(t))),
183 Curve2::QuadraticGraph(g) => g
185 .height(t)
186 .map(|v| Point2::new(t, v))
187 .ok_or_else(|| outside_graph(t)),
188 Curve2::AngleGraph(g) => g
190 .angle(t)
191 .map(|u| Point2::new(u, t))
192 .ok_or_else(|| outside_graph(t)),
193 Curve2::Implicit(c) => c.point(t).ok_or_else(|| outside_graph(t)),
195 Curve2::Lifted(l) => {
197 if let Some((section, first)) = pair_side(l) {
199 let (a, b, _) = section.solve(t).ok_or_else(|| outside_graph(t))?;
200 return finite2(if first { a } else { b }, "curve point");
201 }
202 let p = evaluate3(&l.curve, t)?;
203 l.unwrap_at(t, p).ok_or_else(|| outside_graph(t))
204 }
205 _ => Err(GeomError::Unsupported {
208 backend: axiolid_contracts::BackendId::new("axiolid-reference"),
209 operation: axiolid_contracts::Operation::CurveEvaluation,
210 }),
211 }?;
212 finite2(value, "curve point")
213}
214
215pub fn derivative2(curve: &Curve2, t: Scalar) -> GeomResult<Vec2> {
217 finite(t)?;
218 let value = match curve {
219 Curve2::Line(l) => Ok(l.direction),
220 Curve2::Circle(c) => Ok(conic_tangent2(&c.frame, c.radius, c.radius, t)),
221 Curve2::Ellipse(e) => Ok(conic_tangent2(&e.frame, e.semi_axis_x, e.semi_axis_y, t)),
222 Curve2::Polyline(p) => polyline_tangent(&p.points, p.closed, t),
223 Curve2::BSpline(b) => de_boor_derivative(b, t, |p| [p.x, p.y], |c| Vec2::new(c[0], c[1])),
224 Curve2::Intrinsic(i) => crate::arc_length::intrinsic_tangent(i, t),
228 Curve2::Sinusoid(w) => {
229 let (sin, cos) = t.sin_cos();
230 Ok(Vec2::new(1.0, -w.cosine * sin + w.sine * cos))
231 }
232 Curve2::QuadraticGraph(g) => g
233 .slope(t)
234 .map(|slope| Vec2::new(1.0, slope))
235 .ok_or_else(|| outside_graph(t)),
236 Curve2::AngleGraph(g) => g
237 .slope(t)
238 .map(|slope| Vec2::new(slope, 1.0))
239 .ok_or_else(|| outside_graph(t)),
240 Curve2::Implicit(c) => c.derivative(t).ok_or_else(|| outside_graph(t)),
241 Curve2::Lifted(l) => lifted_rates(l, t).map(|(d, _)| d),
242 _ => Err(GeomError::Unsupported {
245 backend: axiolid_contracts::BackendId::new("axiolid-reference"),
246 operation: axiolid_contracts::Operation::CurveEvaluation,
247 }),
248 }?;
249 finite2(value, "curve derivative")
250}
251
252pub fn second_derivative2(curve: &Curve2, t: Scalar) -> GeomResult<Vec2> {
254 finite(t)?;
255 let value = match curve {
256 Curve2::Line(_) | Curve2::Polyline(_) => Ok(Vec2::ZERO),
257 Curve2::Circle(c) => Ok(conic_second2(&c.frame, c.radius, c.radius, t)),
258 Curve2::Ellipse(e) => Ok(conic_second2(&e.frame, e.semi_axis_x, e.semi_axis_y, t)),
259 Curve2::BSpline(b) => {
260 de_boor_second_derivative(b, t, |p| [p.x, p.y], |c| Vec2::new(c[0], c[1]))
261 }
262 Curve2::Sinusoid(w) => {
263 let (sin, cos) = t.sin_cos();
264 Ok(Vec2::new(0.0, -w.cosine * cos - w.sine * sin))
265 }
266 Curve2::QuadraticGraph(g) => g
267 .bend(t)
268 .map(|bend| Vec2::new(0.0, bend))
269 .ok_or_else(|| outside_graph(t)),
270 Curve2::AngleGraph(g) => g
271 .bend(t)
272 .map(|bend| Vec2::new(bend, 0.0))
273 .ok_or_else(|| outside_graph(t)),
274 Curve2::Implicit(c) => c.second_derivative(t).ok_or_else(|| outside_graph(t)),
275 Curve2::Lifted(l) => lifted_rates(l, t).map(|(_, dd)| dd),
276 _ => Err(GeomError::Unsupported {
277 backend: axiolid_contracts::BackendId::new("axiolid-reference"),
278 operation: axiolid_contracts::Operation::CurveEvaluation,
279 }),
280 }?;
281 finite2(value, "curve second derivative")
282}
283
284pub fn bspline_jet2(curve: &BSplineCurve2, t: Scalar) -> GeomResult<CurveJet<Point2, Vec2>> {
286 Ok(CurveJet {
287 point: de_boor(curve, t, |p| [p.x, p.y], |c| Point2::new(c[0], c[1]))?,
288 first: de_boor_derivative(curve, t, |p| [p.x, p.y], |c| Vec2::new(c[0], c[1]))?,
289 second: de_boor_second_derivative(curve, t, |p| [p.x, p.y], |c| Vec2::new(c[0], c[1]))?,
290 })
291}
292
293pub fn jet2(curve: &Curve2, t: Scalar) -> GeomResult<CurveJet<Point2, Vec2>> {
295 Ok(CurveJet {
296 point: evaluate2(curve, t)?,
297 first: derivative2(curve, t)?,
298 second: second_derivative2(curve, t)?,
299 })
300}
301
302fn lifted_rates(l: &axiolid_curve::LiftedCurve2, t: Scalar) -> GeomResult<(Vec2, Vec2)> {
306 if let Some((section, first)) = pair_side(l) {
307 let (a, b, _) = section.rates(t).ok_or_else(|| outside_graph(t))?;
308 let (aa, bb, _) = section.second_rates(t).ok_or_else(|| outside_graph(t))?;
309 let pick = |x: Point2, y: Point2| if first { x } else { y };
310 let (d, dd) = (pick(a, b), pick(aa, bb));
311 return Ok((Vec2::new(d.x, d.y), Vec2::new(dd.x, dd.y)));
312 }
313 let p = evaluate3(&l.curve, t)?;
314 let uv = l.unwrap_at(t, p).ok_or_else(|| outside_graph(t))?;
315 let jet = l.carrier.jet(uv.x, uv.y);
316 let (a, b, c) = (jet.u.dot(jet.u), jet.u.dot(jet.v), jet.v.dot(jet.v));
317 let det = a * c - b * b;
318 if det == 0.0 || !det.is_finite() {
319 return Err(outside_graph(t));
320 }
321 let solve = |w: Vec3| {
322 let (g0, g1) = (jet.u.dot(w), jet.v.dot(w));
323 Vec2::new((c * g0 - b * g1) / det, (a * g1 - b * g0) / det)
324 };
325 let d = solve(derivative3(&l.curve, t)?);
326 let rest = second_derivative3(&l.curve, t)?
327 - (jet.uu * (d.x * d.x) + jet.uv * (2.0 * d.x * d.y) + jet.vv * (d.y * d.y));
328 Ok((d, solve(rest)))
329}
330
331fn pair_side(l: &axiolid_curve::LiftedCurve2) -> Option<(&axiolid_curve::PairSection3, bool)> {
334 match l.curve.as_ref() {
335 Curve3::PairSection(section) => section.side(&l.carrier).map(|first| (section, first)),
336 _ => None,
337 }
338}
339
340fn outside_graph(t: Scalar) -> GeomError {
343 GeomError::Degenerate(format!(
344 "quadratic section graph has no regular point at t = {t}"
345 ))
346}
347
348pub fn evaluate3(curve: &Curve3, t: Scalar) -> GeomResult<Point3> {
352 finite(t)?;
353 let value = match curve {
354 Curve3::Line(l) => Ok(line_point(l.origin, l.direction, t)),
355 Curve3::Circle(c) => Ok(conic_point3(&c.frame, c.radius, c.radius, t)),
356 Curve3::Ellipse(e) => Ok(conic_point3(&e.frame, e.semi_axis_x, e.semi_axis_y, t)),
357 Curve3::Polyline(p) => polyline_point(&p.points, p.closed, t),
358 Curve3::BSpline(b) => de_boor(b, t, |p| [p.x, p.y, p.z], |c| Point3::new(c[0], c[1], c[2])),
359 Curve3::Intrinsic(i) => crate::frenet::frenet_point(i, t),
364 Curve3::RuledSection(r) => r.point(t).ok_or_else(|| outside_graph(t)),
366 Curve3::TorusSection(r) => r.point(t).ok_or_else(|| outside_graph(t)),
367 Curve3::ImplicitSection(r) => r.point(t).ok_or_else(|| outside_graph(t)),
368 Curve3::PairSection(r) => r.point(t).ok_or_else(|| outside_graph(t)),
369 _ => Err(GeomError::Unsupported {
372 backend: axiolid_contracts::BackendId::new("axiolid-reference"),
373 operation: axiolid_contracts::Operation::CurveEvaluation,
374 }),
375 }?;
376 finite3(value, "curve point")
377}
378
379pub fn derivative3(curve: &Curve3, t: Scalar) -> GeomResult<Vec3> {
381 finite(t)?;
382 let value = match curve {
383 Curve3::Line(l) => Ok(l.direction),
384 Curve3::Circle(c) => Ok(conic_tangent3(&c.frame, c.radius, c.radius, t)),
385 Curve3::Ellipse(e) => Ok(conic_tangent3(&e.frame, e.semi_axis_x, e.semi_axis_y, t)),
386 Curve3::Polyline(p) => polyline_tangent(&p.points, p.closed, t),
387 Curve3::BSpline(b) => {
388 de_boor_derivative(b, t, |p| [p.x, p.y, p.z], |c| Vec3::new(c[0], c[1], c[2]))
389 }
390 Curve3::Intrinsic(i) => crate::frenet::frenet_tangent(i, t),
392 Curve3::RuledSection(r) => r.tangent(t).ok_or_else(|| outside_graph(t)),
393 Curve3::TorusSection(r) => r.tangent(t).ok_or_else(|| outside_graph(t)),
394 Curve3::ImplicitSection(r) => r.tangent(t).ok_or_else(|| outside_graph(t)),
395 Curve3::PairSection(r) => r.tangent(t).ok_or_else(|| outside_graph(t)),
396 _ => Err(GeomError::Unsupported {
399 backend: axiolid_contracts::BackendId::new("axiolid-reference"),
400 operation: axiolid_contracts::Operation::CurveEvaluation,
401 }),
402 }?;
403 finite3(value, "curve derivative")
404}
405
406pub fn second_derivative3(curve: &Curve3, t: Scalar) -> GeomResult<Vec3> {
408 finite(t)?;
409 let value = match curve {
410 Curve3::Line(_) | Curve3::Polyline(_) => Ok(Vec3::ZERO),
411 Curve3::Circle(c) => Ok(conic_second3(&c.frame, c.radius, c.radius, t)),
412 Curve3::Ellipse(e) => Ok(conic_second3(&e.frame, e.semi_axis_x, e.semi_axis_y, t)),
413 Curve3::BSpline(b) => {
414 de_boor_second_derivative(b, t, |p| [p.x, p.y, p.z], |c| Vec3::new(c[0], c[1], c[2]))
415 }
416 Curve3::RuledSection(r) => r.bend(t).ok_or_else(|| outside_graph(t)),
417 Curve3::TorusSection(r) => r.bend(t).ok_or_else(|| outside_graph(t)),
418 Curve3::ImplicitSection(r) => r.bend(t).ok_or_else(|| outside_graph(t)),
419 Curve3::PairSection(r) => r.bend(t).ok_or_else(|| outside_graph(t)),
420 _ => Err(GeomError::Unsupported {
421 backend: axiolid_contracts::BackendId::new("axiolid-reference"),
422 operation: axiolid_contracts::Operation::CurveEvaluation,
423 }),
424 }?;
425 finite3(value, "curve second derivative")
426}
427
428pub fn bspline_jet3(curve: &BSplineCurve3, t: Scalar) -> GeomResult<CurveJet<Point3, Vec3>> {
430 Ok(CurveJet {
431 point: de_boor(
432 curve,
433 t,
434 |p| [p.x, p.y, p.z],
435 |c| Point3::new(c[0], c[1], c[2]),
436 )?,
437 first: de_boor_derivative(
438 curve,
439 t,
440 |p| [p.x, p.y, p.z],
441 |c| Vec3::new(c[0], c[1], c[2]),
442 )?,
443 second: de_boor_second_derivative(
444 curve,
445 t,
446 |p| [p.x, p.y, p.z],
447 |c| Vec3::new(c[0], c[1], c[2]),
448 )?,
449 })
450}
451
452pub fn jet3(curve: &Curve3, t: Scalar) -> GeomResult<CurveJet<Point3, Vec3>> {
454 Ok(CurveJet {
455 point: evaluate3(curve, t)?,
456 first: derivative3(curve, t)?,
457 second: second_derivative3(curve, t)?,
458 })
459}
460
461fn finite(t: Scalar) -> GeomResult<()> {
464 if t.is_finite() {
465 Ok(())
466 } else {
467 Err(GeomError::InvalidInput(format!(
468 "curve parameter must be finite, got {t}"
469 )))
470 }
471}
472
473fn finite2(value: Vec2, what: &str) -> GeomResult<Vec2> {
474 if value.is_finite() {
475 Ok(value)
476 } else {
477 Err(GeomError::Degenerate(format!("{what} is non-finite")))
478 }
479}
480
481fn finite3(value: Vec3, what: &str) -> GeomResult<Vec3> {
482 if value.is_finite() {
483 Ok(value)
484 } else {
485 Err(GeomError::Degenerate(format!("{what} is non-finite")))
486 }
487}
488
489fn line_point<P>(origin: P, direction: P, t: Scalar) -> P
490where
491 P: core::ops::Add<Output = P> + core::ops::Mul<Scalar, Output = P>,
492{
493 origin + direction * t
494}
495
496fn conic_point2(frame: &Frame2, rx: Scalar, ry: Scalar, t: Scalar) -> Point2 {
497 frame.origin + frame.x * (rx * t.cos()) + frame.y * (ry * t.sin())
498}
499
500fn conic_tangent2(frame: &Frame2, rx: Scalar, ry: Scalar, t: Scalar) -> Vec2 {
501 frame.x * (-rx * t.sin()) + frame.y * (ry * t.cos())
502}
503
504fn conic_second2(frame: &Frame2, rx: Scalar, ry: Scalar, t: Scalar) -> Vec2 {
505 frame.x * (-rx * t.cos()) + frame.y * (-ry * t.sin())
506}
507
508fn conic_point3(frame: &Frame3, rx: Scalar, ry: Scalar, t: Scalar) -> Point3 {
509 frame.origin + frame.x * (rx * t.cos()) + frame.y * (ry * t.sin())
510}
511
512fn conic_tangent3(frame: &Frame3, rx: Scalar, ry: Scalar, t: Scalar) -> Vec3 {
513 frame.x * (-rx * t.sin()) + frame.y * (ry * t.cos())
514}
515
516fn conic_second3(frame: &Frame3, rx: Scalar, ry: Scalar, t: Scalar) -> Vec3 {
517 frame.x * (-rx * t.cos()) + frame.y * (-ry * t.sin())
518}
519
520fn polyline_span(count: usize, closed: bool, t: Scalar) -> Option<(usize, usize, Scalar)> {
524 let segments = if closed {
525 count
526 } else {
527 count.saturating_sub(1)
528 };
529 if count < 2 || segments == 0 {
530 return None;
531 }
532 let clamped = t.clamp(0.0, segments as Scalar);
535 let mut index = clamped.floor() as usize;
536 if index >= segments {
537 index = segments - 1;
538 }
539 let local = clamped - index as Scalar;
540 let next = (index + 1) % count;
541 Some((index, next, local))
542}
543
544fn polyline_point<P>(points: &[P], closed: bool, t: Scalar) -> GeomResult<P>
545where
546 P: Copy
547 + core::ops::Add<Output = P>
548 + core::ops::Sub<Output = P>
549 + core::ops::Mul<Scalar, Output = P>,
550{
551 let (i, j, local) = polyline_span(points.len(), closed, t).ok_or_else(|| {
552 GeomError::Degenerate(format!(
553 "polyline with {} points has no evaluable segment",
554 points.len()
555 ))
556 })?;
557 Ok(points[i] + (points[j] - points[i]) * local)
558}
559
560fn polyline_tangent<P>(points: &[P], closed: bool, t: Scalar) -> GeomResult<P>
561where
562 P: Copy + core::ops::Sub<Output = P>,
563{
564 let (i, j, _) = polyline_span(points.len(), closed, t).ok_or_else(|| {
565 GeomError::Degenerate(format!(
566 "polyline with {} points has no evaluable segment",
567 points.len()
568 ))
569 })?;
570 Ok(points[j] - points[i])
572}
573
574pub(crate) fn span_in(knots: &[Scalar], n: usize, d: usize, u: Scalar) -> usize {
582 let mut span = d;
583 for (k, knot) in knots.iter().enumerate().take(n).skip(d) {
584 if *knot <= u {
585 span = k;
586 } else {
587 break;
588 }
589 }
590 span
591}
592
593pub(crate) fn de_boor_recurrence<const N: usize>(
600 knots: &[Scalar],
601 span: usize,
602 d: usize,
603 u: Scalar,
604 points: &mut [[Scalar; N]],
605 weights: &mut [Scalar],
606) {
607 for r in 1..=d {
608 for j in (r..=d).rev() {
609 let i = span - d + j;
610 let denom = knots[i + d + 1 - r] - knots[i];
611 let alpha = if denom.abs() > 0.0 {
612 (u - knots[i]) / denom
613 } else {
614 0.0
615 };
616 for k in 0..N {
617 points[j][k] = points[j - 1][k] * (1.0 - alpha) + points[j][k] * alpha;
618 }
619 weights[j] = weights[j - 1] * (1.0 - alpha) + weights[j] * alpha;
620 }
621 }
622}
623
624fn spline_span<P>(b: &BSplineCurve<P>, t: Scalar) -> GeomResult<(Vec<Scalar>, usize, usize)> {
628 let axis = SplineAxis::new(
629 &b.knots,
630 &b.multiplicities,
631 b.degree,
632 b.control_points.len(),
633 "B-spline curve",
634 )?;
635 if let Some(weights) = &b.weights {
636 if weights.len() != b.control_points.len() {
637 return Err(GeomError::InvalidInput(format!(
638 "B-spline has {} weights for {} control points",
639 weights.len(),
640 b.control_points.len()
641 )));
642 }
643 if weights
644 .iter()
645 .any(|weight| !weight.is_finite() || *weight <= 0.0)
646 {
647 return Err(GeomError::InvalidInput(
648 "B-spline weights must be finite and strictly positive".to_owned(),
649 ));
650 }
651 }
652 let t = axis.clamp(t);
653 let span = span_in(&axis.knots, axis.count, axis.degree, t);
654 Ok((axis.knots, span, axis.degree))
655}
656
657fn finite_control_points<P, const N: usize, F>(
661 control_points: &[P],
662 to: &F,
663) -> GeomResult<Vec<[Scalar; N]>>
664where
665 F: Fn(&P) -> [Scalar; N],
666{
667 let points: Vec<[Scalar; N]> = control_points.iter().map(to).collect();
668 if points
669 .iter()
670 .flatten()
671 .any(|coordinate| !coordinate.is_finite())
672 {
673 return Err(GeomError::InvalidInput(
674 "B-spline control points must be finite".to_owned(),
675 ));
676 }
677 Ok(points)
678}
679
680fn de_boor<P, const N: usize, F, G, Q>(
686 b: &BSplineCurve<P>,
687 t: Scalar,
688 to: F,
689 from: G,
690) -> GeomResult<Q>
691where
692 F: Fn(&P) -> [Scalar; N],
693 G: Fn([Scalar; N]) -> Q,
694{
695 let (knots, span, d) = spline_span(b, t)?;
696 let control_points = finite_control_points(&b.control_points, &to)?;
697 let u = t.clamp(knots[d], knots[b.control_points.len()]);
698
699 let mut work: Vec<[Scalar; N]> = Vec::with_capacity(d + 1);
702 let mut weights: Vec<Scalar> = Vec::with_capacity(d + 1);
703 for j in 0..=d {
704 let idx = span - d + j;
705 let w = b.weights.as_ref().map_or(1.0, |ws| ws[idx]);
706 let c = control_points[idx];
707 let homogeneous = core::array::from_fn(|k| c[k] * w);
710 if homogeneous.iter().any(|value| !value.is_finite()) {
711 return Err(GeomError::Degenerate(
712 "B-spline homogeneous control point overflowed".to_owned(),
713 ));
714 }
715 work.push(homogeneous);
716 weights.push(w);
717 }
718
719 de_boor_recurrence(&knots, span, d, u, &mut work, &mut weights);
722
723 let w = weights[d];
724 if !w.is_finite() || w == 0.0 {
725 return Err(GeomError::Degenerate(
726 "B-spline weight collapsed to zero".to_owned(),
727 ));
728 }
729 Ok(from(core::array::from_fn(|k| work[d][k] / w)))
730}
731
732fn de_boor_derivative<P, const N: usize, F, G, Q>(
741 b: &BSplineCurve<P>,
742 t: Scalar,
743 to: F,
744 from: G,
745) -> GeomResult<Q>
746where
747 F: Fn(&P) -> [Scalar; N],
748 G: Fn([Scalar; N]) -> Q,
749{
750 let (knots, _, d) = spline_span(b, t)?;
751 let control_points = finite_control_points(&b.control_points, &to)?;
752 let n = b.control_points.len();
753 let u = t.clamp(knots[d], knots[n]);
754
755 let hom: Vec<[Scalar; N]> = (0..n)
757 .map(|i| {
758 let w = b.weights.as_ref().map_or(1.0, |ws| ws[i]);
759 let c = control_points[i];
760 core::array::from_fn(|k| c[k] * w)
761 })
762 .collect();
763 if hom.iter().flatten().any(|value| !value.is_finite()) {
764 return Err(GeomError::Degenerate(
765 "B-spline homogeneous control point overflowed".to_owned(),
766 ));
767 }
768 let hw: Vec<Scalar> = (0..n)
769 .map(|i| b.weights.as_ref().map_or(1.0, |ws| ws[i]))
770 .collect();
771
772 let mut dhom: Vec<[Scalar; N]> = Vec::with_capacity(n - 1);
774 let mut dhw: Vec<Scalar> = Vec::with_capacity(n - 1);
775 for i in 0..n - 1 {
776 let denom = knots[i + d + 1] - knots[i + 1];
777 let f = if denom.abs() > 0.0 {
778 d as Scalar / denom
779 } else {
780 0.0
781 };
782 dhom.push(core::array::from_fn(|k| (hom[i + 1][k] - hom[i][k]) * f));
783 dhw.push((hw[i + 1] - hw[i]) * f);
784 }
785
786 let dknots = &knots[1..knots.len() - 1];
788 let (da, dw) = eval_homogeneous(dknots, d - 1, &dhom, &dhw, u);
789 let (a, w) = eval_homogeneous(&knots, d, &hom, &hw, u);
791
792 if !w.is_finite() || w == 0.0 {
793 return Err(GeomError::Degenerate(
794 "B-spline weight collapsed to zero".to_owned(),
795 ));
796 }
797 Ok(from(core::array::from_fn(|k| {
799 (da[k] - (a[k] / w) * dw) / w
800 })))
801}
802
803fn de_boor_second_derivative<P, const N: usize, F, G, Q>(
808 b: &BSplineCurve<P>,
809 t: Scalar,
810 to: F,
811 from: G,
812) -> GeomResult<Q>
813where
814 F: Fn(&P) -> [Scalar; N],
815 G: Fn([Scalar; N]) -> Q,
816{
817 let (knots, _, degree) = spline_span(b, t)?;
818 let control_points = finite_control_points(&b.control_points, &to)?;
819 let count = b.control_points.len();
820 let u = t.clamp(knots[degree], knots[count]);
821
822 let points: Vec<[Scalar; N]> = (0..count)
823 .map(|i| {
824 let weight = b.weights.as_ref().map_or(1.0, |weights| weights[i]);
825 core::array::from_fn(|axis| control_points[i][axis] * weight)
826 })
827 .collect();
828 if points.iter().flatten().any(|value| !value.is_finite()) {
829 return Err(GeomError::Degenerate(
830 "B-spline homogeneous control point overflowed".to_owned(),
831 ));
832 }
833 let weights: Vec<Scalar> = (0..count)
834 .map(|i| b.weights.as_ref().map_or(1.0, |values| values[i]))
835 .collect();
836
837 let (point, weight) = eval_homogeneous(&knots, degree, &points, &weights, u);
838 if !weight.is_finite() || weight == 0.0 {
839 return Err(GeomError::Degenerate(
840 "B-spline weight collapsed to zero".to_owned(),
841 ));
842 }
843
844 let (first_points, first_weights) = derivative_controls(&points, &weights, &knots, degree);
845 let first_knots = &knots[1..knots.len() - 1];
846 let (first, first_weight) =
847 eval_homogeneous(first_knots, degree - 1, &first_points, &first_weights, u);
848 let position: [Scalar; N] = core::array::from_fn(|axis| point[axis] / weight);
849 let first_projected: [Scalar; N] =
850 core::array::from_fn(|axis| (first[axis] - position[axis] * first_weight) / weight);
851
852 let (second, second_weight) = if degree == 1 {
853 ([0.0; N], 0.0)
854 } else {
855 let (second_points, second_weights) =
856 derivative_controls(&first_points, &first_weights, first_knots, degree - 1);
857 let second_knots = &first_knots[1..first_knots.len() - 1];
858 eval_homogeneous(second_knots, degree - 2, &second_points, &second_weights, u)
859 };
860 Ok(from(core::array::from_fn(|axis| {
861 (second[axis] - 2.0 * first_weight * first_projected[axis] - second_weight * position[axis])
862 / weight
863 })))
864}
865
866fn derivative_controls<const N: usize>(
868 points: &[[Scalar; N]],
869 weights: &[Scalar],
870 knots: &[Scalar],
871 degree: usize,
872) -> (Vec<[Scalar; N]>, Vec<Scalar>) {
873 let mut derivative_points = Vec::with_capacity(points.len() - 1);
874 let mut derivative_weights = Vec::with_capacity(weights.len() - 1);
875 for i in 0..points.len() - 1 {
876 let denominator = knots[i + degree + 1] - knots[i + 1];
877 let factor = if denominator.abs() > 0.0 {
878 degree as Scalar / denominator
879 } else {
880 0.0
881 };
882 derivative_points.push(core::array::from_fn(|axis| {
883 (points[i + 1][axis] - points[i][axis]) * factor
884 }));
885 derivative_weights.push((weights[i + 1] - weights[i]) * factor);
886 }
887 (derivative_points, derivative_weights)
888}
889
890pub(crate) fn eval_homogeneous<const N: usize>(
892 knots: &[Scalar],
893 d: usize,
894 hom: &[[Scalar; N]],
895 hw: &[Scalar],
896 u: Scalar,
897) -> ([Scalar; N], Scalar) {
898 let n = hom.len();
899 if d == 0 {
900 let mut idx = 0;
902 for (k, knot) in knots.iter().enumerate().take(n) {
903 if *knot <= u {
904 idx = k;
905 }
906 }
907 return (hom[idx.min(n - 1)], hw[idx.min(n - 1)]);
908 }
909 let mut span = d;
910 for (k, knot) in knots.iter().enumerate().take(n).skip(d) {
911 if *knot <= u {
912 span = k;
913 } else {
914 break;
915 }
916 }
917 let mut work: Vec<[Scalar; N]> = (0..=d).map(|j| hom[span - d + j]).collect();
918 let mut weights: Vec<Scalar> = (0..=d).map(|j| hw[span - d + j]).collect();
919 for r in 1..=d {
920 for j in (r..=d).rev() {
921 let i = span - d + j;
922 let denom = knots[i + d + 1 - r] - knots[i];
923 let alpha = if denom.abs() > 0.0 {
924 (u - knots[i]) / denom
925 } else {
926 0.0
927 };
928 for k in 0..N {
929 work[j][k] = work[j - 1][k] * (1.0 - alpha) + work[j][k] * alpha;
930 }
931 weights[j] = weights[j - 1] * (1.0 - alpha) + weights[j] * alpha;
932 }
933 }
934 (work[d], weights[d])
935}
936
937pub fn flatten2(
954 curve: &Curve2,
955 domain: Interval,
956 chord_tolerance: Scalar,
957 max_depth: u32,
958) -> GeomResult<Vec<Point2>> {
959 const MAX_POINTS: usize = 1 << 16;
963 if !(chord_tolerance.is_finite()
964 && chord_tolerance.is_sign_positive()
965 && chord_tolerance != 0.0)
966 {
967 return Err(GeomError::InvalidInput(format!(
968 "chord tolerance must be positive and finite, got {chord_tolerance}"
969 )));
970 }
971 if let Curve2::Line(_) = curve {
974 return Ok(vec![
975 evaluate2(curve, domain.start)?,
976 evaluate2(curve, domain.end)?,
977 ]);
978 }
979 if let Curve2::Polyline(p) = curve {
980 let natural = polyline_domain(p.points.len(), p.closed);
986 let requested = (domain.end - domain.start).abs();
987 if natural.end > 1.0 && requested <= 1.0 {
988 return Err(GeomError::InvalidInput(format!(
989 "polyline domain {:?} spans {requested} of {} segments; a \
990 polyline parameter is one unit per segment, so this would \
991 discard {} vertices",
992 domain,
993 natural.end,
994 p.points.len().saturating_sub(2)
995 )));
996 }
997 return polyline_flatten(&p.points, p.closed, domain, |t| evaluate2(curve, t));
998 }
999
1000 let mut out = vec![evaluate2(curve, domain.start)?];
1001 let eval = |t| evaluate2(curve, t);
1002 subdivide(
1003 &eval,
1004 domain.start,
1005 domain.end,
1006 chord_tolerance,
1007 max_depth.min(MAX_DEPTH_CEILING),
1008 MAX_POINTS,
1009 &mut out,
1010 )?;
1011 out.push(evaluate2(curve, domain.end)?);
1012 Ok(out)
1013}
1014
1015const MAX_DEPTH_CEILING: u32 = 20;
1020
1021trait ChordPoint: Copy {
1034 fn sub(self, other: Self) -> Self;
1035 fn add_scaled(self, direction: Self, scale: Scalar) -> Self;
1036 fn dot(self, other: Self) -> Scalar;
1037 fn length(self) -> Scalar;
1038 fn length_squared(self) -> Scalar;
1039}
1040
1041impl ChordPoint for Point2 {
1042 fn sub(self, other: Self) -> Self {
1043 self - other
1044 }
1045 fn add_scaled(self, direction: Self, scale: Scalar) -> Self {
1046 self + direction * scale
1047 }
1048 fn dot(self, other: Self) -> Scalar {
1049 Point2::dot(self, other)
1050 }
1051 fn length(self) -> Scalar {
1052 Point2::length(self)
1053 }
1054 fn length_squared(self) -> Scalar {
1055 Point2::length_squared(self)
1056 }
1057}
1058
1059impl ChordPoint for Point3 {
1060 fn sub(self, other: Self) -> Self {
1061 self - other
1062 }
1063 fn add_scaled(self, direction: Self, scale: Scalar) -> Self {
1064 self + direction * scale
1065 }
1066 fn dot(self, other: Self) -> Scalar {
1067 Point3::dot(self, other)
1068 }
1069 fn length(self) -> Scalar {
1070 Point3::length(self)
1071 }
1072 fn length_squared(self) -> Scalar {
1073 Point3::length_squared(self)
1074 }
1075}
1076
1077fn sagitta<P: ChordPoint>(a: P, b: P, m: P) -> Scalar {
1079 let ab = b.sub(a);
1080 let len2 = ab.length_squared();
1081 if len2 <= 0.0 {
1082 return m.sub(a).length();
1085 }
1086 let t = (m.sub(a).dot(ab) / len2).clamp(0.0, 1.0);
1087 m.sub(a.add_scaled(ab, t)).length()
1088}
1089
1090fn subdivide<P, F>(
1097 eval: &F,
1098 a: Scalar,
1099 b: Scalar,
1100 tol: Scalar,
1101 depth: u32,
1102 budget: usize,
1103 out: &mut Vec<P>,
1104) -> GeomResult<()>
1105where
1106 P: ChordPoint,
1107 F: Fn(Scalar) -> GeomResult<P>,
1108{
1109 if out.len() >= budget {
1110 return Err(GeomError::Degenerate(format!(
1111 "curve flattening exceeded {budget} points before meeting the \
1112 chord tolerance {tol}; the curve may be degenerate"
1113 )));
1114 }
1115 let mid = 0.5 * (a + b);
1116 let pa = eval(a)?;
1117 let pb = eval(b)?;
1118 if !(mid > a && mid < b) {
1124 if sagitta(pa, pb, pa) <= tol && (pb.sub(pa)).length() <= tol {
1125 return Ok(());
1126 }
1127 return Err(GeomError::Degenerate(format!(
1128 "curve parameter interval ({a}, {b}) is too small to bisect but \
1129 its chord still exceeds the tolerance {tol}"
1130 )));
1131 }
1132 let pm = eval(mid)?;
1133 if sagitta(pa, pb, pm) <= tol {
1134 return Ok(());
1136 }
1137 if depth == 0 {
1138 return Err(GeomError::BudgetExceeded {
1139 resource: "curve flattening depth",
1140 });
1141 }
1142 subdivide(eval, a, mid, tol, depth - 1, budget, out)?;
1143 out.push(pm);
1144 subdivide(eval, mid, b, tol, depth - 1, budget, out)?;
1145 Ok(())
1146}
1147
1148fn polyline_flatten<P, F>(
1150 points: &[P],
1151 closed: bool,
1152 domain: Interval,
1153 eval: F,
1154) -> GeomResult<Vec<P>>
1155where
1156 P: Copy,
1157 F: Fn(Scalar) -> GeomResult<P>,
1158{
1159 let segments = if closed {
1160 points.len()
1161 } else {
1162 points.len().saturating_sub(1)
1163 };
1164 if segments == 0 {
1165 return Err(GeomError::Degenerate(
1166 "polyline has no evaluable segment".to_owned(),
1167 ));
1168 }
1169 let lo = domain.start.min(domain.end);
1170 let hi = domain.start.max(domain.end);
1171 let mut out = vec![eval(lo)?];
1172 let first = lo.floor() as i64 + 1;
1174 let last = hi.ceil() as i64 - 1;
1175 for k in first..=last {
1176 let t = k as Scalar;
1177 if t > lo && t < hi {
1178 out.push(eval(t)?);
1179 }
1180 }
1181 out.push(eval(hi)?);
1182 Ok(out)
1183}
1184
1185impl CurveEvaluator<Curve2> for ScalarCurve {
1188 type Point = Point2;
1189 type Derivative = Vec2;
1190 type Error = GeomError;
1191
1192 fn domain(&self, curve: &Curve2) -> Interval {
1193 domain2(curve)
1194 }
1195
1196 fn evaluate(
1197 &self,
1198 curve: &Curve2,
1199 t: Scalar,
1200 _tolerance: Tolerance,
1201 ) -> Result<Self::Point, Self::Error> {
1202 evaluate2(curve, t)
1203 }
1204
1205 fn derivative(
1206 &self,
1207 curve: &Curve2,
1208 t: Scalar,
1209 _tolerance: Tolerance,
1210 ) -> Result<Self::Derivative, Self::Error> {
1211 derivative2(curve, t)
1212 }
1213}
1214
1215impl CurveEvaluator<Curve3> for ScalarCurve {
1216 type Point = Point3;
1217 type Derivative = Vec3;
1218 type Error = GeomError;
1219
1220 fn domain(&self, curve: &Curve3) -> Interval {
1221 domain3(curve)
1222 }
1223
1224 fn evaluate(
1225 &self,
1226 curve: &Curve3,
1227 t: Scalar,
1228 _tolerance: Tolerance,
1229 ) -> Result<Self::Point, Self::Error> {
1230 evaluate3(curve, t)
1231 }
1232
1233 fn derivative(
1234 &self,
1235 curve: &Curve3,
1236 t: Scalar,
1237 _tolerance: Tolerance,
1238 ) -> Result<Self::Derivative, Self::Error> {
1239 derivative3(curve, t)
1240 }
1241}
1242
1243#[allow(unused)]
1245fn _type_anchors(_: Circle2, _: Circle3, _: Ellipse2, _: Ellipse3, _: Line2, _: Line3) {}
1246#[allow(unused)]
1247fn _poly_anchors(_: Polyline2, _: Polyline3) {}
1248
1249pub fn flatten3(
1257 curve: &Curve3,
1258 domain: Interval,
1259 chord_tolerance: Scalar,
1260 max_depth: u32,
1261) -> GeomResult<Vec<Point3>> {
1262 const MAX_POINTS: usize = 1 << 16;
1266 if !(chord_tolerance.is_finite()
1267 && chord_tolerance.is_sign_positive()
1268 && chord_tolerance != 0.0)
1269 {
1270 return Err(GeomError::InvalidInput(format!(
1271 "chord tolerance must be positive and finite, got {chord_tolerance}"
1272 )));
1273 }
1274 if let Curve3::Line(_) = curve {
1277 return Ok(vec![
1278 evaluate3(curve, domain.start)?,
1279 evaluate3(curve, domain.end)?,
1280 ]);
1281 }
1282 if let Curve3::Polyline(p) = curve {
1283 let natural = polyline_domain(p.points.len(), p.closed);
1287 let requested = (domain.end - domain.start).abs();
1288 if natural.end > 1.0 && requested <= 1.0 {
1289 return Err(GeomError::InvalidInput(format!(
1290 "polyline domain {:?} spans {requested} of {} segments; a \
1291 polyline parameter is one unit per segment, so this would \
1292 discard {} vertices",
1293 domain,
1294 natural.end,
1295 p.points.len().saturating_sub(2)
1296 )));
1297 }
1298 return polyline_flatten(&p.points, p.closed, domain, |t| evaluate3(curve, t));
1299 }
1300
1301 let eval = |t| evaluate3(curve, t);
1302 let mut out = vec![eval(domain.start)?];
1303 subdivide(
1304 &eval,
1305 domain.start,
1306 domain.end,
1307 chord_tolerance,
1308 max_depth.min(MAX_DEPTH_CEILING),
1309 MAX_POINTS,
1310 &mut out,
1311 )?;
1312 out.push(eval(domain.end)?);
1313 Ok(out)
1314}
1315
1316fn no_closed_form_inversion() -> GeomError {
1321 GeomError::InvalidInput(
1322 "curve family has no closed form inversion; a point trim on this basis \
1323 would require iteration, which trim resolution does not perform"
1324 .to_owned(),
1325 )
1326}
1327
1328fn point_not_on_curve(distance: Scalar, tolerance: Scalar) -> GeomError {
1330 GeomError::InvalidInput(format!(
1331 "point is {distance} from the curve, outside the {tolerance} tolerance; \
1332 refusing rather than projecting it onto the nearest parameter"
1333 ))
1334}
1335
1336fn invert_line(
1342 origin: impl Into<[Scalar; 3]>,
1343 direction: [Scalar; 3],
1344 point: [Scalar; 3],
1345 tolerance: Scalar,
1346) -> GeomResult<Scalar> {
1347 let origin = origin.into();
1348 let dd = direction.iter().map(|c| c * c).sum::<Scalar>();
1349 if !dd.is_finite() || dd <= Scalar::EPSILON {
1350 return Err(GeomError::InvalidInput(
1351 "line direction is degenerate, so no parameter names a point".to_owned(),
1352 ));
1353 }
1354 let offset = [
1355 point[0] - origin[0],
1356 point[1] - origin[1],
1357 point[2] - origin[2],
1358 ];
1359 let t = offset
1360 .iter()
1361 .zip(direction.iter())
1362 .map(|(o, d)| o * d)
1363 .sum::<Scalar>()
1364 / dd;
1365 let residual = [
1368 offset[0] - direction[0] * t,
1369 offset[1] - direction[1] * t,
1370 offset[2] - direction[2] * t,
1371 ];
1372 let distance = residual.iter().map(|c| c * c).sum::<Scalar>().sqrt();
1373 if distance > tolerance {
1374 return Err(point_not_on_curve(distance, tolerance));
1375 }
1376 Ok(t)
1377}
1378
1379fn invert_conic(
1385 local_x: Scalar,
1386 local_y: Scalar,
1387 semi_x: Scalar,
1388 semi_y: Scalar,
1389) -> GeomResult<Scalar> {
1390 if !(semi_x.is_finite() && semi_y.is_finite()) || semi_x <= 0.0 || semi_y <= 0.0 {
1391 return Err(GeomError::InvalidInput(
1392 "conic semi-axes must be finite and positive to invert a point".to_owned(),
1393 ));
1394 }
1395 let angle = (local_y / semi_y).atan2(local_x / semi_x);
1396 if !angle.is_finite() {
1397 return Err(GeomError::InvalidInput(
1398 "conic inversion produced a non-finite angle".to_owned(),
1399 ));
1400 }
1401 Ok(angle.rem_euclid(std::f64::consts::TAU))
1404}
1405
1406pub fn invert2(curve: &Curve2, point: Point2, tolerance: Tolerance) -> GeomResult<Scalar> {
1418 finite2(point, "inversion point")?;
1419 let linear = tolerance.linear();
1420 match curve {
1421 Curve2::Line(l) => invert_line(
1422 [l.origin.x, l.origin.y, 0.0],
1423 [l.direction.x, l.direction.y, 0.0],
1424 [point.x, point.y, 0.0],
1425 linear,
1426 ),
1427 Curve2::Circle(c) => {
1428 let t = invert_conic_in_frame2(&c.frame, point, c.radius, c.radius)?;
1429 verify2(curve, t, point, linear)
1430 }
1431 Curve2::Ellipse(e) => {
1432 let t = invert_conic_in_frame2(&e.frame, point, e.semi_axis_x, e.semi_axis_y)?;
1433 verify2(curve, t, point, linear)
1434 }
1435 Curve2::Sinusoid(_) | Curve2::QuadraticGraph(_) => verify2(curve, point.x, point, linear),
1438 Curve2::AngleGraph(g) => {
1441 let u = g.angle(point.y).ok_or_else(|| outside_graph(point.y))?;
1442 let turns = ((point.x - u) / std::f64::consts::TAU).round();
1443 let shifted = Point2::new(point.x - turns * std::f64::consts::TAU, point.y);
1444 verify2(curve, point.y, shifted, linear)
1445 }
1446 Curve2::Lifted(l) => {
1448 let lifted = l.carrier.jet(point.x, point.y).point;
1449 let t = invert3(&l.curve, lifted, tolerance)?;
1450 verify2(curve, t, point, linear)
1451 }
1452 Curve2::Implicit(c) => {
1454 let t = c
1455 .parameter_of(point)
1456 .ok_or_else(|| point_not_on_curve(Scalar::INFINITY, linear))?;
1457 verify2(curve, t, point, linear)
1458 }
1459 _ => Err(no_closed_form_inversion()),
1460 }
1461}
1462
1463fn invert_conic_in_frame2(
1465 frame: &axiolid_core::Frame2,
1466 point: Point2,
1467 semi_x: Scalar,
1468 semi_y: Scalar,
1469) -> GeomResult<Scalar> {
1470 let offset = point - frame.origin;
1471 invert_conic(offset.dot(frame.x), offset.dot(frame.y), semi_x, semi_y)
1472}
1473
1474fn verify2(curve: &Curve2, t: Scalar, point: Point2, tolerance: Scalar) -> GeomResult<Scalar> {
1481 let found = evaluate2(curve, t)?;
1482 let distance = (found - point).length();
1483 if distance > tolerance {
1484 return Err(point_not_on_curve(distance, tolerance));
1485 }
1486 Ok(t)
1487}
1488
1489pub fn invert3(curve: &Curve3, point: Point3, tolerance: Tolerance) -> GeomResult<Scalar> {
1497 finite3(point, "inversion point")?;
1498 let linear = tolerance.linear();
1499 match curve {
1500 Curve3::Line(l) => invert_line(
1501 [l.origin.x, l.origin.y, l.origin.z],
1502 [l.direction.x, l.direction.y, l.direction.z],
1503 [point.x, point.y, point.z],
1504 linear,
1505 ),
1506 Curve3::Circle(c) => {
1507 let t = invert_conic_in_frame3(&c.frame, point, c.radius, c.radius)?;
1508 verify3(curve, t, point, linear)
1509 }
1510 Curve3::Ellipse(e) => {
1511 let t = invert_conic_in_frame3(&e.frame, point, e.semi_axis_x, e.semi_axis_y)?;
1512 verify3(curve, t, point, linear)
1513 }
1514 Curve3::RuledSection(r) => {
1517 let carrier = axiolid_curve::Carrier::Ruled(r.carrier);
1518 let (u, _) = carrier.parameters(point);
1519 first_on_curve(curve, &turns_of(u), point, linear)
1520 }
1521 Curve3::TorusSection(r) => {
1522 let carrier = axiolid_curve::Carrier::Torus(r.torus);
1523 let (_, v) = carrier.parameters(point);
1524 first_on_curve(curve, &turns_of(v), point, linear)
1525 }
1526 Curve3::PairSection(r) => {
1528 let t = r
1529 .parameter_of(point)
1530 .ok_or_else(|| point_not_on_curve(Scalar::INFINITY, linear))?;
1531 verify3(curve, t, point, linear)
1532 }
1533 Curve3::ImplicitSection(r) => {
1536 if let axiolid_curve::Carrier::Spline(b) = &r.carrier {
1539 let surface = axiolid_surface::Surface::BSpline((**b).clone());
1540 let (u, v) = crate::surface::locate(&surface, point, tolerance)?;
1541 let t = r
1542 .curve
1543 .parameter_of(Point2::new(u, v))
1544 .ok_or_else(|| point_not_on_curve(Scalar::INFINITY, linear))?;
1545 return verify3(curve, t, point, linear);
1546 }
1547 let (u, v) = r.carrier.parameters(point);
1548 let (pu, pv) = r.carrier.periodic();
1549 let turns = |periodic: bool| -> &'static [Scalar] {
1550 if periodic {
1551 &[0.0, 1.0, -1.0, 2.0, -2.0]
1552 } else {
1553 &[0.0]
1554 }
1555 };
1556 let mut candidates = Vec::new();
1557 for &ku in turns(pu) {
1558 for &kv in turns(pv) {
1559 let tau = std::f64::consts::TAU;
1560 if let Some(t) = r
1561 .curve
1562 .parameter_of(Point2::new(u + ku * tau, v + kv * tau))
1563 {
1564 candidates.push(t);
1565 }
1566 }
1567 }
1568 first_on_curve(curve, &candidates, point, linear)
1569 }
1570 _ => Err(no_closed_form_inversion()),
1571 }
1572}
1573
1574fn turns_of(angle: Scalar) -> Vec<Scalar> {
1576 let tau = std::f64::consts::TAU;
1577 vec![
1578 angle,
1579 angle + tau,
1580 angle - tau,
1581 angle + 2.0 * tau,
1582 angle - 2.0 * tau,
1583 ]
1584}
1585
1586fn first_on_curve(
1588 curve: &Curve3,
1589 candidates: &[Scalar],
1590 point: Point3,
1591 tolerance: Scalar,
1592) -> GeomResult<Scalar> {
1593 let mut nearest = Scalar::INFINITY;
1594 for &t in candidates {
1595 if let Ok(found) = evaluate3(curve, t) {
1596 let distance = (found - point).length();
1597 if distance <= tolerance {
1598 return Ok(t);
1599 }
1600 nearest = nearest.min(distance);
1601 }
1602 }
1603 Err(point_not_on_curve(nearest, tolerance))
1604}
1605
1606pub fn locate3(curve: &Curve3, point: Point3, tolerance: Tolerance) -> GeomResult<Scalar> {
1615 match invert3(curve, point, tolerance) {
1616 Ok(t) => Ok(t),
1617 Err(error) => match curve {
1618 Curve3::BSpline(b) => {
1619 let domain = spline_domain(b);
1620 let t = nearest_parameter(
1621 domain,
1622 b.control_points.len().max(2) * 64,
1623 |t| evaluate3(curve, t).map(|p| (p - point).length()),
1624 |t| {
1625 let (p, d, dd) = (
1626 evaluate3(curve, t)?,
1627 derivative3(curve, t)?,
1628 second_derivative3(curve, t)?,
1629 );
1630 let r = p - point;
1631 Ok((r.dot(d), d.dot(d) + r.dot(dd)))
1632 },
1633 )?;
1634 verify3(curve, t, point, tolerance.linear())
1635 }
1636 _ => Err(error),
1637 },
1638 }
1639}
1640
1641pub fn locate2(curve: &Curve2, point: Point2, tolerance: Tolerance) -> GeomResult<Scalar> {
1648 match invert2(curve, point, tolerance) {
1649 Ok(t) => Ok(t),
1650 Err(error) => match curve {
1651 Curve2::BSpline(b) => {
1652 let domain = spline_domain(b);
1653 let t = nearest_parameter(
1654 domain,
1655 b.control_points.len().max(2) * 64,
1656 |t| evaluate2(curve, t).map(|p| (p - point).length()),
1657 |t| {
1658 let (p, d, dd) = (
1659 evaluate2(curve, t)?,
1660 derivative2(curve, t)?,
1661 second_derivative2(curve, t)?,
1662 );
1663 let r = p - point;
1664 Ok((r.dot(d), d.dot(d) + r.dot(dd)))
1665 },
1666 )?;
1667 verify2(curve, t, point, tolerance.linear())
1668 }
1669 _ => Err(error),
1670 },
1671 }
1672}
1673
1674fn nearest_parameter(
1678 domain: Interval,
1679 samples: usize,
1680 distance: impl Fn(Scalar) -> GeomResult<Scalar>,
1681 gradient: impl Fn(Scalar) -> GeomResult<(Scalar, Scalar)>,
1682) -> GeomResult<Scalar> {
1683 let (lo, hi) = (domain.start.min(domain.end), domain.start.max(domain.end));
1684 let mut best = (Scalar::INFINITY, lo);
1685 for i in 0..=samples {
1686 let t = lo + (hi - lo) * i as Scalar / samples as Scalar;
1687 let d = distance(t)?;
1688 if d < best.0 {
1689 best = (d, t);
1690 }
1691 }
1692 let mut t = best.1;
1693 for _ in 0..60 {
1694 let (g, h) = gradient(t)?;
1695 if h <= 0.0 || !h.is_finite() {
1696 break;
1697 }
1698 let next = (t - g / h).clamp(lo, hi);
1699 let moved = (next - t).abs();
1700 t = next;
1701 if moved <= 4.0 * Scalar::EPSILON * (1.0 + t.abs()) {
1702 break;
1703 }
1704 }
1705 Ok(t)
1706}
1707
1708fn invert_conic_in_frame3(
1710 frame: &axiolid_core::Frame3,
1711 point: Point3,
1712 semi_x: Scalar,
1713 semi_y: Scalar,
1714) -> GeomResult<Scalar> {
1715 let offset = point - frame.origin;
1716 invert_conic(offset.dot(frame.x), offset.dot(frame.y), semi_x, semi_y)
1717}
1718
1719fn verify3(curve: &Curve3, t: Scalar, point: Point3, tolerance: Scalar) -> GeomResult<Scalar> {
1721 let found = evaluate3(curve, t)?;
1722 let distance = (found - point).length();
1723 if distance > tolerance {
1724 return Err(point_not_on_curve(distance, tolerance));
1725 }
1726 Ok(t)
1727}