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 Curve2::Chain(c) => c.length().map_or(
96 Interval {
97 start: 0.0,
98 end: 0.0,
99 },
100 |end| Interval { start: 0.0, end },
101 ),
102 _ => Interval {
104 start: 0.0,
105 end: 0.0,
106 },
107 }
108}
109
110#[must_use]
112pub fn domain3(curve: &Curve3) -> Interval {
113 match curve {
114 Curve3::Line(_) => Interval::UNIT,
115 Curve3::Circle(_) | Curve3::Ellipse(_) => full_turn(),
116 Curve3::Polyline(p) => polyline_domain(p.points.len(), p.closed),
117 Curve3::BSpline(b) => spline_domain(b),
118 Curve3::Intrinsic(i) if i.length.is_finite() && i.length > 0.0 => Interval {
122 start: 0.0,
123 end: i.length,
124 },
125 Curve3::Banked(b) if b.cant.is_well_formed() => Interval {
128 start: 0.0,
129 end: b.span(),
130 },
131 _ => Interval {
132 start: 0.0,
133 end: 0.0,
134 },
135 }
136}
137
138fn full_turn() -> Interval {
139 Interval {
140 start: 0.0,
141 end: core::f64::consts::TAU,
142 }
143}
144
145fn polyline_domain(count: usize, closed: bool) -> Interval {
147 let segments = if closed {
148 count
149 } else {
150 count.saturating_sub(1)
151 };
152 Interval {
153 start: 0.0,
154 end: segments as Scalar,
155 }
156}
157
158fn spline_domain<P>(b: &BSplineCurve<P>) -> Interval {
161 SplineAxis::new(
162 &b.knots,
163 &b.multiplicities,
164 b.degree,
165 b.control_points.len(),
166 "B-spline curve",
167 )
168 .map_or(
169 Interval {
170 start: 0.0,
171 end: 0.0,
172 },
173 |axis| {
174 let (start, end) = axis.domain();
175 Interval { start, end }
176 },
177 )
178}
179
180pub fn evaluate2(curve: &Curve2, t: Scalar) -> GeomResult<Point2> {
184 finite(t)?;
185 let value = match curve {
186 Curve2::Line(l) => Ok(line_point(l.origin, l.direction, t)),
187 Curve2::Circle(c) => Ok(conic_point2(&c.frame, c.radius, c.radius, t)),
188 Curve2::Ellipse(e) => Ok(conic_point2(&e.frame, e.semi_axis_x, e.semi_axis_y, t)),
189 Curve2::Polyline(p) => polyline_point(&p.points, p.closed, t),
190 Curve2::BSpline(b) => de_boor(b, t, |p| [p.x, p.y], |c| Point2::new(c[0], c[1])),
191 Curve2::Intrinsic(i) => crate::arc_length::intrinsic_point(i, t),
195 Curve2::Chain(c) => crate::chain::chain_point(c, t),
197 Curve2::Sinusoid(w) => Ok(Point2::new(t, w.height(t))),
199 Curve2::QuadraticGraph(g) => g
201 .height(t)
202 .map(|v| Point2::new(t, v))
203 .ok_or_else(|| outside_graph(t)),
204 Curve2::AngleGraph(g) => g
206 .angle(t)
207 .map(|u| Point2::new(u, t))
208 .ok_or_else(|| outside_graph(t)),
209 Curve2::Implicit(c) => c.point(t).ok_or_else(|| outside_graph(t)),
211 Curve2::Lifted(l) => {
213 if let Some((section, first)) = pair_side(l) {
215 let (a, b, _) = section.solve(t).ok_or_else(|| outside_graph(t))?;
216 return finite2(if first { a } else { b }, "curve point");
217 }
218 let p = evaluate3(&l.curve, t)?;
219 l.unwrap_at(t, p).ok_or_else(|| outside_graph(t))
220 }
221 _ => Err(GeomError::Unsupported {
224 backend: axiolid_contracts::BackendId::new("axiolid-reference"),
225 operation: axiolid_contracts::Operation::CurveEvaluation,
226 }),
227 }?;
228 finite2(value, "curve point")
229}
230
231pub fn derivative2(curve: &Curve2, t: Scalar) -> GeomResult<Vec2> {
233 finite(t)?;
234 let value = match curve {
235 Curve2::Line(l) => Ok(l.direction),
236 Curve2::Circle(c) => Ok(conic_tangent2(&c.frame, c.radius, c.radius, t)),
237 Curve2::Ellipse(e) => Ok(conic_tangent2(&e.frame, e.semi_axis_x, e.semi_axis_y, t)),
238 Curve2::Polyline(p) => polyline_tangent(&p.points, p.closed, t),
239 Curve2::BSpline(b) => de_boor_derivative(b, t, |p| [p.x, p.y], |c| Vec2::new(c[0], c[1])),
240 Curve2::Intrinsic(i) => crate::arc_length::intrinsic_tangent(i, t),
244 Curve2::Chain(c) => crate::chain::chain_tangent(c, t),
245 Curve2::Sinusoid(w) => {
246 let (sin, cos) = t.sin_cos();
247 Ok(Vec2::new(1.0, -w.cosine * sin + w.sine * cos))
248 }
249 Curve2::QuadraticGraph(g) => g
250 .slope(t)
251 .map(|slope| Vec2::new(1.0, slope))
252 .ok_or_else(|| outside_graph(t)),
253 Curve2::AngleGraph(g) => g
254 .slope(t)
255 .map(|slope| Vec2::new(slope, 1.0))
256 .ok_or_else(|| outside_graph(t)),
257 Curve2::Implicit(c) => c.derivative(t).ok_or_else(|| outside_graph(t)),
258 Curve2::Lifted(l) => lifted_rates(l, t).map(|(d, _)| d),
259 _ => Err(GeomError::Unsupported {
262 backend: axiolid_contracts::BackendId::new("axiolid-reference"),
263 operation: axiolid_contracts::Operation::CurveEvaluation,
264 }),
265 }?;
266 finite2(value, "curve derivative")
267}
268
269pub fn second_derivative2(curve: &Curve2, t: Scalar) -> GeomResult<Vec2> {
271 finite(t)?;
272 let value = match curve {
273 Curve2::Line(_) | Curve2::Polyline(_) => Ok(Vec2::ZERO),
274 Curve2::Circle(c) => Ok(conic_second2(&c.frame, c.radius, c.radius, t)),
275 Curve2::Ellipse(e) => Ok(conic_second2(&e.frame, e.semi_axis_x, e.semi_axis_y, t)),
276 Curve2::BSpline(b) => {
277 de_boor_second_derivative(b, t, |p| [p.x, p.y], |c| Vec2::new(c[0], c[1]))
278 }
279 Curve2::Sinusoid(w) => {
280 let (sin, cos) = t.sin_cos();
281 Ok(Vec2::new(0.0, -w.cosine * cos - w.sine * sin))
282 }
283 Curve2::QuadraticGraph(g) => g
284 .bend(t)
285 .map(|bend| Vec2::new(0.0, bend))
286 .ok_or_else(|| outside_graph(t)),
287 Curve2::AngleGraph(g) => g
288 .bend(t)
289 .map(|bend| Vec2::new(bend, 0.0))
290 .ok_or_else(|| outside_graph(t)),
291 Curve2::Implicit(c) => c.second_derivative(t).ok_or_else(|| outside_graph(t)),
292 Curve2::Lifted(l) => lifted_rates(l, t).map(|(_, dd)| dd),
293 _ => Err(GeomError::Unsupported {
294 backend: axiolid_contracts::BackendId::new("axiolid-reference"),
295 operation: axiolid_contracts::Operation::CurveEvaluation,
296 }),
297 }?;
298 finite2(value, "curve second derivative")
299}
300
301pub fn bspline_jet2(curve: &BSplineCurve2, t: Scalar) -> GeomResult<CurveJet<Point2, Vec2>> {
303 Ok(CurveJet {
304 point: de_boor(curve, t, |p| [p.x, p.y], |c| Point2::new(c[0], c[1]))?,
305 first: de_boor_derivative(curve, t, |p| [p.x, p.y], |c| Vec2::new(c[0], c[1]))?,
306 second: de_boor_second_derivative(curve, t, |p| [p.x, p.y], |c| Vec2::new(c[0], c[1]))?,
307 })
308}
309
310pub fn jet2(curve: &Curve2, t: Scalar) -> GeomResult<CurveJet<Point2, Vec2>> {
312 Ok(CurveJet {
313 point: evaluate2(curve, t)?,
314 first: derivative2(curve, t)?,
315 second: second_derivative2(curve, t)?,
316 })
317}
318
319fn lifted_rates(l: &axiolid_curve::LiftedCurve2, t: Scalar) -> GeomResult<(Vec2, Vec2)> {
323 if let Some((section, first)) = pair_side(l) {
324 let (a, b, _) = section.rates(t).ok_or_else(|| outside_graph(t))?;
325 let (aa, bb, _) = section.second_rates(t).ok_or_else(|| outside_graph(t))?;
326 let pick = |x: Point2, y: Point2| if first { x } else { y };
327 let (d, dd) = (pick(a, b), pick(aa, bb));
328 return Ok((Vec2::new(d.x, d.y), Vec2::new(dd.x, dd.y)));
329 }
330 let p = evaluate3(&l.curve, t)?;
331 let uv = l.unwrap_at(t, p).ok_or_else(|| outside_graph(t))?;
332 let jet = l.carrier.jet(uv.x, uv.y);
333 let (a, b, c) = (jet.u.dot(jet.u), jet.u.dot(jet.v), jet.v.dot(jet.v));
334 let det = a * c - b * b;
335 if det == 0.0 || !det.is_finite() {
336 return Err(outside_graph(t));
337 }
338 let solve = |w: Vec3| {
339 let (g0, g1) = (jet.u.dot(w), jet.v.dot(w));
340 Vec2::new((c * g0 - b * g1) / det, (a * g1 - b * g0) / det)
341 };
342 let d = solve(derivative3(&l.curve, t)?);
343 let rest = second_derivative3(&l.curve, t)?
344 - (jet.uu * (d.x * d.x) + jet.uv * (2.0 * d.x * d.y) + jet.vv * (d.y * d.y));
345 Ok((d, solve(rest)))
346}
347
348fn pair_side(l: &axiolid_curve::LiftedCurve2) -> Option<(&axiolid_curve::PairSection3, bool)> {
351 match l.curve.as_ref() {
352 Curve3::PairSection(section) => section.side(&l.carrier).map(|first| (section, first)),
353 _ => None,
354 }
355}
356
357fn outside_graph(t: Scalar) -> GeomError {
360 GeomError::Degenerate(format!(
361 "quadratic section graph has no regular point at t = {t}"
362 ))
363}
364
365pub fn evaluate3(curve: &Curve3, t: Scalar) -> GeomResult<Point3> {
369 finite(t)?;
370 let value = match curve {
371 Curve3::Line(l) => Ok(line_point(l.origin, l.direction, t)),
372 Curve3::Circle(c) => Ok(conic_point3(&c.frame, c.radius, c.radius, t)),
373 Curve3::Ellipse(e) => Ok(conic_point3(&e.frame, e.semi_axis_x, e.semi_axis_y, t)),
374 Curve3::Polyline(p) => polyline_point(&p.points, p.closed, t),
375 Curve3::BSpline(b) => de_boor(b, t, |p| [p.x, p.y, p.z], |c| Point3::new(c[0], c[1], c[2])),
376 Curve3::Intrinsic(i) => crate::frenet::frenet_point(i, t),
381 Curve3::RuledSection(r) => r.point(t).ok_or_else(|| outside_graph(t)),
383 Curve3::TorusSection(r) => r.point(t).ok_or_else(|| outside_graph(t)),
384 Curve3::ImplicitSection(r) => r.point(t).ok_or_else(|| outside_graph(t)),
385 Curve3::PairSection(r) => r.point(t).ok_or_else(|| outside_graph(t)),
386 Curve3::Banked(b) => crate::banked::banked_point(b, t),
388 _ => Err(GeomError::Unsupported {
391 backend: axiolid_contracts::BackendId::new("axiolid-reference"),
392 operation: axiolid_contracts::Operation::CurveEvaluation,
393 }),
394 }?;
395 finite3(value, "curve point")
396}
397
398pub fn derivative3(curve: &Curve3, t: Scalar) -> GeomResult<Vec3> {
400 finite(t)?;
401 let value = match curve {
402 Curve3::Line(l) => Ok(l.direction),
403 Curve3::Circle(c) => Ok(conic_tangent3(&c.frame, c.radius, c.radius, t)),
404 Curve3::Ellipse(e) => Ok(conic_tangent3(&e.frame, e.semi_axis_x, e.semi_axis_y, t)),
405 Curve3::Polyline(p) => polyline_tangent(&p.points, p.closed, t),
406 Curve3::BSpline(b) => {
407 de_boor_derivative(b, t, |p| [p.x, p.y, p.z], |c| Vec3::new(c[0], c[1], c[2]))
408 }
409 Curve3::Intrinsic(i) => crate::frenet::frenet_tangent(i, t),
411 Curve3::RuledSection(r) => r.tangent(t).ok_or_else(|| outside_graph(t)),
412 Curve3::TorusSection(r) => r.tangent(t).ok_or_else(|| outside_graph(t)),
413 Curve3::ImplicitSection(r) => r.tangent(t).ok_or_else(|| outside_graph(t)),
414 Curve3::PairSection(r) => r.tangent(t).ok_or_else(|| outside_graph(t)),
415 Curve3::Banked(b) => crate::banked::banked_derivative(b, t),
417 _ => Err(GeomError::Unsupported {
420 backend: axiolid_contracts::BackendId::new("axiolid-reference"),
421 operation: axiolid_contracts::Operation::CurveEvaluation,
422 }),
423 }?;
424 finite3(value, "curve derivative")
425}
426
427pub fn second_derivative3(curve: &Curve3, t: Scalar) -> GeomResult<Vec3> {
429 finite(t)?;
430 let value = match curve {
431 Curve3::Line(_) | Curve3::Polyline(_) => Ok(Vec3::ZERO),
432 Curve3::Circle(c) => Ok(conic_second3(&c.frame, c.radius, c.radius, t)),
433 Curve3::Ellipse(e) => Ok(conic_second3(&e.frame, e.semi_axis_x, e.semi_axis_y, t)),
434 Curve3::BSpline(b) => {
435 de_boor_second_derivative(b, t, |p| [p.x, p.y, p.z], |c| Vec3::new(c[0], c[1], c[2]))
436 }
437 Curve3::RuledSection(r) => r.bend(t).ok_or_else(|| outside_graph(t)),
438 Curve3::TorusSection(r) => r.bend(t).ok_or_else(|| outside_graph(t)),
439 Curve3::ImplicitSection(r) => r.bend(t).ok_or_else(|| outside_graph(t)),
440 Curve3::PairSection(r) => r.bend(t).ok_or_else(|| outside_graph(t)),
441 _ => Err(GeomError::Unsupported {
442 backend: axiolid_contracts::BackendId::new("axiolid-reference"),
443 operation: axiolid_contracts::Operation::CurveEvaluation,
444 }),
445 }?;
446 finite3(value, "curve second derivative")
447}
448
449pub fn bspline_jet3(curve: &BSplineCurve3, t: Scalar) -> GeomResult<CurveJet<Point3, Vec3>> {
451 Ok(CurveJet {
452 point: de_boor(
453 curve,
454 t,
455 |p| [p.x, p.y, p.z],
456 |c| Point3::new(c[0], c[1], c[2]),
457 )?,
458 first: de_boor_derivative(
459 curve,
460 t,
461 |p| [p.x, p.y, p.z],
462 |c| Vec3::new(c[0], c[1], c[2]),
463 )?,
464 second: de_boor_second_derivative(
465 curve,
466 t,
467 |p| [p.x, p.y, p.z],
468 |c| Vec3::new(c[0], c[1], c[2]),
469 )?,
470 })
471}
472
473pub fn jet3(curve: &Curve3, t: Scalar) -> GeomResult<CurveJet<Point3, Vec3>> {
475 Ok(CurveJet {
476 point: evaluate3(curve, t)?,
477 first: derivative3(curve, t)?,
478 second: second_derivative3(curve, t)?,
479 })
480}
481
482fn finite(t: Scalar) -> GeomResult<()> {
485 if t.is_finite() {
486 Ok(())
487 } else {
488 Err(GeomError::InvalidInput(format!(
489 "curve parameter must be finite, got {t}"
490 )))
491 }
492}
493
494fn finite2(value: Vec2, what: &str) -> GeomResult<Vec2> {
495 if value.is_finite() {
496 Ok(value)
497 } else {
498 Err(GeomError::Degenerate(format!("{what} is non-finite")))
499 }
500}
501
502fn finite3(value: Vec3, what: &str) -> GeomResult<Vec3> {
503 if value.is_finite() {
504 Ok(value)
505 } else {
506 Err(GeomError::Degenerate(format!("{what} is non-finite")))
507 }
508}
509
510fn line_point<P>(origin: P, direction: P, t: Scalar) -> P
511where
512 P: core::ops::Add<Output = P> + core::ops::Mul<Scalar, Output = P>,
513{
514 origin + direction * t
515}
516
517fn conic_point2(frame: &Frame2, rx: Scalar, ry: Scalar, t: Scalar) -> Point2 {
518 frame.origin + frame.x * (rx * t.cos()) + frame.y * (ry * t.sin())
519}
520
521fn conic_tangent2(frame: &Frame2, rx: Scalar, ry: Scalar, t: Scalar) -> Vec2 {
522 frame.x * (-rx * t.sin()) + frame.y * (ry * t.cos())
523}
524
525fn conic_second2(frame: &Frame2, rx: Scalar, ry: Scalar, t: Scalar) -> Vec2 {
526 frame.x * (-rx * t.cos()) + frame.y * (-ry * t.sin())
527}
528
529fn conic_point3(frame: &Frame3, rx: Scalar, ry: Scalar, t: Scalar) -> Point3 {
530 frame.origin + frame.x * (rx * t.cos()) + frame.y * (ry * t.sin())
531}
532
533fn conic_tangent3(frame: &Frame3, rx: Scalar, ry: Scalar, t: Scalar) -> Vec3 {
534 frame.x * (-rx * t.sin()) + frame.y * (ry * t.cos())
535}
536
537fn conic_second3(frame: &Frame3, rx: Scalar, ry: Scalar, t: Scalar) -> Vec3 {
538 frame.x * (-rx * t.cos()) + frame.y * (-ry * t.sin())
539}
540
541fn polyline_span(count: usize, closed: bool, t: Scalar) -> Option<(usize, usize, Scalar)> {
545 let segments = if closed {
546 count
547 } else {
548 count.saturating_sub(1)
549 };
550 if count < 2 || segments == 0 {
551 return None;
552 }
553 let clamped = t.clamp(0.0, segments as Scalar);
556 let mut index = clamped.floor() as usize;
557 if index >= segments {
558 index = segments - 1;
559 }
560 let local = clamped - index as Scalar;
561 let next = (index + 1) % count;
562 Some((index, next, local))
563}
564
565fn polyline_point<P>(points: &[P], closed: bool, t: Scalar) -> GeomResult<P>
566where
567 P: Copy
568 + core::ops::Add<Output = P>
569 + core::ops::Sub<Output = P>
570 + core::ops::Mul<Scalar, Output = P>,
571{
572 let (i, j, local) = polyline_span(points.len(), closed, t).ok_or_else(|| {
573 GeomError::Degenerate(format!(
574 "polyline with {} points has no evaluable segment",
575 points.len()
576 ))
577 })?;
578 Ok(points[i] + (points[j] - points[i]) * local)
579}
580
581fn polyline_tangent<P>(points: &[P], closed: bool, t: Scalar) -> GeomResult<P>
582where
583 P: Copy + core::ops::Sub<Output = P>,
584{
585 let (i, j, _) = polyline_span(points.len(), closed, t).ok_or_else(|| {
586 GeomError::Degenerate(format!(
587 "polyline with {} points has no evaluable segment",
588 points.len()
589 ))
590 })?;
591 Ok(points[j] - points[i])
593}
594
595pub(crate) fn span_in(knots: &[Scalar], n: usize, d: usize, u: Scalar) -> usize {
603 let mut span = d;
604 for (k, knot) in knots.iter().enumerate().take(n).skip(d) {
605 if *knot <= u {
606 span = k;
607 } else {
608 break;
609 }
610 }
611 span
612}
613
614pub(crate) fn de_boor_recurrence<const N: usize>(
621 knots: &[Scalar],
622 span: usize,
623 d: usize,
624 u: Scalar,
625 points: &mut [[Scalar; N]],
626 weights: &mut [Scalar],
627) {
628 for r in 1..=d {
629 for j in (r..=d).rev() {
630 let i = span - d + j;
631 let denom = knots[i + d + 1 - r] - knots[i];
632 let alpha = if denom.abs() > 0.0 {
633 (u - knots[i]) / denom
634 } else {
635 0.0
636 };
637 for k in 0..N {
638 points[j][k] = points[j - 1][k] * (1.0 - alpha) + points[j][k] * alpha;
639 }
640 weights[j] = weights[j - 1] * (1.0 - alpha) + weights[j] * alpha;
641 }
642 }
643}
644
645fn spline_span<P>(b: &BSplineCurve<P>, t: Scalar) -> GeomResult<(Vec<Scalar>, usize, usize)> {
649 let axis = SplineAxis::new(
650 &b.knots,
651 &b.multiplicities,
652 b.degree,
653 b.control_points.len(),
654 "B-spline curve",
655 )?;
656 if let Some(weights) = &b.weights {
657 if weights.len() != b.control_points.len() {
658 return Err(GeomError::InvalidInput(format!(
659 "B-spline has {} weights for {} control points",
660 weights.len(),
661 b.control_points.len()
662 )));
663 }
664 if weights
665 .iter()
666 .any(|weight| !weight.is_finite() || *weight <= 0.0)
667 {
668 return Err(GeomError::InvalidInput(
669 "B-spline weights must be finite and strictly positive".to_owned(),
670 ));
671 }
672 }
673 let t = axis.clamp(t);
674 let span = span_in(&axis.knots, axis.count, axis.degree, t);
675 Ok((axis.knots, span, axis.degree))
676}
677
678fn finite_control_points<P, const N: usize, F>(
682 control_points: &[P],
683 to: &F,
684) -> GeomResult<Vec<[Scalar; N]>>
685where
686 F: Fn(&P) -> [Scalar; N],
687{
688 let points: Vec<[Scalar; N]> = control_points.iter().map(to).collect();
689 if points
690 .iter()
691 .flatten()
692 .any(|coordinate| !coordinate.is_finite())
693 {
694 return Err(GeomError::InvalidInput(
695 "B-spline control points must be finite".to_owned(),
696 ));
697 }
698 Ok(points)
699}
700
701fn de_boor<P, const N: usize, F, G, Q>(
707 b: &BSplineCurve<P>,
708 t: Scalar,
709 to: F,
710 from: G,
711) -> GeomResult<Q>
712where
713 F: Fn(&P) -> [Scalar; N],
714 G: Fn([Scalar; N]) -> Q,
715{
716 let (knots, span, d) = spline_span(b, t)?;
717 let control_points = finite_control_points(&b.control_points, &to)?;
718 let u = t.clamp(knots[d], knots[b.control_points.len()]);
719
720 let mut work: Vec<[Scalar; N]> = Vec::with_capacity(d + 1);
723 let mut weights: Vec<Scalar> = Vec::with_capacity(d + 1);
724 for j in 0..=d {
725 let idx = span - d + j;
726 let w = b.weights.as_ref().map_or(1.0, |ws| ws[idx]);
727 let c = control_points[idx];
728 let homogeneous = core::array::from_fn(|k| c[k] * w);
731 if homogeneous.iter().any(|value| !value.is_finite()) {
732 return Err(GeomError::Degenerate(
733 "B-spline homogeneous control point overflowed".to_owned(),
734 ));
735 }
736 work.push(homogeneous);
737 weights.push(w);
738 }
739
740 de_boor_recurrence(&knots, span, d, u, &mut work, &mut weights);
743
744 let w = weights[d];
745 if !w.is_finite() || w == 0.0 {
746 return Err(GeomError::Degenerate(
747 "B-spline weight collapsed to zero".to_owned(),
748 ));
749 }
750 Ok(from(core::array::from_fn(|k| work[d][k] / w)))
751}
752
753fn de_boor_derivative<P, const N: usize, F, G, Q>(
762 b: &BSplineCurve<P>,
763 t: Scalar,
764 to: F,
765 from: G,
766) -> GeomResult<Q>
767where
768 F: Fn(&P) -> [Scalar; N],
769 G: Fn([Scalar; N]) -> Q,
770{
771 let (knots, _, d) = spline_span(b, t)?;
772 let control_points = finite_control_points(&b.control_points, &to)?;
773 let n = b.control_points.len();
774 let u = t.clamp(knots[d], knots[n]);
775
776 let hom: Vec<[Scalar; N]> = (0..n)
778 .map(|i| {
779 let w = b.weights.as_ref().map_or(1.0, |ws| ws[i]);
780 let c = control_points[i];
781 core::array::from_fn(|k| c[k] * w)
782 })
783 .collect();
784 if hom.iter().flatten().any(|value| !value.is_finite()) {
785 return Err(GeomError::Degenerate(
786 "B-spline homogeneous control point overflowed".to_owned(),
787 ));
788 }
789 let hw: Vec<Scalar> = (0..n)
790 .map(|i| b.weights.as_ref().map_or(1.0, |ws| ws[i]))
791 .collect();
792
793 let mut dhom: Vec<[Scalar; N]> = Vec::with_capacity(n - 1);
795 let mut dhw: Vec<Scalar> = Vec::with_capacity(n - 1);
796 for i in 0..n - 1 {
797 let denom = knots[i + d + 1] - knots[i + 1];
798 let f = if denom.abs() > 0.0 {
799 d as Scalar / denom
800 } else {
801 0.0
802 };
803 dhom.push(core::array::from_fn(|k| (hom[i + 1][k] - hom[i][k]) * f));
804 dhw.push((hw[i + 1] - hw[i]) * f);
805 }
806
807 let dknots = &knots[1..knots.len() - 1];
809 let (da, dw) = eval_homogeneous(dknots, d - 1, &dhom, &dhw, u);
810 let (a, w) = eval_homogeneous(&knots, d, &hom, &hw, u);
812
813 if !w.is_finite() || w == 0.0 {
814 return Err(GeomError::Degenerate(
815 "B-spline weight collapsed to zero".to_owned(),
816 ));
817 }
818 Ok(from(core::array::from_fn(|k| {
820 (da[k] - (a[k] / w) * dw) / w
821 })))
822}
823
824fn de_boor_second_derivative<P, const N: usize, F, G, Q>(
829 b: &BSplineCurve<P>,
830 t: Scalar,
831 to: F,
832 from: G,
833) -> GeomResult<Q>
834where
835 F: Fn(&P) -> [Scalar; N],
836 G: Fn([Scalar; N]) -> Q,
837{
838 let (knots, _, degree) = spline_span(b, t)?;
839 let control_points = finite_control_points(&b.control_points, &to)?;
840 let count = b.control_points.len();
841 let u = t.clamp(knots[degree], knots[count]);
842
843 let points: Vec<[Scalar; N]> = (0..count)
844 .map(|i| {
845 let weight = b.weights.as_ref().map_or(1.0, |weights| weights[i]);
846 core::array::from_fn(|axis| control_points[i][axis] * weight)
847 })
848 .collect();
849 if points.iter().flatten().any(|value| !value.is_finite()) {
850 return Err(GeomError::Degenerate(
851 "B-spline homogeneous control point overflowed".to_owned(),
852 ));
853 }
854 let weights: Vec<Scalar> = (0..count)
855 .map(|i| b.weights.as_ref().map_or(1.0, |values| values[i]))
856 .collect();
857
858 let (point, weight) = eval_homogeneous(&knots, degree, &points, &weights, u);
859 if !weight.is_finite() || weight == 0.0 {
860 return Err(GeomError::Degenerate(
861 "B-spline weight collapsed to zero".to_owned(),
862 ));
863 }
864
865 let (first_points, first_weights) = derivative_controls(&points, &weights, &knots, degree);
866 let first_knots = &knots[1..knots.len() - 1];
867 let (first, first_weight) =
868 eval_homogeneous(first_knots, degree - 1, &first_points, &first_weights, u);
869 let position: [Scalar; N] = core::array::from_fn(|axis| point[axis] / weight);
870 let first_projected: [Scalar; N] =
871 core::array::from_fn(|axis| (first[axis] - position[axis] * first_weight) / weight);
872
873 let (second, second_weight) = if degree == 1 {
874 ([0.0; N], 0.0)
875 } else {
876 let (second_points, second_weights) =
877 derivative_controls(&first_points, &first_weights, first_knots, degree - 1);
878 let second_knots = &first_knots[1..first_knots.len() - 1];
879 eval_homogeneous(second_knots, degree - 2, &second_points, &second_weights, u)
880 };
881 Ok(from(core::array::from_fn(|axis| {
882 (second[axis] - 2.0 * first_weight * first_projected[axis] - second_weight * position[axis])
883 / weight
884 })))
885}
886
887fn derivative_controls<const N: usize>(
889 points: &[[Scalar; N]],
890 weights: &[Scalar],
891 knots: &[Scalar],
892 degree: usize,
893) -> (Vec<[Scalar; N]>, Vec<Scalar>) {
894 let mut derivative_points = Vec::with_capacity(points.len() - 1);
895 let mut derivative_weights = Vec::with_capacity(weights.len() - 1);
896 for i in 0..points.len() - 1 {
897 let denominator = knots[i + degree + 1] - knots[i + 1];
898 let factor = if denominator.abs() > 0.0 {
899 degree as Scalar / denominator
900 } else {
901 0.0
902 };
903 derivative_points.push(core::array::from_fn(|axis| {
904 (points[i + 1][axis] - points[i][axis]) * factor
905 }));
906 derivative_weights.push((weights[i + 1] - weights[i]) * factor);
907 }
908 (derivative_points, derivative_weights)
909}
910
911pub(crate) fn eval_homogeneous<const N: usize>(
913 knots: &[Scalar],
914 d: usize,
915 hom: &[[Scalar; N]],
916 hw: &[Scalar],
917 u: Scalar,
918) -> ([Scalar; N], Scalar) {
919 let n = hom.len();
920 if d == 0 {
921 let mut idx = 0;
923 for (k, knot) in knots.iter().enumerate().take(n) {
924 if *knot <= u {
925 idx = k;
926 }
927 }
928 return (hom[idx.min(n - 1)], hw[idx.min(n - 1)]);
929 }
930 let mut span = d;
931 for (k, knot) in knots.iter().enumerate().take(n).skip(d) {
932 if *knot <= u {
933 span = k;
934 } else {
935 break;
936 }
937 }
938 let mut work: Vec<[Scalar; N]> = (0..=d).map(|j| hom[span - d + j]).collect();
939 let mut weights: Vec<Scalar> = (0..=d).map(|j| hw[span - d + j]).collect();
940 for r in 1..=d {
941 for j in (r..=d).rev() {
942 let i = span - d + j;
943 let denom = knots[i + d + 1 - r] - knots[i];
944 let alpha = if denom.abs() > 0.0 {
945 (u - knots[i]) / denom
946 } else {
947 0.0
948 };
949 for k in 0..N {
950 work[j][k] = work[j - 1][k] * (1.0 - alpha) + work[j][k] * alpha;
951 }
952 weights[j] = weights[j - 1] * (1.0 - alpha) + weights[j] * alpha;
953 }
954 }
955 (work[d], weights[d])
956}
957
958pub fn flatten2(
980 curve: &Curve2,
981 domain: Interval,
982 chord_tolerance: Scalar,
983 max_depth: u32,
984) -> GeomResult<Vec<Point2>> {
985 const MAX_POINTS: usize = 1 << 16;
989 if !(chord_tolerance.is_finite()
990 && chord_tolerance.is_sign_positive()
991 && chord_tolerance != 0.0)
992 {
993 return Err(GeomError::InvalidInput(format!(
994 "chord tolerance must be positive and finite, got {chord_tolerance}"
995 )));
996 }
997 if let Curve2::Line(_) = curve {
1000 return Ok(vec![
1001 evaluate2(curve, domain.start)?,
1002 evaluate2(curve, domain.end)?,
1003 ]);
1004 }
1005 if let Curve2::Polyline(p) = curve {
1006 let natural = polyline_domain(p.points.len(), p.closed);
1012 let requested = (domain.end - domain.start).abs();
1013 if natural.end > 1.0 && requested <= 1.0 {
1014 return Err(GeomError::InvalidInput(format!(
1015 "polyline domain {:?} spans {requested} of {} segments; a \
1016 polyline parameter is one unit per segment, so this would \
1017 discard {} vertices",
1018 domain,
1019 natural.end,
1020 p.points.len().saturating_sub(2)
1021 )));
1022 }
1023 return polyline_flatten(&p.points, p.closed, domain, |t| evaluate2(curve, t));
1024 }
1025
1026 let mut out = vec![evaluate2(curve, domain.start)?];
1027 let eval = |t| evaluate2(curve, t);
1028 let bound = |a, b| crate::bound::chord_bound2(curve, a, b);
1029 for (a, b) in smooth_pieces(domain, &crate::bound::continuity_breaks2(curve, 1)) {
1030 subdivide(
1031 &eval,
1032 &bound,
1033 a,
1034 b,
1035 chord_tolerance,
1036 max_depth.min(MAX_DEPTH_CEILING),
1037 MAX_POINTS,
1038 &mut out,
1039 )?;
1040 out.push(evaluate2(curve, b)?);
1041 }
1042 Ok(out)
1043}
1044
1045fn smooth_pieces(domain: Interval, breaks: &[Scalar]) -> Vec<(Scalar, Scalar)> {
1051 let (lo, hi) = if domain.start <= domain.end {
1052 (domain.start, domain.end)
1053 } else {
1054 (domain.end, domain.start)
1055 };
1056 let mut cuts: Vec<Scalar> = breaks
1057 .iter()
1058 .copied()
1059 .filter(|&t| t > lo && t < hi)
1060 .collect();
1061 cuts.sort_by(Scalar::total_cmp);
1062 cuts.dedup();
1063 if domain.start > domain.end {
1064 cuts.reverse();
1065 }
1066 let mut out = Vec::with_capacity(cuts.len() + 1);
1067 let mut from = domain.start;
1068 for cut in cuts {
1069 out.push((from, cut));
1070 from = cut;
1071 }
1072 out.push((from, domain.end));
1073 out
1074}
1075
1076const MAX_DEPTH_CEILING: u32 = 20;
1081
1082trait ChordPoint: Copy {
1095 fn sub(self, other: Self) -> Self;
1096 fn add_scaled(self, direction: Self, scale: Scalar) -> Self;
1097 fn dot(self, other: Self) -> Scalar;
1098 fn length(self) -> Scalar;
1099 fn length_squared(self) -> Scalar;
1100}
1101
1102impl ChordPoint for Point2 {
1103 fn sub(self, other: Self) -> Self {
1104 self - other
1105 }
1106 fn add_scaled(self, direction: Self, scale: Scalar) -> Self {
1107 self + direction * scale
1108 }
1109 fn dot(self, other: Self) -> Scalar {
1110 Point2::dot(self, other)
1111 }
1112 fn length(self) -> Scalar {
1113 Point2::length(self)
1114 }
1115 fn length_squared(self) -> Scalar {
1116 Point2::length_squared(self)
1117 }
1118}
1119
1120impl ChordPoint for Point3 {
1121 fn sub(self, other: Self) -> Self {
1122 self - other
1123 }
1124 fn add_scaled(self, direction: Self, scale: Scalar) -> Self {
1125 self + direction * scale
1126 }
1127 fn dot(self, other: Self) -> Scalar {
1128 Point3::dot(self, other)
1129 }
1130 fn length(self) -> Scalar {
1131 Point3::length(self)
1132 }
1133 fn length_squared(self) -> Scalar {
1134 Point3::length_squared(self)
1135 }
1136}
1137
1138fn sagitta<P: ChordPoint>(a: P, b: P, m: P) -> Scalar {
1140 let ab = b.sub(a);
1141 let len2 = ab.length_squared();
1142 if len2 <= 0.0 {
1143 return m.sub(a).length();
1146 }
1147 let t = (m.sub(a).dot(ab) / len2).clamp(0.0, 1.0);
1148 m.sub(a.add_scaled(ab, t)).length()
1149}
1150
1151#[allow(clippy::too_many_arguments)]
1166fn subdivide<P, F, B>(
1167 eval: &F,
1168 bound: &B,
1169 a: Scalar,
1170 b: Scalar,
1171 tol: Scalar,
1172 depth: u32,
1173 budget: usize,
1174 out: &mut Vec<P>,
1175) -> GeomResult<()>
1176where
1177 P: ChordPoint,
1178 F: Fn(Scalar) -> GeomResult<P>,
1179 B: Fn(Scalar, Scalar) -> Option<Scalar>,
1180{
1181 if out.len() >= budget {
1182 return Err(GeomError::Degenerate(format!(
1183 "curve flattening exceeded {budget} points before meeting the \
1184 chord tolerance {tol}; the curve may be degenerate"
1185 )));
1186 }
1187 let mid = 0.5 * (a + b);
1188 let pa = eval(a)?;
1189 let pb = eval(b)?;
1190 if !(mid > a && mid < b) {
1196 if sagitta(pa, pb, pa) <= tol && (pb.sub(pa)).length() <= tol {
1197 return Ok(());
1198 }
1199 return Err(GeomError::Degenerate(format!(
1200 "curve parameter interval ({a}, {b}) is too small to bisect but \
1201 its chord still exceeds the tolerance {tol}"
1202 )));
1203 }
1204 let pm = eval(mid)?;
1205 if sagitta(pa, pb, pm) <= tol && bound(a, b).is_none_or(|certified| certified <= tol) {
1206 return Ok(());
1208 }
1209 if depth == 0 {
1210 return Err(GeomError::BudgetExceeded {
1211 resource: "curve flattening depth",
1212 });
1213 }
1214 subdivide(eval, bound, a, mid, tol, depth - 1, budget, out)?;
1215 out.push(pm);
1216 subdivide(eval, bound, mid, b, tol, depth - 1, budget, out)?;
1217 Ok(())
1218}
1219
1220fn polyline_flatten<P, F>(
1222 points: &[P],
1223 closed: bool,
1224 domain: Interval,
1225 eval: F,
1226) -> GeomResult<Vec<P>>
1227where
1228 P: Copy,
1229 F: Fn(Scalar) -> GeomResult<P>,
1230{
1231 let segments = if closed {
1232 points.len()
1233 } else {
1234 points.len().saturating_sub(1)
1235 };
1236 if segments == 0 {
1237 return Err(GeomError::Degenerate(
1238 "polyline has no evaluable segment".to_owned(),
1239 ));
1240 }
1241 let lo = domain.start.min(domain.end);
1242 let hi = domain.start.max(domain.end);
1243 let mut out = vec![eval(lo)?];
1244 let first = lo.floor() as i64 + 1;
1246 let last = hi.ceil() as i64 - 1;
1247 for k in first..=last {
1248 let t = k as Scalar;
1249 if t > lo && t < hi {
1250 out.push(eval(t)?);
1251 }
1252 }
1253 out.push(eval(hi)?);
1254 Ok(out)
1255}
1256
1257impl CurveEvaluator<Curve2> for ScalarCurve {
1260 type Point = Point2;
1261 type Derivative = Vec2;
1262 type Error = GeomError;
1263
1264 fn domain(&self, curve: &Curve2) -> Interval {
1265 domain2(curve)
1266 }
1267
1268 fn evaluate(
1269 &self,
1270 curve: &Curve2,
1271 t: Scalar,
1272 _tolerance: Tolerance,
1273 ) -> Result<Self::Point, Self::Error> {
1274 evaluate2(curve, t)
1275 }
1276
1277 fn derivative(
1278 &self,
1279 curve: &Curve2,
1280 t: Scalar,
1281 _tolerance: Tolerance,
1282 ) -> Result<Self::Derivative, Self::Error> {
1283 derivative2(curve, t)
1284 }
1285}
1286
1287impl CurveEvaluator<Curve3> for ScalarCurve {
1288 type Point = Point3;
1289 type Derivative = Vec3;
1290 type Error = GeomError;
1291
1292 fn domain(&self, curve: &Curve3) -> Interval {
1293 domain3(curve)
1294 }
1295
1296 fn evaluate(
1297 &self,
1298 curve: &Curve3,
1299 t: Scalar,
1300 _tolerance: Tolerance,
1301 ) -> Result<Self::Point, Self::Error> {
1302 evaluate3(curve, t)
1303 }
1304
1305 fn derivative(
1306 &self,
1307 curve: &Curve3,
1308 t: Scalar,
1309 _tolerance: Tolerance,
1310 ) -> Result<Self::Derivative, Self::Error> {
1311 derivative3(curve, t)
1312 }
1313}
1314
1315#[allow(unused)]
1317fn _type_anchors(_: Circle2, _: Circle3, _: Ellipse2, _: Ellipse3, _: Line2, _: Line3) {}
1318#[allow(unused)]
1319fn _poly_anchors(_: Polyline2, _: Polyline3) {}
1320
1321pub fn flatten3(
1329 curve: &Curve3,
1330 domain: Interval,
1331 chord_tolerance: Scalar,
1332 max_depth: u32,
1333) -> GeomResult<Vec<Point3>> {
1334 const MAX_POINTS: usize = 1 << 16;
1338 if !(chord_tolerance.is_finite()
1339 && chord_tolerance.is_sign_positive()
1340 && chord_tolerance != 0.0)
1341 {
1342 return Err(GeomError::InvalidInput(format!(
1343 "chord tolerance must be positive and finite, got {chord_tolerance}"
1344 )));
1345 }
1346 if let Curve3::Line(_) = curve {
1349 return Ok(vec![
1350 evaluate3(curve, domain.start)?,
1351 evaluate3(curve, domain.end)?,
1352 ]);
1353 }
1354 if let Curve3::Polyline(p) = curve {
1355 let natural = polyline_domain(p.points.len(), p.closed);
1359 let requested = (domain.end - domain.start).abs();
1360 if natural.end > 1.0 && requested <= 1.0 {
1361 return Err(GeomError::InvalidInput(format!(
1362 "polyline domain {:?} spans {requested} of {} segments; a \
1363 polyline parameter is one unit per segment, so this would \
1364 discard {} vertices",
1365 domain,
1366 natural.end,
1367 p.points.len().saturating_sub(2)
1368 )));
1369 }
1370 return polyline_flatten(&p.points, p.closed, domain, |t| evaluate3(curve, t));
1371 }
1372
1373 let eval = |t| evaluate3(curve, t);
1374 let bound = |a, b| crate::bound::chord_bound3(curve, a, b);
1375 let mut out = vec![eval(domain.start)?];
1376 for (a, b) in smooth_pieces(domain, &crate::bound::continuity_breaks3(curve, 1)) {
1377 subdivide(
1378 &eval,
1379 &bound,
1380 a,
1381 b,
1382 chord_tolerance,
1383 max_depth.min(MAX_DEPTH_CEILING),
1384 MAX_POINTS,
1385 &mut out,
1386 )?;
1387 out.push(eval(b)?);
1388 }
1389 Ok(out)
1390}
1391
1392fn no_closed_form_inversion() -> GeomError {
1397 GeomError::InvalidInput(
1398 "curve family has no closed form inversion; a point trim on this basis \
1399 would require iteration, which trim resolution does not perform"
1400 .to_owned(),
1401 )
1402}
1403
1404fn point_not_on_curve(distance: Scalar, tolerance: Scalar) -> GeomError {
1406 GeomError::InvalidInput(format!(
1407 "point is {distance} from the curve, outside the {tolerance} tolerance; \
1408 refusing rather than projecting it onto the nearest parameter"
1409 ))
1410}
1411
1412fn invert_line(
1418 origin: impl Into<[Scalar; 3]>,
1419 direction: [Scalar; 3],
1420 point: [Scalar; 3],
1421 tolerance: Scalar,
1422) -> GeomResult<Scalar> {
1423 let origin = origin.into();
1424 let dd = direction.iter().map(|c| c * c).sum::<Scalar>();
1425 if !dd.is_finite() || dd <= Scalar::EPSILON {
1426 return Err(GeomError::InvalidInput(
1427 "line direction is degenerate, so no parameter names a point".to_owned(),
1428 ));
1429 }
1430 let offset = [
1431 point[0] - origin[0],
1432 point[1] - origin[1],
1433 point[2] - origin[2],
1434 ];
1435 let t = offset
1436 .iter()
1437 .zip(direction.iter())
1438 .map(|(o, d)| o * d)
1439 .sum::<Scalar>()
1440 / dd;
1441 let residual = [
1444 offset[0] - direction[0] * t,
1445 offset[1] - direction[1] * t,
1446 offset[2] - direction[2] * t,
1447 ];
1448 let distance = residual.iter().map(|c| c * c).sum::<Scalar>().sqrt();
1449 if distance > tolerance {
1450 return Err(point_not_on_curve(distance, tolerance));
1451 }
1452 Ok(t)
1453}
1454
1455fn invert_conic(
1461 local_x: Scalar,
1462 local_y: Scalar,
1463 semi_x: Scalar,
1464 semi_y: Scalar,
1465) -> GeomResult<Scalar> {
1466 if !(semi_x.is_finite() && semi_y.is_finite()) || semi_x <= 0.0 || semi_y <= 0.0 {
1467 return Err(GeomError::InvalidInput(
1468 "conic semi-axes must be finite and positive to invert a point".to_owned(),
1469 ));
1470 }
1471 let angle = (local_y / semi_y).atan2(local_x / semi_x);
1472 if !angle.is_finite() {
1473 return Err(GeomError::InvalidInput(
1474 "conic inversion produced a non-finite angle".to_owned(),
1475 ));
1476 }
1477 Ok(angle.rem_euclid(std::f64::consts::TAU))
1480}
1481
1482pub fn invert2(curve: &Curve2, point: Point2, tolerance: Tolerance) -> GeomResult<Scalar> {
1494 finite2(point, "inversion point")?;
1495 let linear = tolerance.linear();
1496 match curve {
1497 Curve2::Line(l) => invert_line(
1498 [l.origin.x, l.origin.y, 0.0],
1499 [l.direction.x, l.direction.y, 0.0],
1500 [point.x, point.y, 0.0],
1501 linear,
1502 ),
1503 Curve2::Circle(c) => {
1504 let t = invert_conic_in_frame2(&c.frame, point, c.radius, c.radius)?;
1505 verify2(curve, t, point, linear)
1506 }
1507 Curve2::Ellipse(e) => {
1508 let t = invert_conic_in_frame2(&e.frame, point, e.semi_axis_x, e.semi_axis_y)?;
1509 verify2(curve, t, point, linear)
1510 }
1511 Curve2::Sinusoid(_) | Curve2::QuadraticGraph(_) => verify2(curve, point.x, point, linear),
1514 Curve2::AngleGraph(g) => {
1517 let u = g.angle(point.y).ok_or_else(|| outside_graph(point.y))?;
1518 let turns = ((point.x - u) / std::f64::consts::TAU).round();
1519 let shifted = Point2::new(point.x - turns * std::f64::consts::TAU, point.y);
1520 verify2(curve, point.y, shifted, linear)
1521 }
1522 Curve2::Lifted(l) => {
1524 let lifted = l.carrier.jet(point.x, point.y).point;
1525 let t = invert3(&l.curve, lifted, tolerance)?;
1526 verify2(curve, t, point, linear)
1527 }
1528 Curve2::Implicit(c) => {
1530 let t = c
1531 .parameter_of(point)
1532 .ok_or_else(|| point_not_on_curve(Scalar::INFINITY, linear))?;
1533 verify2(curve, t, point, linear)
1534 }
1535 _ => Err(no_closed_form_inversion()),
1536 }
1537}
1538
1539fn invert_conic_in_frame2(
1541 frame: &axiolid_core::Frame2,
1542 point: Point2,
1543 semi_x: Scalar,
1544 semi_y: Scalar,
1545) -> GeomResult<Scalar> {
1546 let offset = point - frame.origin;
1547 invert_conic(offset.dot(frame.x), offset.dot(frame.y), semi_x, semi_y)
1548}
1549
1550fn verify2(curve: &Curve2, t: Scalar, point: Point2, tolerance: Scalar) -> GeomResult<Scalar> {
1557 let found = evaluate2(curve, t)?;
1558 let distance = (found - point).length();
1559 if distance > tolerance {
1560 return Err(point_not_on_curve(distance, tolerance));
1561 }
1562 Ok(t)
1563}
1564
1565pub fn invert3(curve: &Curve3, point: Point3, tolerance: Tolerance) -> GeomResult<Scalar> {
1573 finite3(point, "inversion point")?;
1574 let linear = tolerance.linear();
1575 match curve {
1576 Curve3::Line(l) => invert_line(
1577 [l.origin.x, l.origin.y, l.origin.z],
1578 [l.direction.x, l.direction.y, l.direction.z],
1579 [point.x, point.y, point.z],
1580 linear,
1581 ),
1582 Curve3::Circle(c) => {
1583 let t = invert_conic_in_frame3(&c.frame, point, c.radius, c.radius)?;
1584 verify3(curve, t, point, linear)
1585 }
1586 Curve3::Ellipse(e) => {
1587 let t = invert_conic_in_frame3(&e.frame, point, e.semi_axis_x, e.semi_axis_y)?;
1588 verify3(curve, t, point, linear)
1589 }
1590 Curve3::RuledSection(r) => {
1593 let carrier = axiolid_curve::Carrier::Ruled(r.carrier);
1594 let (u, _) = carrier.parameters(point);
1595 first_on_curve(curve, &turns_of(u), point, linear)
1596 }
1597 Curve3::TorusSection(r) => {
1598 let carrier = axiolid_curve::Carrier::Torus(r.torus);
1599 let (_, v) = carrier.parameters(point);
1600 first_on_curve(curve, &turns_of(v), point, linear)
1601 }
1602 Curve3::PairSection(r) => {
1604 let t = r
1605 .parameter_of(point)
1606 .ok_or_else(|| point_not_on_curve(Scalar::INFINITY, linear))?;
1607 verify3(curve, t, point, linear)
1608 }
1609 Curve3::ImplicitSection(r) => {
1612 if let axiolid_curve::Carrier::Spline(b) = &r.carrier {
1615 let surface = axiolid_surface::Surface::BSpline((**b).clone());
1616 let (u, v) = crate::surface::locate(&surface, point, tolerance)?;
1617 let t = r
1618 .curve
1619 .parameter_of(Point2::new(u, v))
1620 .ok_or_else(|| point_not_on_curve(Scalar::INFINITY, linear))?;
1621 return verify3(curve, t, point, linear);
1622 }
1623 let (u, v) = r.carrier.parameters(point);
1624 let (pu, pv) = r.carrier.periodic();
1625 let turns = |periodic: bool| -> &'static [Scalar] {
1626 if periodic {
1627 &[0.0, 1.0, -1.0, 2.0, -2.0]
1628 } else {
1629 &[0.0]
1630 }
1631 };
1632 let mut candidates = Vec::new();
1633 for &ku in turns(pu) {
1634 for &kv in turns(pv) {
1635 let tau = std::f64::consts::TAU;
1636 if let Some(t) = r
1637 .curve
1638 .parameter_of(Point2::new(u + ku * tau, v + kv * tau))
1639 {
1640 candidates.push(t);
1641 }
1642 }
1643 }
1644 first_on_curve(curve, &candidates, point, linear)
1645 }
1646 _ => Err(no_closed_form_inversion()),
1647 }
1648}
1649
1650fn turns_of(angle: Scalar) -> Vec<Scalar> {
1652 let tau = std::f64::consts::TAU;
1653 vec![
1654 angle,
1655 angle + tau,
1656 angle - tau,
1657 angle + 2.0 * tau,
1658 angle - 2.0 * tau,
1659 ]
1660}
1661
1662fn first_on_curve(
1664 curve: &Curve3,
1665 candidates: &[Scalar],
1666 point: Point3,
1667 tolerance: Scalar,
1668) -> GeomResult<Scalar> {
1669 let mut nearest = Scalar::INFINITY;
1670 for &t in candidates {
1671 if let Ok(found) = evaluate3(curve, t) {
1672 let distance = (found - point).length();
1673 if distance <= tolerance {
1674 return Ok(t);
1675 }
1676 nearest = nearest.min(distance);
1677 }
1678 }
1679 Err(point_not_on_curve(nearest, tolerance))
1680}
1681
1682pub fn locate3(curve: &Curve3, point: Point3, tolerance: Tolerance) -> GeomResult<Scalar> {
1691 match invert3(curve, point, tolerance) {
1692 Ok(t) => Ok(t),
1693 Err(error) => match curve {
1694 Curve3::BSpline(b) => {
1695 let domain = spline_domain(b);
1696 let t = nearest_parameter(
1697 domain,
1698 b.control_points.len().max(2) * 64,
1699 |t| evaluate3(curve, t).map(|p| (p - point).length()),
1700 |t| {
1701 let (p, d, dd) = (
1702 evaluate3(curve, t)?,
1703 derivative3(curve, t)?,
1704 second_derivative3(curve, t)?,
1705 );
1706 let r = p - point;
1707 Ok((r.dot(d), d.dot(d) + r.dot(dd)))
1708 },
1709 )?;
1710 verify3(curve, t, point, tolerance.linear())
1711 }
1712 _ => Err(error),
1713 },
1714 }
1715}
1716
1717pub fn locate2(curve: &Curve2, point: Point2, tolerance: Tolerance) -> GeomResult<Scalar> {
1724 match invert2(curve, point, tolerance) {
1725 Ok(t) => Ok(t),
1726 Err(error) => match curve {
1727 Curve2::BSpline(b) => {
1728 let domain = spline_domain(b);
1729 let t = nearest_parameter(
1730 domain,
1731 b.control_points.len().max(2) * 64,
1732 |t| evaluate2(curve, t).map(|p| (p - point).length()),
1733 |t| {
1734 let (p, d, dd) = (
1735 evaluate2(curve, t)?,
1736 derivative2(curve, t)?,
1737 second_derivative2(curve, t)?,
1738 );
1739 let r = p - point;
1740 Ok((r.dot(d), d.dot(d) + r.dot(dd)))
1741 },
1742 )?;
1743 verify2(curve, t, point, tolerance.linear())
1744 }
1745 _ => Err(error),
1746 },
1747 }
1748}
1749
1750fn nearest_parameter(
1754 domain: Interval,
1755 samples: usize,
1756 distance: impl Fn(Scalar) -> GeomResult<Scalar>,
1757 gradient: impl Fn(Scalar) -> GeomResult<(Scalar, Scalar)>,
1758) -> GeomResult<Scalar> {
1759 let (lo, hi) = (domain.start.min(domain.end), domain.start.max(domain.end));
1760 let mut best = (Scalar::INFINITY, lo);
1761 for i in 0..=samples {
1762 let t = lo + (hi - lo) * i as Scalar / samples as Scalar;
1763 let d = distance(t)?;
1764 if d < best.0 {
1765 best = (d, t);
1766 }
1767 }
1768 let mut t = best.1;
1769 for _ in 0..60 {
1770 let (g, h) = gradient(t)?;
1771 if h <= 0.0 || !h.is_finite() {
1772 break;
1773 }
1774 let next = (t - g / h).clamp(lo, hi);
1775 let moved = (next - t).abs();
1776 t = next;
1777 if moved <= 4.0 * Scalar::EPSILON * (1.0 + t.abs()) {
1778 break;
1779 }
1780 }
1781 Ok(t)
1782}
1783
1784fn invert_conic_in_frame3(
1786 frame: &axiolid_core::Frame3,
1787 point: Point3,
1788 semi_x: Scalar,
1789 semi_y: Scalar,
1790) -> GeomResult<Scalar> {
1791 let offset = point - frame.origin;
1792 invert_conic(offset.dot(frame.x), offset.dot(frame.y), semi_x, semi_y)
1793}
1794
1795fn verify3(curve: &Curve3, t: Scalar, point: Point3, tolerance: Scalar) -> GeomResult<Scalar> {
1797 let found = evaluate3(curve, t)?;
1798 let distance = (found - point).length();
1799 if distance > tolerance {
1800 return Err(point_not_on_curve(distance, tolerance));
1801 }
1802 Ok(t)
1803}