1use ogeom_core::{OgeomResult, Tolerances, ogeom_bail};
43use ogeom_math::{KnotVector, Point, Point2};
44
45use crate::curve::BSplineCurve;
46use crate::curve2d::BSpline2d;
47
48mod seeded;
49
50pub use seeded::{Ended, ScatteredFit, ScatteredOptions, Target, fit_surface_scattered_over};
51
52#[allow(
53 clippy::cast_precision_loss,
54 reason = "sample counts are far below 2^52"
55)]
56fn precise(n: usize) -> f64 {
57 n as f64
58}
59
60#[derive(Debug, Clone, PartialEq)]
62pub struct Fitted<C> {
63 pub curve: C,
65 pub error: f64,
70 pub met: bool,
77}
78
79pub fn fit_points(
91 points: &[Point],
92 degree: usize,
93 tolerance: f64,
94 tol: Tolerances,
95) -> OgeomResult<Fitted<BSplineCurve>> {
96 let (knots, control, error, met) = fit::<3>(
97 &points.iter().map(|p| [p.x, p.y, p.z]).collect::<Vec<_>>(),
98 degree,
99 tolerance,
100 false,
101 tol,
102 )?;
103 let curve = BSplineCurve::new(
104 knots,
105 control
106 .into_iter()
107 .map(|c| Point::new(c[0], c[1], c[2]))
108 .collect(),
109 tol,
110 )?;
111 Ok(Fitted { curve, error, met })
112}
113
114pub fn fit_points_faired(
132 points: &[Point],
133 degree: usize,
134 controls: usize,
135 smoothing: f64,
136 tol: Tolerances,
137) -> OgeomResult<Fitted<BSplineCurve>> {
138 if !smoothing.is_finite() || smoothing < 0.0 {
139 ogeom_bail!(
140 Construction,
141 "a smoothing weight of {smoothing} is not a weight"
142 );
143 }
144 if controls < degree + 1 {
145 ogeom_bail!(
146 Construction,
147 "{controls} control points cannot carry degree {degree}"
148 );
149 }
150 let m = points.len();
151 if m < 2 {
152 ogeom_bail!(Construction, "a fair curve needs at least two points");
153 }
154
155 let knots = KnotVector::clamped_uniform(degree, controls)?;
157 let (lo, hi) = (knots.domain_start(), knots.domain_end());
158 let mut cumulative = vec![0.0f64; m];
159 for i in 1..m {
160 cumulative[i] = cumulative[i - 1] + points[i].distance(points[i - 1]);
161 }
162 let total = cumulative[m - 1];
163 if total <= tol.confusion() {
164 ogeom_bail!(
165 Construction,
166 "the points coincide; there is no curve to fair"
167 );
168 }
169 let parameters: Vec<f64> = cumulative
170 .iter()
171 .map(|c| lo + (hi - lo) * (c / total))
172 .collect();
173
174 let n = controls;
177 let mut a = nalgebra::DMatrix::<f64>::zeros(m, n);
178 for (k, &u) in parameters.iter().enumerate() {
179 let span = knots.span_unchecked(u);
180 let basis = knots.basis(span, u);
181 let first = span - degree;
182 for (j, b) in basis.iter().enumerate() {
183 a[(k, first + j)] = *b;
184 }
185 }
186 let mut d = nalgebra::DMatrix::<f64>::zeros(n.saturating_sub(2), n);
187 for r in 0..n.saturating_sub(2) {
188 d[(r, r)] = 1.0;
189 d[(r, r + 1)] = -2.0;
190 d[(r, r + 2)] = 1.0;
191 }
192 let k_full = a.transpose() * &a + d.transpose() * &d * smoothing;
193
194 let free: Vec<usize> = (1..n - 1).collect();
195 let mut reduced = nalgebra::DMatrix::<f64>::zeros(free.len(), free.len());
196 for (ri, &i) in free.iter().enumerate() {
197 for (rj, &j) in free.iter().enumerate() {
198 reduced[(ri, rj)] = k_full[(i, j)];
199 }
200 }
201 let decomposition = reduced.lu();
202
203 let mut control = vec![Point::ORIGIN; n];
204 control[0] = points[0];
205 control[n - 1] = points[m - 1];
206 for axis in 0..3 {
207 let b_data =
208 nalgebra::DVector::from_iterator(m, points.iter().map(|p| [p.x, p.y, p.z][axis]));
209 let full_rhs = a.transpose() * &b_data;
210 let mut rhs = nalgebra::DVector::<f64>::zeros(free.len());
211 for (ri, &i) in free.iter().enumerate() {
212 rhs[ri] = full_rhs[i]
213 - k_full[(i, 0)] * [points[0].x, points[0].y, points[0].z][axis]
214 - k_full[(i, n - 1)] * [points[m - 1].x, points[m - 1].y, points[m - 1].z][axis];
215 }
216 let Some(solved) = decomposition.solve(&rhs) else {
217 ogeom_bail!(
218 Numeric,
219 "the faired system is singular; fewer controls or more points"
220 );
221 };
222 for (ri, &i) in free.iter().enumerate() {
223 match axis {
224 0 => control[i].x = solved[ri],
225 1 => control[i].y = solved[ri],
226 _ => control[i].z = solved[ri],
227 }
228 }
229 }
230
231 let curve = BSplineCurve::new(knots, control, tol)?;
232 let mut error = 0.0f64;
233 {
234 use crate::traits::Curve3d as _;
235 for (point, &u) in points.iter().zip(¶meters) {
236 error = error.max(curve.point_at(u, tol)?.distance(*point));
237 }
238 }
239 Ok(Fitted {
240 curve,
241 error,
242 met: true,
243 })
244}
245
246pub fn fit_points_closed(
264 points: &[Point],
265 degree: usize,
266 tolerance: f64,
267 tol: Tolerances,
268) -> OgeomResult<Fitted<BSplineCurve>> {
269 let Some((first, last)) = points.first().zip(points.last()) else {
270 ogeom_bail!(Construction, "a closed fit needs points");
271 };
272 if !first.is_equal(*last, tol) {
273 ogeom_bail!(
274 Construction,
275 "a closed fit needs a loop: the first point repeated at the end"
276 );
277 }
278 let (knots, control, error, met) = fit::<3>(
279 &points.iter().map(|p| [p.x, p.y, p.z]).collect::<Vec<_>>(),
280 degree,
281 tolerance,
282 true,
283 tol,
284 )?;
285 let curve = BSplineCurve::new(
286 knots,
287 control
288 .into_iter()
289 .map(|c| Point::new(c[0], c[1], c[2]))
290 .collect(),
291 tol,
292 )?;
293 Ok(Fitted { curve, error, met })
294}
295
296pub fn fit_points_2d(
305 points: &[Point2],
306 degree: usize,
307 tolerance: f64,
308 tol: Tolerances,
309) -> OgeomResult<Fitted<BSpline2d>> {
310 let (knots, control, error, met) = fit::<2>(
311 &points.iter().map(|p| [p.x, p.y]).collect::<Vec<_>>(),
312 degree,
313 tolerance,
314 false,
315 tol,
316 )?;
317 let curve = BSpline2d::new(
318 knots,
319 control
320 .into_iter()
321 .map(|c| Point2::new(c[0], c[1]))
322 .collect(),
323 tol,
324 )?;
325 Ok(Fitted { curve, error, met })
326}
327
328#[allow(clippy::type_complexity)]
343pub fn fit_points_joint(
344 points: &[Point],
345 on_a: &[Point2],
346 on_b: &[Point2],
347 degree: usize,
348 tolerance: f64,
349 tol: Tolerances,
350) -> OgeomResult<(Fitted<BSplineCurve>, BSpline2d, BSpline2d)> {
351 fit_points_joint_inner(points, on_a, on_b, degree, tolerance, None, tol)
352}
353
354#[allow(clippy::type_complexity)]
367pub fn fit_points_joint_closed(
368 points: &[Point],
369 on_a: &[Point2],
370 on_b: &[Point2],
371 degree: usize,
372 tolerance: f64,
373 tol: Tolerances,
374) -> OgeomResult<(Fitted<BSplineCurve>, BSpline2d, BSpline2d)> {
375 fit_points_joint_inner(points, on_a, on_b, degree, tolerance, Some(7), tol)
376}
377
378#[allow(clippy::type_complexity)]
388pub fn fit_points_joint_winding(
389 points: &[Point],
390 on_a: &[Point2],
391 on_b: &[Point2],
392 degree: usize,
393 tolerance: f64,
394 tol: Tolerances,
395) -> OgeomResult<(Fitted<BSplineCurve>, BSpline2d, BSpline2d)> {
396 fit_points_joint_inner(points, on_a, on_b, degree, tolerance, Some(3), tol)
397}
398
399#[allow(
416 clippy::type_complexity,
417 clippy::too_many_arguments,
418 reason = "one trace in three spaces, its stations and the fit's terms"
419)]
420pub fn fit_points_joint_from(
421 points: &[Point],
422 on_a: &[Point2],
423 on_b: &[Point2],
424 stations: &[usize],
425 closed: bool,
426 degree: usize,
427 tolerance: f64,
428 tol: Tolerances,
429) -> OgeomResult<(Fitted<BSplineCurve>, BSpline2d, BSpline2d)> {
430 joint_from(
431 points, on_a, on_b, stations, closed, degree, tolerance, false, tol,
432 )
433}
434
435#[allow(
450 clippy::type_complexity,
451 clippy::too_many_arguments,
452 reason = "one trace in three spaces, its stations and the fit's terms"
453)]
454pub fn fit_points_joint_from_held(
455 points: &[Point],
456 on_a: &[Point2],
457 on_b: &[Point2],
458 stations: &[usize],
459 closed: bool,
460 degree: usize,
461 tolerance: f64,
462 tol: Tolerances,
463) -> OgeomResult<(Fitted<BSplineCurve>, BSpline2d, BSpline2d)> {
464 joint_from(
465 points, on_a, on_b, stations, closed, degree, tolerance, true, tol,
466 )
467}
468
469#[allow(
470 clippy::type_complexity,
471 clippy::too_many_arguments,
472 reason = "one trace in three spaces, its stations and the fit's terms"
473)]
474fn joint_from(
475 points: &[Point],
476 on_a: &[Point2],
477 on_b: &[Point2],
478 stations: &[usize],
479 closed: bool,
480 degree: usize,
481 tolerance: f64,
482 within: bool,
483 tol: Tolerances,
484) -> OgeomResult<(Fitted<BSplineCurve>, BSpline2d, BSpline2d)> {
485 if points.len() != on_a.len() || points.len() != on_b.len() {
486 ogeom_bail!(
487 Construction,
488 "a joint fit needs the same trace seen in every space"
489 );
490 }
491 if !tolerance.is_finite() || tolerance <= 0.0 {
492 ogeom_bail!(Construction, "a tolerance of {tolerance} is not a distance");
493 }
494 if degree == 0 || points.len() < 2 {
495 ogeom_bail!(
496 Construction,
497 "a fit needs a degree of at least one and two points"
498 );
499 }
500 let joined: Vec<[f64; 7]> = points
501 .iter()
502 .zip(on_a)
503 .zip(on_b)
504 .map(|((p, a), b)| [p.x, p.y, p.z, a.x, a.y, b.x, b.y])
505 .collect();
506 let parameters = chordal::<7>(&joined);
507 if parameters.windows(2).any(|w| w[1] <= w[0]) {
508 ogeom_bail!(
509 Construction,
510 "two consecutive samples coincide and leave no parameter between them"
511 );
512 }
513 let kept: Vec<(f64, usize)> = stations
514 .iter()
515 .filter_map(|&i| parameters.get(i))
516 .filter(|&&u| u > 0.0 && u < 1.0)
517 .map(|&u| (u, 1))
518 .collect();
519 let (knots, mut controls) = fit_family::<7>(
520 std::slice::from_ref(&joined),
521 ¶meters,
522 degree,
523 tolerance,
524 closed,
525 false,
526 &kept,
527 within,
528 )?;
529 let Some(control) = controls.pop() else {
530 ogeom_bail!(NotDone, "the joint fit solved nothing");
531 };
532 let error = residuals::<7>(&knots, &control, &joined, ¶meters)
533 .iter()
534 .fold(0.0_f64, |acc, e| acc.max(e.1));
535 let curve = BSplineCurve::new(
536 knots.clone(),
537 control
538 .iter()
539 .map(|c| Point::new(c[0], c[1], c[2]))
540 .collect(),
541 tol,
542 )?;
543 let pa = BSpline2d::new(
544 knots.clone(),
545 control.iter().map(|c| Point2::new(c[3], c[4])).collect(),
546 tol,
547 )?;
548 let pb = BSpline2d::new(
549 knots,
550 control.iter().map(|c| Point2::new(c[5], c[6])).collect(),
551 tol,
552 )?;
553 let met = error <= tolerance;
554 Ok((Fitted { curve, error, met }, pa, pb))
555}
556
557#[allow(clippy::type_complexity)]
558fn fit_points_joint_inner(
559 points: &[Point],
560 on_a: &[Point2],
561 on_b: &[Point2],
562 degree: usize,
563 tolerance: f64,
564 smooth_loop: Option<usize>,
565 tol: Tolerances,
566) -> OgeomResult<(Fitted<BSplineCurve>, BSpline2d, BSpline2d)> {
567 if points.len() != on_a.len() || points.len() != on_b.len() {
568 ogeom_bail!(
569 Construction,
570 "a joint fit needs the same trace seen in every space"
571 );
572 }
573 let joined: Vec<[f64; 7]> = points
574 .iter()
575 .zip(on_a)
576 .zip(on_b)
577 .map(|((p, a), b)| [p.x, p.y, p.z, a.x, a.y, b.x, b.y])
578 .collect();
579 let first = fit_spaced::<7>(
586 &joined,
587 degree,
588 tolerance,
589 smooth_loop,
590 Spacing::Centripetal,
591 tol,
592 )?;
593 let (knots, control, error, met) = if first.3 {
594 first
595 } else {
596 match fit_spaced::<7>(
597 &joined,
598 degree,
599 tolerance,
600 smooth_loop,
601 Spacing::ChordLength,
602 tol,
603 ) {
604 Ok(second) if second.2 < first.2 => second,
605 _ => first,
606 }
607 };
608 let curve = BSplineCurve::new(
609 knots.clone(),
610 control
611 .iter()
612 .map(|c| Point::new(c[0], c[1], c[2]))
613 .collect(),
614 tol,
615 )?;
616 let pa = BSpline2d::new(
617 knots.clone(),
618 control.iter().map(|c| Point2::new(c[3], c[4])).collect(),
619 tol,
620 )?;
621 let pb = BSpline2d::new(
622 knots,
623 control.iter().map(|c| Point2::new(c[5], c[6])).collect(),
624 tol,
625 )?;
626 Ok((Fitted { curve, error, met }, pa, pb))
627}
628
629pub fn fit_points_2d_at(
646 parameters: &[f64],
647 points: &[Point2],
648 degree: usize,
649 tolerance: f64,
650 tol: Tolerances,
651) -> OgeomResult<Fitted<BSpline2d>> {
652 fit_points_2d_at_inner(parameters, points, degree, tolerance, false, tol)
653}
654
655pub fn fit_points_at(
665 parameters: &[f64],
666 points: &[Point],
667 degree: usize,
668 tolerance: f64,
669 tol: Tolerances,
670) -> OgeomResult<Fitted<BSplineCurve>> {
671 fit_points_at_inner(parameters, points, degree, tolerance, false, tol)
672}
673
674pub fn fit_points_at_closed(
681 parameters: &[f64],
682 points: &[Point],
683 degree: usize,
684 tolerance: f64,
685 tol: Tolerances,
686) -> OgeomResult<Fitted<BSplineCurve>> {
687 fit_points_at_inner(parameters, points, degree, tolerance, true, tol)
688}
689
690fn fit_points_at_inner(
691 parameters: &[f64],
692 points: &[Point],
693 degree: usize,
694 tolerance: f64,
695 closed: bool,
696 tol: Tolerances,
697) -> OgeomResult<Fitted<BSplineCurve>> {
698 if parameters.len() != points.len() {
699 ogeom_bail!(Construction, "one parameter per point, or the fit is a lie");
700 }
701 if parameters.windows(2).any(|w| w[1] <= w[0]) {
702 ogeom_bail!(Construction, "fixed parameters must strictly increase");
703 }
704 if !tolerance.is_finite() || tolerance <= 0.0 {
705 ogeom_bail!(Construction, "a tolerance of {tolerance} is not a distance");
706 }
707 if degree == 0 {
708 ogeom_bail!(Construction, "a fit needs a degree of at least one");
709 }
710 let data: Vec<[f64; 3]> = points.iter().map(|p| [p.x, p.y, p.z]).collect();
711 if data.len() < 2 {
712 ogeom_bail!(Construction, "a fit needs at least two points");
713 }
714 let degree = degree.min(data.len() - 1);
715 let (a, b) = (parameters[0], parameters[parameters.len() - 1]);
716 let mut knots = single_span(degree, a, b)?;
717
718 const ROUNDS: usize = 32;
719 let mut best: Option<(KnotVector, Vec<[f64; 3]>, f64)> = None;
720 for _ in 0..ROUNDS {
721 let control = match least_squares::<3>(&knots, &data, parameters, closed) {
722 Ok(control) => control,
723 Err(e) => {
724 if best.is_some() {
725 break;
726 }
727 return Err(e);
728 }
729 };
730 let errors = residuals::<3>(&knots, &control, &data, parameters);
731 let worst = errors.iter().fold(0.0_f64, |acc, e| acc.max(e.1));
732 if best.as_ref().is_none_or(|(_, _, held)| worst < *held) {
733 best = Some((knots.clone(), control.clone(), worst));
734 }
735 if worst <= tolerance {
736 let curve = build_curve_3(knots, control, tol)?;
737 return Ok(Fitted {
738 curve,
739 error: worst,
740 met: true,
741 });
742 }
743 if knots.control_point_count() >= data.len() {
744 break;
745 }
746 let Some(refined) = refined_where_bad(&knots, &errors, tolerance, false)? else {
747 break;
748 };
749 knots = refined_or_interpolating(refined, parameters, closed)?;
750 }
751 let Some((knots, control, worst)) = best else {
752 ogeom_bail!(Construction, "the fit found no usable rounds");
753 };
754 let curve = build_curve_3(knots, control, tol)?;
755 Ok(Fitted {
756 curve,
757 error: worst,
758 met: false,
759 })
760}
761
762pub const SAMPLED_SPANS: usize = 8192;
765
766pub const SAMPLED_LOOP_SPANS: usize = 1024;
771
772pub fn fit_curve_sampled(
796 point: impl FnMut(f64) -> OgeomResult<Point>,
797 ts: &[f64],
798 closed: bool,
799 degree: usize,
800 tolerance: f64,
801 tol: Tolerances,
802) -> OgeomResult<Fitted<BSplineCurve>> {
803 let (knots, control, error, met) =
804 sampled::<3, Point>(point, |p| [p.x, p.y, p.z], ts, closed, degree, tolerance)?;
805 Ok(Fitted {
806 curve: build_curve_3(knots, control, tol)?,
807 error,
808 met,
809 })
810}
811
812pub fn fit_curve_2d_sampled(
820 point: impl FnMut(f64) -> OgeomResult<Point2>,
821 ts: &[f64],
822 closed: bool,
823 degree: usize,
824 tolerance: f64,
825 tol: Tolerances,
826) -> OgeomResult<Fitted<BSpline2d>> {
827 let (knots, control, error, met) =
828 sampled::<2, Point2>(point, |p| [p.x, p.y], ts, closed, degree, tolerance)?;
829 let curve = BSpline2d::new(
830 knots,
831 control
832 .into_iter()
833 .map(|c| Point2::new(c[0], c[1]))
834 .collect(),
835 tol,
836 )?;
837 Ok(Fitted { curve, error, met })
838}
839
840type SampledFit<const D: usize> = (KnotVector, Vec<[f64; D]>, f64, bool);
843
844fn sampled<const D: usize, P>(
846 mut trace: impl FnMut(f64) -> OgeomResult<P>,
847 coords: impl Fn(&P) -> [f64; D],
848 ts: &[f64],
849 closed: bool,
850 degree: usize,
851 tolerance: f64,
852) -> OgeomResult<SampledFit<D>> {
853 if !(tolerance > 0.0 && tolerance.is_finite()) {
854 ogeom_bail!(Construction, "a tolerance of {tolerance} is not a distance");
855 }
856 if degree == 0 {
857 ogeom_bail!(Construction, "a fit needs a degree of at least one");
858 }
859 if ts.len() < 2 || ts.iter().any(|t| !t.is_finite()) || ts.windows(2).any(|w| w[1] <= w[0]) {
860 ogeom_bail!(
861 Construction,
862 "a sampled fit needs strictly increasing parameters, at least two"
863 );
864 }
865 let most = if closed {
866 SAMPLED_LOOP_SPANS
867 } else {
868 SAMPLED_SPANS
869 };
870 let mut ts = ts.to_vec();
871 let mut seen: ogeom_core::FastMap<u64, [f64; D]> = ogeom_core::FastMap::default();
874 let mut at = |t: f64| -> OgeomResult<[f64; D]> {
875 if let Some(p) = seen.get(&t.to_bits()) {
876 return Ok(*p);
877 }
878 let p = coords(&trace(t)?);
879 seen.insert(t.to_bits(), p);
880 Ok(p)
881 };
882 let mut best: Option<(KnotVector, Vec<[f64; D]>, f64, bool)> = None;
884 loop {
885 let mut data = ts.iter().map(|t| at(*t)).collect::<OgeomResult<Vec<_>>>()?;
886 if closed {
887 data[ts.len() - 1] = data[0];
888 }
889 let mut split: Vec<bool> = Vec::new();
893 for interpolate in [false, true] {
894 let (knots, mut controls) = fit_family::<D>(
895 std::slice::from_ref(&data),
896 &ts,
897 degree,
898 tolerance * 0.5,
899 closed,
900 interpolate,
901 &[],
902 false,
903 )?;
904 let Some(control) = controls.pop() else {
905 ogeom_bail!(NotDone, "the curve fit solved nothing");
906 };
907 let mut error = residuals::<D>(&knots, &control, &data, &ts)
908 .iter()
909 .fold(0.0_f64, |acc, e| acc.max(e.1));
910 split = vec![false; ts.len() - 1];
911 let mut worst = Vec::with_capacity(ts.len() - 1);
912 for (i, pair) in ts.windows(2).enumerate() {
913 let mut off = (0.0_f64, 0);
914 for (k, t) in eighths(pair[0], pair[1]).into_iter().enumerate() {
915 let here = distance::<D>(&at(t)?, &position::<D>(&knots, &control, t));
916 if here > off.0 {
917 off = (here, k);
918 }
919 }
920 error = error.max(off.0);
921 split[i] = off.0 > tolerance;
922 worst.push(off);
923 }
924 let climbed = climbed_peaks(&ts, &worst, error, |t| {
925 Ok(distance::<D>(&at(t)?, &position::<D>(&knots, &control, t)))
926 });
927 for (i, peak) in climbed {
928 error = error.max(peak);
929 split[i] |= peak > tolerance;
930 }
931 let read = error <= tolerance;
934 if read {
935 let dense = read_densely(
936 &mut |t| {
937 Ok(at(t).map_or(0.0, |p| {
938 distance::<D>(&p, &position::<D>(&knots, &control, t))
939 }))
940 },
941 &knot_breaks(&knots, (ts[0], ts[ts.len() - 1])),
942 0,
943 error,
944 tolerance,
945 )?;
946 error = error.max(dense.widest.0);
947 for t in dense.misses {
948 split[span_of(&ts, t)] = true;
949 }
950 }
951 if error <= tolerance {
952 return Ok((knots, control, error, true));
953 }
954 if best.as_ref().is_none_or(|b| error < b.2) {
955 best = Some((knots, control, error, read));
956 }
957 }
958 let mut next = Vec::with_capacity(ts.len() * 2);
959 for (pair, &m) in ts.windows(2).zip(&split) {
960 next.push(pair[0]);
961 if m {
962 next.push(f64::midpoint(pair[0], pair[1]));
963 }
964 }
965 next.push(ts[ts.len() - 1]);
966 if next.len() == ts.len() || next.len() > most + 1 {
967 break;
968 }
969 ts = next;
970 }
971 let Some((knots, control, mut error, read)) = best else {
972 ogeom_bail!(Construction, "the curve could not be sampled");
973 };
974 if !read {
975 let dense = read_densely(
976 &mut |t| {
977 Ok(at(t).map_or(0.0, |p| {
978 distance::<D>(&p, &position::<D>(&knots, &control, t))
979 }))
980 },
981 &knot_breaks(&knots, (ts[0], ts[ts.len() - 1])),
982 0,
983 error,
984 tolerance,
985 )?;
986 error = error.max(dense.widest.0);
987 }
988 Ok((knots, control, error, false))
989}
990
991pub fn fit_trace_sampled(
1017 mut trace: impl FnMut(f64) -> OgeomResult<(Point, Point2)>,
1018 on_surface: impl Fn(Point2) -> OgeomResult<Point>,
1019 ts: &[f64],
1020 degree: usize,
1021 tolerance: f64,
1022 tol: Tolerances,
1023) -> OgeomResult<(Fitted<BSplineCurve>, BSpline2d)> {
1024 use crate::traits::Curve3d as _;
1025 const MOST: usize = 1024;
1026 if !(tolerance > 0.0 && tolerance.is_finite()) {
1027 ogeom_bail!(Construction, "a tolerance of {tolerance} is not a distance");
1028 }
1029 if degree == 0 {
1030 ogeom_bail!(Construction, "a fit needs a degree of at least one");
1031 }
1032 if ts.len() < 2 || ts.iter().any(|t| !t.is_finite()) || ts.windows(2).any(|w| w[1] <= w[0]) {
1033 ogeom_bail!(
1034 Construction,
1035 "a sampled fit needs strictly increasing parameters, at least two"
1036 );
1037 }
1038 let mut ts = ts.to_vec();
1039 let mut seen: ogeom_core::FastMap<u64, (Point, Point2)> = ogeom_core::FastMap::default();
1040 let mut at = |t: f64| -> OgeomResult<(Point, Point2)> {
1041 if let Some(p) = seen.get(&t.to_bits()) {
1042 return Ok(*p);
1043 }
1044 let p = trace(t)?;
1045 seen.insert(t.to_bits(), p);
1046 Ok(p)
1047 };
1048 let mut speed = [0.0_f64; 2];
1050 for t in &ts {
1051 let (_, uv) = at(*t)?;
1052 let here = on_surface(uv)?;
1053 for (axis, step) in [(0, Point2::new(1.0, 0.0)), (1, Point2::new(0.0, 1.0))] {
1054 let h = 1e-6 * (1.0 + uv.x.abs().max(uv.y.abs()));
1055 let mut moved = None;
1058 for sign in [1.0, -1.0] {
1059 let at = Point2::new(uv.x + sign * step.x * h, uv.y + sign * step.y * h);
1060 match on_surface(at) {
1061 Ok(p) => {
1062 moved = Some(p);
1063 break;
1064 }
1065 Err(ogeom_core::OgeomError::Domain(_)) => {}
1066 Err(e) => return Err(e),
1067 }
1068 }
1069 if let Some(p) = moved {
1070 speed[axis] = speed[axis].max(p.distance(here) / h);
1071 }
1072 }
1073 }
1074 let weight = speed.map(|s| if s > 0.0 && s.is_finite() { s } else { 1.0 });
1075 let mut best: Option<((Fitted<BSplineCurve>, BSpline2d), bool)> = None;
1077 loop {
1078 let samples = ts.iter().map(|t| at(*t)).collect::<OgeomResult<Vec<_>>>()?;
1079 let data: Vec<[f64; 5]> = samples
1080 .iter()
1081 .map(|(p, uv)| [p.x, p.y, p.z, uv.x * weight[0], uv.y * weight[1]])
1082 .collect();
1083 let mut split: Vec<bool> = Vec::new();
1084 for interpolate in [false, true] {
1085 let (knots, mut controls) = fit_family::<5>(
1086 std::slice::from_ref(&data),
1087 &ts,
1088 degree,
1089 tolerance * 0.5,
1090 false,
1091 interpolate,
1092 &[],
1093 false,
1094 )?;
1095 let Some(control) = controls.pop() else {
1096 ogeom_bail!(NotDone, "the trace fit solved nothing");
1097 };
1098 let curve = build_curve_3(
1099 knots.clone(),
1100 control.iter().map(|c| [c[0], c[1], c[2]]).collect(),
1101 tol,
1102 )?;
1103 let image = BSpline2d::new(
1104 knots,
1105 control
1106 .iter()
1107 .map(|c| Point2::new(c[3] / weight[0], c[4] / weight[1]))
1108 .collect(),
1109 tol,
1110 )?;
1111 let mut off_at = |t: f64| -> OgeomResult<f64> {
1112 let (p, _) = at(t)?;
1113 let c = curve.point_at(t, tol)?;
1114 let Some(s) = lifted(&on_surface, &image, t, tol)? else {
1115 return Ok(f64::INFINITY);
1116 };
1117 Ok(c.distance(p).max(s.distance(p)).max(c.distance(s)))
1118 };
1119 let mut error = 0.0_f64;
1120 for t in &ts {
1121 error = error.max(off_at(*t)?);
1122 }
1123 split = vec![false; ts.len() - 1];
1124 let mut worst = Vec::with_capacity(ts.len() - 1);
1125 for (i, pair) in ts.windows(2).enumerate() {
1126 let mut off = (0.0_f64, 0);
1127 for (k, t) in eighths(pair[0], pair[1]).into_iter().enumerate() {
1128 let here = off_at(t)?;
1129 if here > off.0 {
1130 off = (here, k);
1131 }
1132 }
1133 error = error.max(off.0);
1134 split[i] = off.0 > tolerance;
1135 worst.push(off);
1136 }
1137 for (i, peak) in climbed_peaks(&ts, &worst, error, &mut off_at) {
1138 error = error.max(peak);
1139 split[i] |= peak > tolerance;
1140 }
1141 let read = error <= tolerance;
1143 if read {
1144 let dense = read_densely(
1145 &mut |t| Ok(off_at(t).unwrap_or(0.0)),
1146 &knot_breaks(curve.knots(), (ts[0], ts[ts.len() - 1])),
1147 0,
1148 error,
1149 tolerance,
1150 )?;
1151 error = error.max(dense.widest.0);
1152 for t in dense.misses {
1153 split[span_of(&ts, t)] = true;
1154 }
1155 }
1156 let met = error <= tolerance;
1157 let candidate = (Fitted { curve, error, met }, image);
1158 if met {
1159 return Ok(candidate);
1160 }
1161 if best.as_ref().is_none_or(|b| error < b.0.0.error) {
1162 best = Some((candidate, read));
1163 }
1164 }
1165 let mut next = Vec::with_capacity(ts.len() * 2);
1166 for (pair, &m) in ts.windows(2).zip(&split) {
1167 next.push(pair[0]);
1168 if m {
1169 next.push(f64::midpoint(pair[0], pair[1]));
1170 }
1171 }
1172 next.push(ts[ts.len() - 1]);
1173 if next.len() == ts.len() || next.len() > MOST + 1 {
1174 break;
1175 }
1176 ts = next;
1177 }
1178 let Some(((mut fitted, image), read)) = best else {
1179 ogeom_bail!(Construction, "the trace could not be sampled");
1180 };
1181 if !read {
1182 let mut off_at = |t: f64| -> OgeomResult<f64> {
1183 let (p, _) = at(t)?;
1184 let c = fitted.curve.point_at(t, tol)?;
1185 let Some(s) = lifted(&on_surface, &image, t, tol)? else {
1186 return Ok(f64::INFINITY);
1187 };
1188 Ok(c.distance(p).max(s.distance(p)).max(c.distance(s)))
1189 };
1190 let dense = read_densely(
1191 &mut |t| Ok(off_at(t).unwrap_or(0.0)),
1192 &knot_breaks(fitted.curve.knots(), (ts[0], ts[ts.len() - 1])),
1193 0,
1194 fitted.error,
1195 tolerance,
1196 )?;
1197 fitted.error = fitted.error.max(dense.widest.0);
1198 }
1199 Ok((fitted, image))
1200}
1201
1202fn lifted(
1206 on_surface: &impl Fn(Point2) -> OgeomResult<Point>,
1207 image: &BSpline2d,
1208 t: f64,
1209 tol: Tolerances,
1210) -> OgeomResult<Option<Point>> {
1211 use crate::traits::Curve2d as _;
1212 match on_surface(image.point_at(t, tol)?) {
1213 Ok(p) => Ok(Some(p)),
1214 Err(ogeom_core::OgeomError::Domain(_)) => Ok(None),
1215 Err(e) => Err(e),
1216 }
1217}
1218
1219fn climbed_peaks(
1227 ts: &[f64],
1228 worst: &[(f64, usize)],
1229 error: f64,
1230 mut off: impl FnMut(f64) -> OgeomResult<f64>,
1231) -> Vec<(usize, f64)> {
1232 const NEAR_PEAK: f64 = 0.8;
1233 const PEAKS: usize = 16;
1234 let ratio = (5.0_f64.sqrt() - 1.0) / 2.0;
1235 let mut spans: Vec<usize> = (0..worst.len())
1236 .filter(|&i| worst[i].0 >= error * NEAR_PEAK && worst[i].0 > 0.0)
1237 .collect();
1238 spans.sort_by(|&a, &b| worst[b].0.total_cmp(&worst[a].0));
1239 let mut read = |t: f64| off(t).unwrap_or(0.0);
1240 spans
1241 .into_iter()
1242 .take(PEAKS)
1243 .map(|i| {
1244 let (lo, hi) = (ts[i], ts[i + 1]);
1245 let eighth = (hi - lo) / 8.0;
1246 let at = lo + eighth * precise(worst[i].1 + 1);
1248 let (mut a, mut b) = (at - eighth, at + eighth);
1249 let mut top = worst[i].0;
1250 for _ in 0..30 {
1251 let (c, d) = (b - (b - a) * ratio, a + (b - a) * ratio);
1252 let (rc, rd) = (read(c), read(d));
1253 top = top.max(rc).max(rd);
1254 if rc > rd {
1255 b = d;
1256 } else {
1257 a = c;
1258 }
1259 }
1260 (i, top)
1261 })
1262 .collect()
1263}
1264
1265fn eighths(a: f64, b: f64) -> [f64; 7] {
1270 let mid = f64::midpoint(a, b);
1271 let (q1, q3) = (f64::midpoint(a, mid), f64::midpoint(mid, b));
1272 [
1273 f64::midpoint(a, q1),
1274 q1,
1275 f64::midpoint(q1, mid),
1276 mid,
1277 f64::midpoint(mid, q3),
1278 q3,
1279 f64::midpoint(q3, b),
1280 ]
1281}
1282
1283fn build_curve_3(
1284 knots: KnotVector,
1285 control: Vec<[f64; 3]>,
1286 tol: Tolerances,
1287) -> OgeomResult<BSplineCurve> {
1288 BSplineCurve::new(
1289 knots,
1290 control
1291 .into_iter()
1292 .map(|c| Point::new(c[0], c[1], c[2]))
1293 .collect(),
1294 tol,
1295 )
1296}
1297
1298pub fn fit_points_2d_at_closed(
1309 parameters: &[f64],
1310 points: &[Point2],
1311 degree: usize,
1312 tolerance: f64,
1313 tol: Tolerances,
1314) -> OgeomResult<Fitted<BSpline2d>> {
1315 fit_points_2d_at_inner(parameters, points, degree, tolerance, true, tol)
1316}
1317
1318fn fit_points_2d_at_inner(
1319 parameters: &[f64],
1320 points: &[Point2],
1321 degree: usize,
1322 tolerance: f64,
1323 closed: bool,
1324 tol: Tolerances,
1325) -> OgeomResult<Fitted<BSpline2d>> {
1326 if parameters.len() != points.len() {
1327 ogeom_bail!(Construction, "one parameter per point, or the fit is a lie");
1328 }
1329 if parameters.windows(2).any(|w| w[1] <= w[0]) {
1330 ogeom_bail!(Construction, "fixed parameters must strictly increase");
1331 }
1332 if !tolerance.is_finite() || tolerance <= 0.0 {
1333 ogeom_bail!(Construction, "a tolerance of {tolerance} is not a distance");
1334 }
1335 if degree == 0 {
1336 ogeom_bail!(Construction, "a fit needs a degree of at least one");
1337 }
1338 let data: Vec<[f64; 2]> = points.iter().map(|p| [p.x, p.y]).collect();
1339 if data.len() < 2 {
1340 ogeom_bail!(Construction, "a fit needs at least two points");
1341 }
1342 let degree = degree.min(data.len() - 1);
1343 let (a, b) = (parameters[0], parameters[parameters.len() - 1]);
1344 let mut knots = single_span(degree, a, b)?;
1345
1346 const ROUNDS: usize = 32;
1347 let mut best: Option<(KnotVector, Vec<[f64; 2]>, f64)> = None;
1348 for _ in 0..ROUNDS {
1349 let control = match least_squares::<2>(&knots, &data, parameters, closed) {
1353 Ok(control) => control,
1354 Err(e) => {
1355 if best.is_some() {
1356 break;
1357 }
1358 return Err(e);
1359 }
1360 };
1361 let errors = residuals::<2>(&knots, &control, &data, parameters);
1362 let worst = errors.iter().fold(0.0_f64, |acc, e| acc.max(e.1));
1363 if best.as_ref().is_none_or(|(_, _, held)| worst < *held) {
1364 best = Some((knots.clone(), control.clone(), worst));
1365 }
1366 if worst <= tolerance {
1367 let curve = BSpline2d::new(
1368 knots,
1369 control
1370 .into_iter()
1371 .map(|c| Point2::new(c[0], c[1]))
1372 .collect(),
1373 tol,
1374 )?;
1375 return Ok(Fitted {
1376 curve,
1377 error: worst,
1378 met: true,
1379 });
1380 }
1381 if knots.control_point_count() >= data.len() {
1382 break;
1383 }
1384 let Some(refined) = refined_where_bad(&knots, &errors, tolerance, false)? else {
1385 break;
1386 };
1387 knots = refined_or_interpolating(refined, parameters, closed)?;
1388 }
1389 let (knots, control, error) = best.ok_or_else(|| {
1390 ogeom_core::ogeom_err!(Construction, "the fixed-parameter fit never solved")
1391 })?;
1392 let curve = BSpline2d::new(
1393 knots,
1394 control
1395 .into_iter()
1396 .map(|c| Point2::new(c[0], c[1]))
1397 .collect(),
1398 tol,
1399 )?;
1400 Ok(Fitted {
1401 curve,
1402 error,
1403 met: false,
1404 })
1405}
1406
1407#[derive(Clone, Copy, Debug, PartialEq, Eq)]
1410enum Spacing {
1411 Centripetal,
1414 ChordLength,
1422}
1423
1424fn fit<const D: usize>(
1430 points: &[[f64; D]],
1431 degree: usize,
1432 tolerance: f64,
1433 smooth_loop: bool,
1434 tol: Tolerances,
1435) -> OgeomResult<(KnotVector, Vec<[f64; D]>, f64, bool)> {
1436 fit_spaced::<D>(
1437 points,
1438 degree,
1439 tolerance,
1440 smooth_loop.then_some(D),
1441 Spacing::Centripetal,
1442 tol,
1443 )
1444}
1445
1446fn fit_spaced<const D: usize>(
1447 points: &[[f64; D]],
1448 degree: usize,
1449 tolerance: f64,
1450 smooth_loop: Option<usize>,
1451 spacing: Spacing,
1452 tol: Tolerances,
1453) -> OgeomResult<(KnotVector, Vec<[f64; D]>, f64, bool)> {
1454 if !tolerance.is_finite() || tolerance <= 0.0 {
1455 ogeom_bail!(Construction, "a tolerance of {tolerance} is not a distance");
1456 }
1457 if degree == 0 {
1458 ogeom_bail!(Construction, "a fit needs a degree of at least one");
1459 }
1460 let points = collapse::<D>(points, tol);
1461 if points.len() < 2 {
1462 ogeom_bail!(
1463 Construction,
1464 "a fit needs at least two distinct points, got {}",
1465 points.len()
1466 );
1467 }
1468 let degree = degree.min(points.len() - 1);
1469 let closed = smooth_loop.is_some_and(|meeting| {
1477 let (first, last) = (&points[0], &points[points.len() - 1]);
1478 first[..meeting]
1479 .iter()
1480 .zip(&last[..meeting])
1481 .map(|(x, y)| (x - y) * (x - y))
1482 .sum::<f64>()
1483 .sqrt()
1484 <= tol.confusion()
1485 });
1486 let parameters = match spacing {
1487 Spacing::Centripetal => centripetal::<D>(&points),
1488 Spacing::ChordLength => chordal::<D>(&points),
1489 };
1490
1491 let (a, b) = (parameters[0], parameters[parameters.len() - 1]);
1494 let mut knots = single_span(degree, a, b)?;
1495
1496 const ROUNDS: usize = 32;
1499 let mut parameters = parameters;
1500 let mut best: Option<(KnotVector, Vec<[f64; D]>, f64)> = None;
1501 for _ in 0..ROUNDS {
1502 let control = match least_squares::<D>(&knots, &points, ¶meters, closed) {
1507 Ok(control) => control,
1508 Err(e) => {
1509 if let Some((knots, control, worst)) = best {
1510 return Ok((knots, control, worst, false));
1511 }
1512 return Err(e);
1513 }
1514 };
1515 for _ in 0..2 {
1525 correct_parameters::<D>(&knots, &control, &points, &mut parameters, closed);
1526 }
1527 let mut errors = residuals::<D>(&knots, &control, &points, ¶meters);
1528 wandering::<D>(&knots, &control, &points, ¶meters, &mut errors);
1529 let worst = errors.iter().fold(0.0_f64, |acc, e| acc.max(e.1));
1530 if best.as_ref().is_none_or(|(_, _, held)| worst < *held) {
1531 best = Some((knots.clone(), control.clone(), worst));
1532 }
1533 if worst <= tolerance {
1534 return Ok((knots, control, worst, true));
1535 }
1536
1537 if knots.control_point_count() >= points.len() + usize::from(closed) {
1542 break;
1543 }
1544 let Some(refined) = refined_where_bad(&knots, &errors, tolerance, false)? else {
1545 break;
1546 };
1547 knots = refined_or_interpolating(refined, ¶meters, closed)?;
1548 }
1549
1550 #[allow(clippy::unwrap_used, reason = "at least one round always runs")]
1551 let (knots, control, worst) = best.unwrap();
1552 Ok((knots, control, worst, false))
1553}
1554
1555fn collapse<const D: usize>(points: &[[f64; D]], tol: Tolerances) -> Vec<[f64; D]> {
1558 let mut out: Vec<[f64; D]> = Vec::with_capacity(points.len());
1559 for p in points {
1560 if out
1561 .last()
1562 .is_some_and(|q| distance::<D>(p, q) <= tol.confusion() * 0.01)
1563 {
1564 continue;
1565 }
1566 out.push(*p);
1567 }
1568 out
1569}
1570
1571fn distance<const D: usize>(a: &[f64; D], b: &[f64; D]) -> f64 {
1572 a.iter()
1573 .zip(b)
1574 .map(|(x, y)| (x - y) * (x - y))
1575 .sum::<f64>()
1576 .sqrt()
1577}
1578
1579pub fn fit_surface_scattered(
1599 points: &[Point],
1600 degree: usize,
1601 controls: (usize, usize),
1602 smoothing: f64,
1603 tol: Tolerances,
1604) -> OgeomResult<Fitted<crate::BSplineSurface>> {
1605 use crate::traits::Surface as _;
1606 if !smoothing.is_finite() || smoothing < 0.0 {
1607 ogeom_bail!(
1608 Construction,
1609 "a smoothing weight of {smoothing} is not a weight"
1610 );
1611 }
1612 let (nu, nv) = controls;
1613 if nu < degree + 1 || nv < degree + 1 {
1614 ogeom_bail!(
1615 Construction,
1616 "{nu}x{nv} control points cannot carry degree {degree}"
1617 );
1618 }
1619 let m = points.len();
1620 if m < 4 {
1621 ogeom_bail!(Construction, "a scattered fit needs at least four points");
1622 }
1623
1624 let centroid = {
1627 let mut sum = ogeom_math::Vector::ZERO;
1628 for p in points {
1629 sum += p.to_vector();
1630 }
1631 sum / precise(m)
1632 };
1633 let mut covariance = nalgebra::Matrix3::<f64>::zeros();
1634 for p in points {
1635 let d = p.to_vector() - centroid;
1636 let v = nalgebra::Vector3::new(d.x, d.y, d.z);
1637 covariance += v * v.transpose();
1638 }
1639 let eigen = nalgebra::SymmetricEigen::new(covariance);
1640 let mut order: Vec<usize> = (0..3).collect();
1641 order.sort_by(|a, b| {
1642 eigen.eigenvalues[*b]
1643 .partial_cmp(&eigen.eigenvalues[*a])
1644 .unwrap_or(core::cmp::Ordering::Equal)
1645 });
1646 let axis = |i: usize| {
1647 let c = eigen.eigenvectors.column(order[i]);
1648 ogeom_math::Vector::new(c[0], c[1], c[2])
1649 };
1650 let (u_axis, v_axis) = (axis(0), axis(1));
1651
1652 let mut spans = Vec::with_capacity(m);
1654 let (mut ulo, mut uhi) = (f64::INFINITY, f64::NEG_INFINITY);
1655 let (mut vlo, mut vhi) = (f64::INFINITY, f64::NEG_INFINITY);
1656 for p in points {
1657 let d = p.to_vector() - centroid;
1658 let (pu, pv) = (d.dot(u_axis), d.dot(v_axis));
1659 ulo = ulo.min(pu);
1660 uhi = uhi.max(pu);
1661 vlo = vlo.min(pv);
1662 vhi = vhi.max(pv);
1663 spans.push((pu, pv));
1664 }
1665 if uhi - ulo <= tol.confusion() || vhi - vlo <= tol.confusion() {
1666 ogeom_bail!(
1667 Construction,
1668 "the cloud is flat in a principal direction; fit a curve"
1669 );
1670 }
1671 let u_knots = KnotVector::clamped_uniform(degree, nu)?;
1672 let v_knots = KnotVector::clamped_uniform(degree, nv)?;
1673 let (ka, kb) = (u_knots.domain_start(), u_knots.domain_end());
1674 let (la, lb) = (v_knots.domain_start(), v_knots.domain_end());
1675 let parameters: Vec<(f64, f64)> = spans
1676 .iter()
1677 .map(|(pu, pv)| {
1678 (
1679 ka + (kb - ka) * ((pu - ulo) / (uhi - ulo)),
1680 la + (lb - la) * ((pv - vlo) / (vhi - vlo)),
1681 )
1682 })
1683 .collect();
1684
1685 let n = nu * nv;
1687 let mut a = nalgebra::DMatrix::<f64>::zeros(m, n);
1688 for (k, &(pu, pv)) in parameters.iter().enumerate() {
1689 let uspan = u_knots.span_unchecked(pu);
1690 let vspan = v_knots.span_unchecked(pv);
1691 let ub = u_knots.basis(uspan, pu);
1692 let vb = v_knots.basis(vspan, pv);
1693 let ufirst = uspan - degree;
1694 let vfirst = vspan - degree;
1695 for (j, bv) in vb.iter().enumerate() {
1696 for (i, bu) in ub.iter().enumerate() {
1697 a[(k, (ufirst + i) * nv + (vfirst + j))] = bu * bv;
1698 }
1699 }
1700 }
1701 let mut k_full = a.transpose() * &a;
1702 let mut add_penalty = |along_u: bool| {
1703 let (count_a, count_b) = if along_u { (nu, nv) } else { (nv, nu) };
1706 for jb in 0..count_b {
1707 for ia in 0..count_a.saturating_sub(2) {
1708 let base = |offset: usize| -> usize {
1709 if along_u {
1710 (ia + offset) * nv + jb
1711 } else {
1712 jb * nv + ia + offset
1713 }
1714 };
1715 let idx = [base(0), base(1), base(2)];
1716 let w = [1.0, -2.0, 1.0];
1717 for x in 0..3 {
1718 for y in 0..3 {
1719 k_full[(idx[x], idx[y])] += smoothing * w[x] * w[y];
1720 }
1721 }
1722 }
1723 }
1724 };
1725 add_penalty(true);
1726 add_penalty(false);
1727
1728 let decomposition = k_full.clone().lu();
1729 let mut control = vec![Point::ORIGIN; n];
1730 for axis_i in 0..3 {
1731 let b = nalgebra::DVector::from_iterator(m, points.iter().map(|p| [p.x, p.y, p.z][axis_i]));
1732 let rhs = a.transpose() * &b;
1733 let Some(solved) = decomposition.solve(&rhs) else {
1734 ogeom_bail!(
1735 Numeric,
1736 "the scattered system is singular; raise the smoothing or lower the controls"
1737 );
1738 };
1739 for (slot, value) in solved.iter().enumerate() {
1740 match axis_i {
1741 0 => control[slot].x = *value,
1742 1 => control[slot].y = *value,
1743 _ => control[slot].z = *value,
1744 }
1745 }
1746 }
1747
1748 let grid = ogeom_math::ControlGrid::new(control, nu, nv)?;
1749 let surface = crate::BSplineSurface::new(u_knots, v_knots, &grid, tol)?;
1750 let mut error = 0.0f64;
1751 for (p, &(pu, pv)) in points.iter().zip(¶meters) {
1752 error = error.max(surface.point_at(pu, pv, tol)?.distance(*p));
1753 }
1754 Ok(Fitted {
1755 curve: surface,
1756 error,
1757 met: true,
1758 })
1759}
1760
1761pub fn fit_surface_scattered_at(
1786 points: &[Point],
1787 parameters: &[(f64, f64)],
1788 u_knots: &KnotVector,
1789 v_knots: &KnotVector,
1790 fairing: f64,
1791 tol: Tolerances,
1792) -> OgeomResult<Fitted<crate::BSplineSurface>> {
1793 use crate::traits::Surface as _;
1794 if !fairing.is_finite() || fairing < 0.0 {
1795 ogeom_bail!(
1796 Construction,
1797 "a fairing weight of {fairing} is not a weight"
1798 );
1799 }
1800 let m = points.len();
1801 if m != parameters.len() {
1802 ogeom_bail!(
1803 Construction,
1804 "{m} points were given {} parameters",
1805 parameters.len()
1806 );
1807 }
1808 if m < 4 {
1809 ogeom_bail!(Construction, "a scattered fit needs at least four points");
1810 }
1811 let (ua, ub) = u_knots.domain();
1812 let (va, vb) = v_knots.domain();
1813 if let Some(&(u, v)) = parameters
1814 .iter()
1815 .find(|&&(u, v)| !(u >= ua && u <= ub && v >= va && v <= vb))
1816 {
1817 ogeom_bail!(
1818 Construction,
1819 "the parameter ({u}, {v}) is outside the domain [{ua}, {ub}] x [{va}, {vb}]"
1820 );
1821 }
1822 let (pu, pv) = (u_knots.degree(), v_knots.degree());
1823 let (nu, nv) = (u_knots.control_point_count(), v_knots.control_point_count());
1824 let n = nu * nv;
1825 let rows_along_u = nu >= nv;
1830 let slot = |i: usize, j: usize| {
1831 if rows_along_u { i * nv + j } else { j * nu + i }
1832 };
1833 let band = if rows_along_u {
1834 pu * nv + pv
1835 } else {
1836 pv * nu + pu
1837 };
1838 let mut lower = vec![vec![0.0; band + 1]; n];
1839 let mut rhs = vec![[0.0; 3]; n];
1840 let add = |lower: &mut [Vec<f64>], r: usize, c: usize, value: f64| {
1841 let (r, c) = if r >= c { (r, c) } else { (c, r) };
1842 lower[r][r - c] += value;
1843 };
1844
1845 let share = 1.0 / precise(m);
1846 let mut terms: Vec<(usize, f64)> = Vec::with_capacity((pu + 1) * (pv + 1));
1847 for (p, &(u, v)) in points.iter().zip(parameters) {
1848 let (us, vs) = (u_knots.span_unchecked(u), v_knots.span_unchecked(v));
1849 let (bu, bv) = (u_knots.basis(us, u), v_knots.basis(vs, v));
1850 terms.clear();
1851 for (a, x) in bu.iter().enumerate() {
1852 for (b, y) in bv.iter().enumerate() {
1853 terms.push((slot(us - pu + a, vs - pv + b), x * y));
1854 }
1855 }
1856 for (k, &(r, x)) in terms.iter().enumerate() {
1857 rhs[r][0] += share * x * p.x;
1858 rhs[r][1] += share * x * p.y;
1859 rhs[r][2] += share * x * p.z;
1860 for &(c, y) in &terms[..=k] {
1861 add(&mut lower, r, c, share * x * y);
1862 }
1863 }
1864 }
1865
1866 if fairing > 0.0 {
1872 let (gu, gv) = (derivative_grams(u_knots), derivative_grams(v_knots));
1873 for a in 0..nu {
1874 for b in 0..nv {
1875 let r = slot(a, b);
1876 for c in a.saturating_sub(pu)..(a + pu + 1).min(nu) {
1877 for d in b.saturating_sub(pv)..(b + pv + 1).min(nv) {
1878 let col = slot(c, d);
1879 if col > r {
1880 continue;
1881 }
1882 let (f, x) = (a.min(c), a.abs_diff(c));
1883 let (e, y) = (b.min(d), b.abs_diff(d));
1884 let energy = gu[2][f][x] * gv[0][e][y]
1885 + 2.0 * gu[1][f][x] * gv[1][e][y]
1886 + gu[0][f][x] * gv[2][e][y];
1887 lower[r][r - col] += fairing * energy;
1888 }
1889 }
1890 }
1891 }
1892 }
1893
1894 let Some(factor) = banded_cholesky(&lower, band) else {
1895 ogeom_bail!(
1896 Numeric,
1897 "the scattered system is singular; raise the fairing or lower the controls"
1898 );
1899 };
1900 let solved: Vec<Vec<f64>> = (0..3)
1901 .map(|axis| {
1902 let b: Vec<f64> = rhs.iter().map(|r| r[axis]).collect();
1903 banded_substitute(&factor, band, &b)
1904 })
1905 .collect();
1906 let mut control = Vec::with_capacity(n);
1908 for i in 0..nu {
1909 for j in 0..nv {
1910 let k = slot(i, j);
1911 control.push(Point::new(solved[0][k], solved[1][k], solved[2][k]));
1912 }
1913 }
1914 let grid = ogeom_math::ControlGrid::new(control, nu, nv)?;
1915 let surface = crate::BSplineSurface::new(u_knots.clone(), v_knots.clone(), &grid, tol)?;
1916 let mut error = 0.0_f64;
1917 for (p, &(u, v)) in points.iter().zip(parameters) {
1918 error = error.max(surface.point_at(u, v, tol)?.distance(*p));
1919 }
1920 Ok(Fitted {
1921 curve: surface,
1922 error,
1923 met: true,
1924 })
1925}
1926
1927fn derivative_grams(knots: &KnotVector) -> [Vec<Vec<f64>>; 3] {
1932 let p = knots.degree();
1933 let n = knots.control_point_count();
1934 let mut grams: [Vec<Vec<f64>>; 3] = core::array::from_fn(|_| vec![vec![0.0; p + 1]; n]);
1935 let distinct = knots.distinct();
1936 for w in distinct.windows(2) {
1937 let (a, b) = (w[0].0, w[1].0);
1938 for (t, weight) in ogeom_math::gauss_legendre_rule(a, b) {
1939 let span = knots.span_unchecked(t);
1940 let rows = knots.basis_derivatives(span, t, 2);
1941 for (k, gram) in grams.iter_mut().enumerate() {
1942 let Some(row) = rows.get(k) else {
1943 continue;
1944 };
1945 for i in 0..=p {
1946 for j in i..=p {
1947 gram[span - p + i][j - i] += weight * row[i] * row[j];
1948 }
1949 }
1950 }
1951 }
1952 }
1953 grams
1954}
1955
1956pub fn fill_boundary(
1974 bottom: &crate::Curve,
1975 top: &crate::Curve,
1976 left: &crate::Curve,
1977 right: &crate::Curve,
1978 samples: usize,
1979 tolerance: f64,
1980 tol: Tolerances,
1981) -> OgeomResult<Fitted<crate::BSplineSurface>> {
1982 use crate::traits::Curve3d as _;
1983 let samples = samples.max(4);
1984 let at = |curve: &crate::Curve, t: f64| -> OgeomResult<Point> {
1985 let (lo, hi) = curve.domain();
1986 curve.point_at(lo + (hi - lo) * t, tol)
1987 };
1988 let c00 = at(bottom, 0.0)?;
1990 let c10 = at(bottom, 1.0)?;
1991 let c01 = at(top, 0.0)?;
1992 let c11 = at(top, 1.0)?;
1993 let slack = tol.confusion() * 1e3;
1994 for (name, a, b) in [
1995 ("bottom-left", c00, at(left, 0.0)?),
1996 ("top-left", c01, at(left, 1.0)?),
1997 ("bottom-right", c10, at(right, 0.0)?),
1998 ("top-right", c11, at(right, 1.0)?),
1999 ] {
2000 if a.distance(b) > slack {
2001 ogeom_bail!(
2002 Construction,
2003 "the {name} corner does not close: the boundaries miss by {}",
2004 a.distance(b)
2005 );
2006 }
2007 }
2008
2009 let coons = |u: f64, v: f64| -> OgeomResult<Point> {
2012 let cu0 = at(bottom, u)?;
2013 let cu1 = at(top, u)?;
2014 let d0v = at(left, v)?;
2015 let d1v = at(right, v)?;
2016 let ruled_u = cu0.to_vector() * (1.0 - v) + cu1.to_vector() * v;
2017 let ruled_v = d0v.to_vector() * (1.0 - u) + d1v.to_vector() * u;
2018 let corners = c00.to_vector() * ((1.0 - u) * (1.0 - v))
2019 + c10.to_vector() * (u * (1.0 - v))
2020 + c01.to_vector() * ((1.0 - u) * v)
2021 + c11.to_vector() * (u * v);
2022 Ok(Point::from_vector(ruled_u + ruled_v - corners))
2023 };
2024 fit_surface_sampled(
2027 coons,
2028 &Sampling::even(((0.0, 1.0), (0.0, 1.0)), samples - 1),
2029 3,
2030 tolerance,
2031 tol,
2032 )
2033}
2034
2035pub fn fit_surface_grid(
2060 rows: &[Vec<Point>],
2061 degree: usize,
2062 tolerance: f64,
2063 tol: Tolerances,
2064) -> OgeomResult<Fitted<crate::BSplineSurface>> {
2065 fit_surface_grid_inner(rows, degree, tolerance, false, false, tol)
2066}
2067
2068pub fn fit_surface_grid_chordal(
2077 rows: &[Vec<Point>],
2078 degree: usize,
2079 tolerance: f64,
2080 tol: Tolerances,
2081) -> OgeomResult<Fitted<crate::BSplineSurface>> {
2082 fit_surface_grid_inner(rows, degree, tolerance, false, true, tol)
2083}
2084
2085pub fn fit_surface_grid_sections(
2097 rows: &[Vec<Point>],
2098 degree: usize,
2099 tolerance: f64,
2100 tol: Tolerances,
2101) -> OgeomResult<Fitted<crate::BSplineSurface>> {
2102 fit_surface_grid_parameterized(rows, degree, tolerance, false, (false, true), tol)
2103}
2104
2105pub fn fit_surface_grid_closed_v(
2116 rows: &[Vec<Point>],
2117 degree: usize,
2118 tolerance: f64,
2119 tol: Tolerances,
2120) -> OgeomResult<Fitted<crate::BSplineSurface>> {
2121 if rows.len() < 3 {
2122 ogeom_bail!(Construction, "a closed skin needs at least three rows");
2123 }
2124 let (first, last) = (&rows[0], &rows[rows.len() - 1]);
2125 if first.len() != last.len()
2126 || first
2127 .iter()
2128 .zip(last)
2129 .any(|(a, b)| a.distance(*b) > tol.confusion() * 100.0)
2130 {
2131 ogeom_bail!(
2132 Construction,
2133 "a closed skin needs a loop: the first row repeated at the end"
2134 );
2135 }
2136 fit_surface_grid_inner(rows, degree, tolerance, true, false, tol)
2137}
2138
2139pub fn fit_surface_grid_closed_v_chordal(
2150 rows: &[Vec<Point>],
2151 degree: usize,
2152 tolerance: f64,
2153 tol: Tolerances,
2154) -> OgeomResult<Fitted<crate::BSplineSurface>> {
2155 if rows.len() < 3 {
2156 ogeom_bail!(Construction, "a closed skin needs at least three rows");
2157 }
2158 let (first, last) = (&rows[0], &rows[rows.len() - 1]);
2159 if first.len() != last.len()
2160 || first
2161 .iter()
2162 .zip(last)
2163 .any(|(a, b)| a.distance(*b) > tol.confusion() * 100.0)
2164 {
2165 ogeom_bail!(
2166 Construction,
2167 "a closed skin needs a loop: the first row repeated at the end"
2168 );
2169 }
2170 fit_surface_grid_inner(rows, degree, tolerance, true, true, tol)
2171}
2172
2173fn fit_surface_grid_inner(
2174 rows: &[Vec<Point>],
2175 degree: usize,
2176 tolerance: f64,
2177 closed_v: bool,
2178 by_chord: bool,
2179 tol: Tolerances,
2180) -> OgeomResult<Fitted<crate::BSplineSurface>> {
2181 fit_surface_grid_parameterized(rows, degree, tolerance, closed_v, (by_chord, by_chord), tol)
2182}
2183
2184fn fit_surface_grid_parameterized(
2188 rows: &[Vec<Point>],
2189 degree: usize,
2190 tolerance: f64,
2191 closed_v: bool,
2192 by_chord: (bool, bool),
2193 tol: Tolerances,
2194) -> OgeomResult<Fitted<crate::BSplineSurface>> {
2195 if !tolerance.is_finite() || tolerance <= 0.0 {
2196 ogeom_bail!(Construction, "a tolerance of {tolerance} is not a distance");
2197 }
2198 let nv = rows.len();
2199 if nv < 2 {
2200 ogeom_bail!(Construction, "a surface fit needs at least two rows");
2201 }
2202 let nu = rows[0].len();
2203 if nu < 2 || rows.iter().any(|r| r.len() != nu) {
2204 ogeom_bail!(Construction, "a surface fit needs a rectangular grid");
2205 }
2206 let raw: Vec<Vec<[f64; 3]>> = rows
2207 .iter()
2208 .map(|r| r.iter().map(|p| [p.x, p.y, p.z]).collect())
2209 .collect();
2210
2211 let (u_params, v_params) = averaged_parameters(&raw, by_chord);
2212 fit_grid_on(
2213 rows,
2214 &raw,
2215 (&u_params, &v_params),
2216 degree,
2217 tolerance,
2218 (closed_v, false),
2219 (&[], &[]),
2220 tol,
2221 )
2222}
2223
2224pub fn grid_parameters(
2234 rows: &[Vec<Point>],
2235 by_chord: (bool, bool),
2236) -> OgeomResult<(Vec<f64>, Vec<f64>)> {
2237 let nu = rows.first().map_or(0, Vec::len);
2238 if rows.len() < 2 || nu < 2 || rows.iter().any(|r| r.len() != nu) {
2239 ogeom_bail!(Construction, "a surface fit needs a rectangular grid");
2240 }
2241 let raw: Vec<Vec<[f64; 3]>> = rows
2242 .iter()
2243 .map(|r| r.iter().map(|p| [p.x, p.y, p.z]).collect())
2244 .collect();
2245 Ok(averaged_parameters(&raw, by_chord))
2246}
2247
2248fn averaged_parameters(raw: &[Vec<[f64; 3]>], by_chord: (bool, bool)) -> (Vec<f64>, Vec<f64>) {
2256 let average = |families: &[Vec<[f64; 3]>], by_chord: bool| -> Vec<f64> {
2257 let mut sums = vec![0.0; families[0].len()];
2258 let mut counted = 0.0_f64;
2259 for family in families {
2260 let extent: f64 = family
2261 .windows(2)
2262 .map(|pair| distance::<3>(&pair[0], &pair[1]))
2263 .sum();
2264 if extent <= 0.0 {
2265 continue;
2266 }
2267 counted += 1.0;
2268 let assigned = if by_chord {
2269 chordal::<3>(family)
2270 } else {
2271 centripetal::<3>(family)
2272 };
2273 for (s, p) in sums.iter_mut().zip(assigned) {
2274 *s += p;
2275 }
2276 }
2277 if counted == 0.0 {
2278 let n = sums.len();
2280 return (0..n)
2281 .map(|i| {
2282 #[allow(clippy::cast_precision_loss)]
2283 let t = i as f64 / (n - 1).max(1) as f64;
2284 t
2285 })
2286 .collect();
2287 }
2288 sums.iter().map(|s| s / counted).collect()
2289 };
2290 let nu = raw[0].len();
2291 let u_params = average(raw, by_chord.0);
2292 let columns: Vec<Vec<[f64; 3]>> = (0..nu)
2293 .map(|i| raw.iter().map(|r| r[i]).collect())
2294 .collect();
2295 let v_params = average(&columns, by_chord.1);
2296 (u_params, v_params)
2297}
2298
2299pub fn fit_surface_grid_at(
2314 u_params: &[f64],
2315 v_params: &[f64],
2316 rows: &[Vec<Point>],
2317 degree: usize,
2318 tolerance: f64,
2319 tol: Tolerances,
2320) -> OgeomResult<Fitted<crate::BSplineSurface>> {
2321 fit_surface_grid_at_with_knots(u_params, v_params, rows, degree, tolerance, (&[], &[]), tol)
2322}
2323
2324type Kept<'a> = &'a [(f64, usize)];
2326
2327pub fn fit_surface_grid_at_with_knots(
2340 u_params: &[f64],
2341 v_params: &[f64],
2342 rows: &[Vec<Point>],
2343 degree: usize,
2344 tolerance: f64,
2345 knots: (Kept<'_>, Kept<'_>),
2346 tol: Tolerances,
2347) -> OgeomResult<Fitted<crate::BSplineSurface>> {
2348 if !tolerance.is_finite() || tolerance <= 0.0 {
2349 ogeom_bail!(Construction, "a tolerance of {tolerance} is not a distance");
2350 }
2351 if rows.len() < 2 || rows.len() != v_params.len() {
2352 ogeom_bail!(
2353 Construction,
2354 "a surface fit needs at least two rows, one v parameter each"
2355 );
2356 }
2357 if u_params.len() < 2 || rows.iter().any(|r| r.len() != u_params.len()) {
2358 ogeom_bail!(
2359 Construction,
2360 "a surface fit needs a rectangular grid, one u parameter per column"
2361 );
2362 }
2363 if u_params.windows(2).any(|w| w[1] <= w[0]) || v_params.windows(2).any(|w| w[1] <= w[0]) {
2364 ogeom_bail!(Construction, "fixed parameters must strictly increase");
2365 }
2366 let raw: Vec<Vec<[f64; 3]>> = rows
2367 .iter()
2368 .map(|r| r.iter().map(|p| [p.x, p.y, p.z]).collect())
2369 .collect();
2370 fit_grid_on(
2371 rows,
2372 &raw,
2373 (u_params, v_params),
2374 degree,
2375 tolerance,
2376 (false, false),
2377 knots,
2378 tol,
2379 )
2380}
2381
2382#[derive(Clone, Debug)]
2384pub struct Sampling {
2385 pub us: Vec<f64>,
2387 pub vs: Vec<f64>,
2389 pub between: (bool, bool),
2394 pub closed_v: bool,
2398 pub most: usize,
2400}
2401
2402impl Sampling {
2403 #[must_use]
2406 pub fn even(domain: ((f64, f64), (f64, f64)), spans: usize) -> Self {
2407 let even = |(a, b): (f64, f64)| -> Vec<f64> {
2408 let n = spans.max(1);
2409 (0..=n)
2410 .map(|i| {
2411 if i == n {
2412 b
2413 } else {
2414 a + (b - a) * precise(i) / precise(n)
2415 }
2416 })
2417 .collect()
2418 };
2419 Self {
2420 us: even(domain.0),
2421 vs: even(domain.1),
2422 between: (true, true),
2423 closed_v: false,
2424 most: 512,
2425 }
2426 }
2427}
2428
2429pub fn fit_surface_sampled(
2457 mut point: impl FnMut(f64, f64) -> OgeomResult<Point>,
2458 sampling: &Sampling,
2459 degree: usize,
2460 tolerance: f64,
2461 tol: Tolerances,
2462) -> OgeomResult<Fitted<crate::BSplineSurface>> {
2463 if !(tolerance > 0.0 && tolerance.is_finite()) {
2464 ogeom_bail!(Construction, "a tolerance of {tolerance} is not a distance");
2465 }
2466 let increasing = |ps: &[f64]| {
2467 ps.len() >= 2 && ps.iter().all(|p| p.is_finite()) && ps.windows(2).all(|w| w[1] > w[0])
2468 };
2469 if !increasing(&sampling.us) || !increasing(&sampling.vs) {
2470 ogeom_bail!(
2471 Construction,
2472 "a sampled fit needs strictly increasing parameters, two a side"
2473 );
2474 }
2475 let (mut us, mut vs) = (sampling.us.clone(), sampling.vs.clone());
2476 let mut best: Option<(Fitted<crate::BSplineSurface>, bool)> = None;
2478 let mut seen: ogeom_core::FastMap<(u64, u64), Point> = ogeom_core::FastMap::default();
2481 let mut at = |u: f64, v: f64| -> OgeomResult<Point> {
2482 if let Some(p) = seen.get(&(u.to_bits(), v.to_bits())) {
2483 return Ok(*p);
2484 }
2485 let p = point(u, v)?;
2486 seen.insert((u.to_bits(), v.to_bits()), p);
2487 Ok(p)
2488 };
2489 loop {
2490 let mut rows = vs
2491 .iter()
2492 .map(|v| us.iter().map(|u| at(*u, *v)).collect())
2493 .collect::<OgeomResult<Vec<Vec<Point>>>>()?;
2494 if sampling.closed_v {
2495 let first = rows[0].clone();
2496 if let Some(last) = rows.last_mut() {
2497 *last = first;
2498 }
2499 }
2500 let raw: Vec<Vec<[f64; 3]>> = rows
2501 .iter()
2502 .map(|r| r.iter().map(|p| [p.x, p.y, p.z]).collect())
2503 .collect();
2504 let mut splits: Option<(Vec<bool>, Vec<bool>)> = None;
2511 for interpolate in [false, true] {
2512 let fitted = fit_grid_on(
2513 &rows,
2514 &raw,
2515 (&us, &vs),
2516 degree,
2517 tolerance * 0.5,
2518 (sampling.closed_v, interpolate),
2519 (&[], &[]),
2520 tol,
2521 )?;
2522 let (between, mut split_u, mut split_v) = measured_between(
2523 &mut at,
2524 &fitted.curve,
2525 (&us, &vs),
2526 sampling.between,
2527 tolerance,
2528 tol,
2529 )?;
2530 let mut error = fitted.error.max(between);
2531 let read_densely = error <= tolerance;
2535 if read_densely {
2536 let along = measured_along_lines(
2537 &mut at,
2538 &fitted.curve,
2539 (&us, &vs),
2540 sampling.between,
2541 tolerance,
2542 error,
2543 (&mut split_u, &mut split_v),
2544 tol,
2545 )?;
2546 error = error.max(along);
2547 }
2548 let candidate = Fitted {
2549 curve: fitted.curve,
2550 error,
2551 met: error <= tolerance,
2552 };
2553 if candidate.met {
2554 return Ok(candidate);
2555 }
2556 if best.as_ref().is_none_or(|b| error < b.0.error) {
2557 best = Some((candidate, read_densely));
2558 }
2559 splits = Some((split_u, split_v));
2560 }
2561 let Some((split_u, split_v)) = splits else {
2562 break;
2563 };
2564 let split = |knots: &[f64], marked: &[bool]| -> Vec<f64> {
2565 let mut out = Vec::with_capacity(knots.len() * 2);
2566 for (pair, &m) in knots.windows(2).zip(marked) {
2567 out.push(pair[0]);
2568 if m {
2569 out.push(f64::midpoint(pair[0], pair[1]));
2570 }
2571 }
2572 out.push(knots[knots.len() - 1]);
2573 out
2574 };
2575 let (next_u, next_v) = (split(&us, &split_u), split(&vs, &split_v));
2576 let grew = next_u.len() > us.len() || next_v.len() > vs.len();
2577 if !grew || next_u.len() > sampling.most + 1 || next_v.len() > sampling.most + 1 {
2578 break;
2579 }
2580 (us, vs) = (next_u, next_v);
2581 }
2582 let Some((mut best, read_densely)) = best else {
2583 ogeom_bail!(Construction, "the surface could not be sampled");
2584 };
2585 if !read_densely {
2586 let (mut split_u, mut split_v) = (vec![false; us.len() - 1], vec![false; vs.len() - 1]);
2587 let along = measured_along_lines(
2588 &mut at,
2589 &best.curve,
2590 (&us, &vs),
2591 sampling.between,
2592 tolerance,
2593 best.error,
2594 (&mut split_u, &mut split_v),
2595 tol,
2596 )?;
2597 best.error = best.error.max(along);
2598 best.met = best.error <= tolerance;
2599 }
2600 Ok(best)
2601}
2602
2603const DENSE_PER_SPAN: usize = 8;
2606
2607fn knot_breaks(knots: &KnotVector, (lo, hi): (f64, f64)) -> Vec<f64> {
2609 let mut breaks: Vec<f64> = knots
2610 .distinct()
2611 .into_iter()
2612 .map(|(k, _)| k)
2613 .filter(|k| (lo..=hi).contains(k))
2614 .collect();
2615 if breaks.first().is_none_or(|k| *k > lo) {
2616 breaks.insert(0, lo);
2617 }
2618 if breaks.last().is_none_or(|k| *k < hi) {
2619 breaks.push(hi);
2620 }
2621 breaks
2622}
2623
2624struct Peak {
2627 height: f64,
2628 at: f64,
2629 reach: (f64, f64),
2630}
2631
2632struct Dense {
2634 widest: (f64, f64),
2636 peaks: Vec<Peak>,
2638 misses: Vec<f64>,
2640}
2641
2642fn climb(
2647 off: &mut dyn FnMut(f64) -> OgeomResult<f64>,
2648 (mut a, mut b): (f64, f64),
2649 mut top: (f64, f64),
2650 lean: bool,
2651) -> OgeomResult<(f64, f64)> {
2652 const STEPS: usize = 40;
2653 const LEAN_STEPS: usize = 30;
2654 let ratio = (5.0_f64.sqrt() - 1.0) / 2.0;
2655 let mut keep = |o: f64, t: f64| {
2656 if o > top.0 {
2657 top = (o, t);
2658 }
2659 };
2660 if lean {
2661 let (mut c, mut d) = (b - (b - a) * ratio, a + (b - a) * ratio);
2662 let (mut oc, mut od) = (off(c)?, off(d)?);
2663 keep(oc, c);
2664 keep(od, d);
2665 for _ in 0..LEAN_STEPS {
2666 if oc > od {
2667 (b, d, od) = (d, c, oc);
2668 c = b - (b - a) * ratio;
2669 oc = off(c)?;
2670 keep(oc, c);
2671 } else {
2672 (a, c, oc) = (c, d, od);
2673 d = a + (b - a) * ratio;
2674 od = off(d)?;
2675 keep(od, d);
2676 }
2677 }
2678 return Ok(top);
2679 }
2680 for _ in 0..STEPS {
2681 let (c, d) = (b - (b - a) * ratio, a + (b - a) * ratio);
2682 let (oc, od) = (off(c)?, off(d)?);
2683 keep(oc, c);
2684 keep(od, d);
2685 if oc > od {
2686 b = d;
2687 } else {
2688 a = c;
2689 }
2690 }
2691 Ok(top)
2692}
2693
2694fn read_densely(
2707 off: &mut dyn FnMut(f64) -> OgeomResult<f64>,
2708 breaks: &[f64],
2709 always: usize,
2710 floor: f64,
2711 over: f64,
2712) -> OgeomResult<Dense> {
2713 const NEAR_PEAK: f64 = 0.8;
2714 let mut ts: Vec<f64> = Vec::with_capacity(breaks.len() * DENSE_PER_SPAN);
2715 for w in breaks.windows(2) {
2716 for k in 0..DENSE_PER_SPAN {
2717 let f = precise(k) / precise(DENSE_PER_SPAN);
2718 ts.push(w[0] + (w[1] - w[0]) * f);
2719 }
2720 }
2721 if let Some(last) = breaks.last() {
2722 ts.push(*last);
2723 }
2724 let readings: Vec<f64> = ts.iter().map(|t| off(*t)).collect::<OgeomResult<_>>()?;
2725 let mut widest = (0.0_f64, ts.first().copied().unwrap_or(0.0));
2726 let mut misses = Vec::new();
2727 for (r, t) in readings.iter().zip(&ts) {
2728 if *r > widest.0 {
2729 widest = (*r, *t);
2730 }
2731 if *r > over {
2732 misses.push(*t);
2733 }
2734 }
2735 let mut standing: Vec<(f64, usize)> = (0..readings.len())
2736 .filter(|&i| {
2737 (i == 0 || readings[i - 1] <= readings[i])
2738 && readings.get(i + 1).is_none_or(|r| *r <= readings[i])
2739 })
2740 .map(|i| (readings[i], i))
2741 .collect();
2742 standing.sort_by(|a, b| b.0.total_cmp(&a.0));
2743 let reference = floor.max(widest.0);
2744 let slope = |k: usize| (readings[k + 1] - readings[k]).abs() / (ts[k + 1] - ts[k]);
2748 let rise = |i: usize| -> f64 {
2749 let gaps = i.saturating_sub(2)..(i + 2).min(ts.len() - 1);
2750 let steepest = gaps.clone().map(slope).fold(0.0_f64, f64::max);
2751 let mut rise = 0.0_f64;
2752 for k in [i.checked_sub(1), Some(i)].into_iter().flatten() {
2753 if k + 1 < ts.len() {
2754 let crest = f64::midpoint(readings[k], readings[k + 1])
2755 + steepest * (ts[k + 1] - ts[k]) / 2.0;
2756 rise = rise.max(crest);
2757 }
2758 }
2759 rise
2760 };
2761 let mut peaks = Vec::new();
2762 for (rank, &(reading, i)) in standing.iter().enumerate() {
2763 if rank >= always && reading.max(rise(i)) < NEAR_PEAK * reference {
2764 continue;
2765 }
2766 let reach = (ts[i.saturating_sub(1)], ts[(i + 1).min(ts.len() - 1)]);
2767 let (height, at) = climb(off, reach, (reading, ts[i]), rank >= always)?;
2768 if height > widest.0 {
2769 widest = (height, at);
2770 }
2771 if height > over {
2772 misses.push(at);
2773 }
2774 peaks.push(Peak { height, at, reach });
2775 }
2776 Ok(Dense {
2777 widest,
2778 peaks,
2779 misses,
2780 })
2781}
2782
2783fn span_of(knots: &[f64], x: f64) -> usize {
2785 knots.partition_point(|k| *k <= x).clamp(1, knots.len() - 1) - 1
2786}
2787
2788#[allow(
2804 clippy::too_many_arguments,
2805 reason = "the sampling, its directions, the spans to split and the miss so far"
2806)]
2807fn measured_along_lines(
2808 point: &mut impl FnMut(f64, f64) -> OgeomResult<Point>,
2809 surface: &crate::BSplineSurface,
2810 (us, vs): (&[f64], &[f64]),
2811 between: (bool, bool),
2812 tolerance: f64,
2813 floor: f64,
2814 (split_u, split_v): (&mut [bool], &mut [bool]),
2815 tol: Tolerances,
2816) -> OgeomResult<f64> {
2817 use crate::traits::Surface as _;
2818 const NEAR_PEAK: f64 = 0.8;
2819 const ACROSS_PEAKS: usize = 16;
2820 const ROW_PEAKS: usize = 8;
2825 const CLIMB_MARGIN: f64 = 0.01;
2826 let mut off = |u: f64, v: f64| -> OgeomResult<f64> {
2828 Ok(match point(u, v) {
2829 Ok(p) => p.distance(surface.point_at(u, v, tol)?),
2830 Err(_) => 0.0,
2831 })
2832 };
2833 let mut error = 0.0_f64;
2834 let mut centres: Vec<(usize, usize)> = Vec::new();
2835 let mut tops: Vec<(Peak, bool, f64, Option<usize>)> = Vec::new();
2839 for along_u in [true, false] {
2840 let (known_along, known_across) = if along_u {
2841 between
2842 } else {
2843 (between.1, between.0)
2844 };
2845 if !known_along {
2846 continue;
2847 }
2848 let (along, across) = if along_u { (us, vs) } else { (vs, us) };
2849 let knots = if along_u {
2850 surface.u_knots()
2851 } else {
2852 surface.v_knots()
2853 };
2854 let breaks = knot_breaks(knots, (along[0], along[along.len() - 1]));
2855 let mut lines: Vec<(f64, Option<usize>)> = across.iter().map(|x| (*x, None)).collect();
2856 if known_across && along_u {
2857 lines.extend(
2858 across
2859 .windows(2)
2860 .enumerate()
2861 .map(|(j, w)| (f64::midpoint(w[0], w[1]), Some(j))),
2862 );
2863 }
2864 for (x, cell) in lines {
2865 let dense = read_densely(
2866 &mut |t| if along_u { off(t, x) } else { off(x, t) },
2867 &breaks,
2868 if along_u { ROW_PEAKS } else { 0 },
2869 floor.max(error),
2870 tolerance,
2871 )?;
2872 error = error.max(dense.widest.0);
2873 for t in dense.misses {
2874 let i = span_of(along, t);
2875 match (cell, along_u) {
2876 (None, true) => split_u[i] = true,
2877 (None, false) => split_v[i] = true,
2878 (Some(j), true) => centres.push((i, j)),
2879 (Some(j), false) => centres.push((j, i)),
2880 }
2881 }
2882 if known_across {
2883 tops.extend(dense.peaks.into_iter().map(|p| (p, along_u, x, cell)));
2884 }
2885 }
2886 }
2887 tops.sort_by(|a, b| b.0.height.total_cmp(&a.0.height));
2888 let near = NEAR_PEAK * floor.max(error);
2889 for (peak, along_u, x, cell) in tops.iter().take(ACROSS_PEAKS) {
2890 if peak.height < near {
2891 break;
2892 }
2893 let (along_u, x, t) = (*along_u, *x, peak.at);
2894 let across = if along_u { vs } else { us };
2895 let reach = match cell {
2898 Some(j) => (across[*j], across[j + 1]),
2899 None => {
2900 let j = across.partition_point(|y| *y < x);
2901 (
2902 f64::midpoint(across[j.saturating_sub(1)], x),
2903 f64::midpoint(x, across[(j + 1).min(across.len() - 1)]),
2904 )
2905 }
2906 };
2907 let (height, y) = climb(
2908 &mut |y| if along_u { off(t, y) } else { off(y, t) },
2909 reach,
2910 (peak.height, x),
2911 true,
2912 )?;
2913 let (height, s) = climb(
2914 &mut |s| if along_u { off(s, y) } else { off(y, s) },
2915 peak.reach,
2916 (height, t),
2917 true,
2918 )?;
2919 let height = height * (1.0 + CLIMB_MARGIN);
2920 error = error.max(height);
2921 if height > tolerance {
2922 let (u, v) = if along_u { (s, y) } else { (y, s) };
2923 centres.push((span_of(us, u), span_of(vs, v)));
2924 }
2925 }
2926 for (i, j) in centres {
2927 if !split_u[i] && !split_v[j] {
2928 split_u[i] = true;
2929 split_v[j] = true;
2930 }
2931 }
2932 Ok(error)
2933}
2934
2935#[allow(clippy::type_complexity, reason = "the error and the spans to split")]
2951fn measured_between(
2952 point: &mut impl FnMut(f64, f64) -> OgeomResult<Point>,
2953 surface: &crate::BSplineSurface,
2954 (us, vs): (&[f64], &[f64]),
2955 between: (bool, bool),
2956 tolerance: f64,
2957 tol: Tolerances,
2958) -> OgeomResult<(f64, Vec<bool>, Vec<bool>)> {
2959 use crate::traits::Surface as _;
2960 const NEAR_PEAK: f64 = 0.8;
2963 const PEAKS: usize = 16;
2964 const CLIMB_MARGIN: f64 = 0.01;
2965 let checks = |knots: &[f64], between: bool| -> Vec<(f64, Option<usize>)> {
2966 let mut out = Vec::with_capacity(knots.len() * 2);
2967 for (i, pair) in knots.windows(2).enumerate() {
2968 out.push((pair[0], None));
2969 if between {
2970 let mid = f64::midpoint(pair[0], pair[1]);
2971 out.push((f64::midpoint(pair[0], mid), Some(i)));
2972 out.push((mid, Some(i)));
2973 out.push((f64::midpoint(mid, pair[1]), Some(i)));
2974 }
2975 }
2976 out.push((knots[knots.len() - 1], None));
2977 out
2978 };
2979 let u_checks = checks(us, between.0);
2980 let v_checks = checks(vs, between.1);
2981 let mut split_u = vec![false; us.len() - 1];
2982 let mut split_v = vec![false; vs.len() - 1];
2983 let mut centres: Vec<(usize, usize)> = Vec::new();
2984 let mut error = 0.0_f64;
2985 let mut readings: Vec<Vec<Option<f64>>> = vec![vec![None; u_checks.len()]; v_checks.len()];
2988 for (row, &(v, v_span)) in v_checks.iter().enumerate() {
2989 for (column, &(u, u_span)) in u_checks.iter().enumerate() {
2990 if u_span.is_none() && v_span.is_none() {
2991 continue;
2992 }
2993 let off = point(u, v)?.distance(surface.point_at(u, v, tol)?);
2994 readings[row][column] = Some(off);
2995 error = error.max(off);
2996 if off > tolerance {
2997 match (u_span, v_span) {
2998 (Some(i), Some(j)) => centres.push((i, j)),
2999 (Some(i), None) => split_u[i] = true,
3000 (None, Some(j)) => split_v[j] = true,
3001 (None, None) => {}
3002 }
3003 }
3004 }
3005 }
3006 let read = |row: usize, column: usize| readings[row][column].unwrap_or(0.0);
3009 let mut peaks: Vec<(f64, usize, usize)> = Vec::new();
3010 for (row, line) in readings.iter().enumerate() {
3011 for (column, reading) in line.iter().enumerate() {
3012 let Some(here) = *reading else {
3013 continue;
3014 };
3015 if here < error * NEAR_PEAK {
3016 continue;
3017 }
3018 let mut top = true;
3019 for (dr, dc) in [
3020 (-1, -1),
3021 (-1, 0),
3022 (-1, 1),
3023 (0, -1),
3024 (0, 1),
3025 (1, -1),
3026 (1, 0),
3027 (1, 1),
3028 ] {
3029 let (Some(r), Some(c)) =
3030 (row.checked_add_signed(dr), column.checked_add_signed(dc))
3031 else {
3032 continue;
3033 };
3034 if r < v_checks.len() && c < u_checks.len() && read(r, c) > here {
3035 top = false;
3036 }
3037 }
3038 if top {
3039 peaks.push((here, row, column));
3040 }
3041 }
3042 }
3043 peaks.sort_by(|a, b| b.0.total_cmp(&a.0));
3044 let ratio = (5.0_f64.sqrt() - 1.0) / 2.0;
3048 let mut off_at = |u: f64, v: f64| -> f64 {
3049 match (point(u, v), surface.point_at(u, v, tol)) {
3050 (Ok(p), Ok(q)) => p.distance(q),
3051 _ => 0.0,
3052 }
3053 };
3054 let reach = |checks: &[(f64, Option<usize>)], k: usize| -> (f64, f64) {
3055 (
3056 checks[k.saturating_sub(1)].0,
3057 checks[(k + 1).min(checks.len() - 1)].0,
3058 )
3059 };
3060 for &(_, row, column) in peaks.iter().take(PEAKS) {
3061 let (mut u, mut v) = (u_checks[column].0, v_checks[row].0);
3062 let mut widest = read(row, column);
3063 let (u_reach, v_reach) = (reach(&u_checks, column), reach(&v_checks, row));
3064 for _ in 0..3 {
3065 for along_u in [true, false] {
3066 if (along_u && !between.0) || (!along_u && !between.1) {
3067 continue;
3068 }
3069 let (mut a, mut b) = if along_u { u_reach } else { v_reach };
3070 let mut at = |x: f64| if along_u { off_at(x, v) } else { off_at(u, x) };
3071 for _ in 0..24 {
3072 let (c, d) = (b - (b - a) * ratio, a + (b - a) * ratio);
3073 if at(c) > at(d) {
3074 b = d;
3075 } else {
3076 a = c;
3077 }
3078 }
3079 let x = f64::midpoint(a, b);
3080 let found = at(x);
3081 if found > widest {
3082 widest = found;
3083 if along_u {
3084 u = x;
3085 } else {
3086 v = x;
3087 }
3088 }
3089 }
3090 }
3091 let widest = widest * (1.0 + CLIMB_MARGIN);
3095 error = error.max(widest);
3096 if widest > tolerance {
3097 let span = |knots: &[f64], x: f64| {
3098 knots.partition_point(|k| *k <= x).clamp(1, knots.len() - 1) - 1
3099 };
3100 let (i, j) = (span(us, u), span(vs, v));
3101 match between {
3102 (true, true) => centres.push((i, j)),
3103 (true, false) => split_u[i] = true,
3104 (false, true) => split_v[j] = true,
3105 (false, false) => {}
3106 }
3107 }
3108 }
3109 for (i, j) in centres {
3110 if !split_u[i] && !split_v[j] {
3111 split_u[i] = true;
3112 split_v[j] = true;
3113 }
3114 }
3115 Ok((error, split_u, split_v))
3116}
3117
3118#[allow(
3120 clippy::too_many_arguments,
3121 reason = "the grid, its two parameterizations and the fit's own settings"
3122)]
3123fn fit_grid_on(
3124 rows: &[Vec<Point>],
3125 raw: &[Vec<[f64; 3]>],
3126 (u_params, v_params): (&[f64], &[f64]),
3127 degree: usize,
3128 tolerance: f64,
3129 (closed_v, interpolate): (bool, bool),
3130 (u_kept, v_kept): (Kept<'_>, Kept<'_>),
3131 tol: Tolerances,
3132) -> OgeomResult<Fitted<crate::BSplineSurface>> {
3133 use crate::traits::Surface as _;
3134 let (u_knots, row_controls) = fit_family::<3>(
3136 raw,
3137 u_params,
3138 degree,
3139 tolerance * 0.5,
3140 false,
3141 interpolate,
3142 u_kept,
3143 false,
3144 )?;
3145 let k = u_knots.control_point_count();
3147 let control_columns: Vec<Vec<[f64; 3]>> = (0..k)
3148 .map(|i| row_controls.iter().map(|r| r[i]).collect())
3149 .collect();
3150 let (v_knots, column_controls) = fit_family::<3>(
3151 &control_columns,
3152 v_params,
3153 degree,
3154 tolerance * 0.5,
3155 closed_v,
3156 interpolate,
3157 v_kept,
3158 false,
3159 )?;
3160 let l = v_knots.control_point_count();
3161
3162 let mut net: Vec<Point> = Vec::with_capacity(k * l);
3165 for column in &column_controls {
3166 for c in column {
3167 net.push(Point::new(c[0], c[1], c[2]));
3168 }
3169 }
3170 let grid = ogeom_math::ControlGrid::new(net, k, l)?;
3171 let surface = crate::BSplineSurface::new(u_knots, v_knots, &grid, tol)?;
3172
3173 let mut worst = 0.0_f64;
3176 for (j, row) in rows.iter().enumerate() {
3177 for (i, p) in row.iter().enumerate() {
3178 let at = surface.point_at(u_params[i], v_params[j], tol)?;
3179 worst = worst.max(at.distance(*p));
3180 }
3181 }
3182 Ok(Fitted {
3183 curve: surface,
3184 error: worst,
3185 met: worst <= tolerance,
3186 })
3187}
3188
3189type FamilyRound<const D: usize> = (KnotVector, Vec<Vec<[f64; D]>>, f64);
3192
3193#[allow(
3201 clippy::too_many_arguments,
3202 reason = "the family, its parameters and the fit's terms"
3203)]
3204fn fit_family<const D: usize>(
3205 family: &[Vec<[f64; D]>],
3206 parameters: &[f64],
3207 degree: usize,
3208 tolerance: f64,
3209 closed: bool,
3210 interpolate: bool,
3211 kept: Kept<'_>,
3212 within: bool,
3213) -> OgeomResult<(KnotVector, Vec<Vec<[f64; D]>>)> {
3214 let degree = degree.min(parameters.len() - 1).max(1);
3215 let (a, b) = (parameters[0], parameters[parameters.len() - 1]);
3216 let mut knots = if interpolate {
3217 interpolating_knots(degree, parameters)?
3218 } else {
3219 let mut knots = single_span(degree, a, b)?;
3220 for &(at, multiplicity) in kept {
3221 if at > a && at < b {
3222 knots = knots.with_knot_inserted(at, multiplicity.min(degree))?;
3223 }
3224 }
3225 knots
3226 };
3227 let kept = !interpolate && kept.iter().any(|&(at, _)| at > a && at < b);
3228 const ROUNDS: usize = 24;
3229 let mut best: Option<FamilyRound<D>> = None;
3230 for _ in 0..ROUNDS {
3231 let mut controls = Vec::with_capacity(family.len());
3232 let mut merged: Vec<(f64, f64)> = parameters.iter().map(|u| (*u, 0.0)).collect();
3233 let mut solvable = true;
3234 for row in family {
3235 match least_squares::<D>(&knots, row, parameters, closed) {
3236 Ok(control) => {
3237 for (slot, entry) in residuals::<D>(&knots, &control, row, parameters)
3238 .iter()
3239 .zip(merged.iter_mut())
3240 {
3241 entry.1 = entry.1.max(slot.1);
3242 }
3243 controls.push(control);
3244 }
3245 Err(_) => {
3246 solvable = false;
3247 break;
3248 }
3249 }
3250 }
3251 if !solvable {
3252 break;
3253 }
3254 let worst = merged.iter().fold(0.0_f64, |acc, e| acc.max(e.1));
3255 if best.as_ref().is_none_or(|(_, _, held)| worst < *held) {
3256 best = Some((knots.clone(), controls, worst));
3257 }
3258 if worst <= tolerance
3259 || knots.control_point_count() >= parameters.len() + usize::from(closed)
3260 {
3261 break;
3262 }
3263 let Some(refined) = refined_where_bad(&knots, &merged, tolerance, within)? else {
3264 break;
3265 };
3266 if kept {
3270 if refined.control_point_count() > parameters.len() {
3271 break;
3272 }
3273 knots = refined;
3274 } else {
3275 knots = refined_or_interpolating(refined, parameters, closed)?;
3276 }
3277 }
3278 let Some((knots, controls, _)) = best else {
3279 ogeom_bail!(NotDone, "the family fit solved no round at all");
3280 };
3281 Ok((knots, controls))
3282}
3283
3284fn refined_or_interpolating(
3293 refined: KnotVector,
3294 parameters: &[f64],
3295 closed: bool,
3296) -> OgeomResult<KnotVector> {
3297 if closed || refined.control_point_count() < parameters.len() {
3298 return Ok(refined);
3299 }
3300 interpolating_knots(refined.degree(), parameters)
3301}
3302
3303fn interpolating_knots(degree: usize, parameters: &[f64]) -> OgeomResult<KnotVector> {
3307 let n = parameters.len();
3308 let (a, b) = (parameters[0], parameters[n - 1]);
3309 let mut knots = vec![a; degree + 1];
3310 for j in 1..n.saturating_sub(degree) {
3311 #[allow(clippy::cast_precision_loss, reason = "a small count")]
3312 let mean = parameters[j..j + degree].iter().sum::<f64>() / degree as f64;
3313 knots.push(mean);
3314 }
3315 knots.extend(std::iter::repeat_n(b, degree + 1));
3316 KnotVector::new(knots, degree)
3317}
3318
3319fn chordal<const D: usize>(points: &[[f64; D]]) -> Vec<f64> {
3326 let mut out = Vec::with_capacity(points.len());
3327 out.push(0.0);
3328 let mut total = 0.0;
3329 for pair in points.windows(2) {
3330 total += distance::<D>(&pair[0], &pair[1]);
3331 out.push(total);
3332 }
3333 if total > 0.0 {
3334 for u in &mut out {
3335 *u /= total;
3336 }
3337 }
3338 if let Some(last) = out.last_mut() {
3339 *last = 1.0;
3340 }
3341 out
3342}
3343
3344fn centripetal<const D: usize>(points: &[[f64; D]]) -> Vec<f64> {
3346 let mut out = Vec::with_capacity(points.len());
3347 out.push(0.0);
3348 let mut total = 0.0;
3349 for pair in points.windows(2) {
3350 total += distance::<D>(&pair[0], &pair[1]).sqrt();
3351 out.push(total);
3352 }
3353 if total > 0.0 {
3354 for u in &mut out {
3355 *u /= total;
3356 }
3357 }
3358 if let Some(last) = out.last_mut() {
3361 *last = 1.0;
3362 }
3363 out
3364}
3365
3366fn single_span(degree: usize, a: f64, b: f64) -> OgeomResult<KnotVector> {
3368 let mut knots = Vec::with_capacity(2 * (degree + 1));
3369 knots.extend(core::iter::repeat_n(a, degree + 1));
3370 knots.extend(core::iter::repeat_n(b, degree + 1));
3371 KnotVector::new(knots, degree)
3372}
3373
3374fn least_squares<const D: usize>(
3380 knots: &KnotVector,
3381 points: &[[f64; D]],
3382 parameters: &[f64],
3383 closed: bool,
3384) -> OgeomResult<Vec<[f64; D]>> {
3385 let n = knots.control_point_count();
3386 let m = points.len();
3387 let degree = knots.degree();
3388
3389 let mut control = vec![[0.0; D]; n];
3390 control[0] = points[0];
3391 control[n - 1] = points[m - 1];
3392 if n <= 2 {
3393 return Ok(control);
3394 }
3395
3396 let t = knots.knots();
3404 let ratio = if closed && n >= 4 {
3405 let d_start = t[degree + 1] - t[1];
3406 let d_end = t[n + degree - 1] - t[n - 1];
3407 (d_start > 0.0 && d_end > 0.0).then(|| d_end / d_start)
3408 } else {
3409 None
3410 };
3411 let eliminated = ratio.map(|_| n - 2);
3412 let unknown_count = match eliminated {
3413 Some(_) => n - 3,
3414 None => n - 2,
3415 };
3416 if unknown_count == 0 {
3417 if let (Some(r), Some(e)) = (ratio, eliminated) {
3418 for d in 0..D {
3422 control[e][d] = r.mul_add(points[0][d], points[m - 1][d]) / (1.0 + r);
3423 }
3424 let _ = r;
3425 }
3426 return Ok(control);
3427 }
3428
3429 let mut normal = NormalMatrix::new(unknown_count, degree, ratio.is_some());
3434 let mut rhs = vec![vec![0.0; unknown_count]; D];
3435
3436 let mut rows: Vec<(usize, Row)> = Vec::with_capacity(m);
3437 for (k, &u) in parameters.iter().enumerate() {
3438 let span = knots.span_unchecked(u);
3439 let basis = knots.basis(span, u);
3440 let first = span - degree;
3441 let mut row: Row = basis
3442 .iter()
3443 .enumerate()
3444 .map(|(j, b)| (first + j, *b))
3445 .collect();
3446 if let (Some(r), Some(e)) = (ratio, eliminated)
3450 && let Some(position) = row.iter().position(|(i, _)| *i == e)
3451 {
3452 let (_, b_e) = row.remove(position);
3453 row.push((usize::MAX, b_e * r));
3454 row.push((n - 1, b_e));
3455 row.push((1, -r * b_e));
3456 }
3457 rows.push((k, row));
3458 }
3459
3460 for (k, row) in &rows {
3461 let mut target = points[*k];
3464 for (index, b) in row {
3465 if *index == 0 || *index == usize::MAX {
3466 for d in 0..D {
3467 target[d] -= b * points[0][d];
3468 }
3469 } else if *index == n - 1 {
3470 for d in 0..D {
3471 target[d] -= b * points[m - 1][d];
3472 }
3473 }
3474 }
3475 let is_known = |i: usize| i == 0 || i == n - 1 || i == usize::MAX;
3476 for (i, bi) in row {
3477 if is_known(*i) {
3478 continue;
3479 }
3480 for (j, bj) in row {
3481 if is_known(*j) {
3482 continue;
3483 }
3484 normal.add(i - 1, j - 1, bi * bj);
3485 }
3486 for d in 0..D {
3487 rhs[d][i - 1] += bi * target[d];
3488 }
3489 }
3490 }
3491
3492 let Some(solved) = normal.solve(&rhs) else {
3495 ogeom_bail!(
3496 NotDone,
3497 "the fitting system is singular: a knot span contains no data"
3498 );
3499 };
3500 for (d, column) in solved.iter().enumerate() {
3501 for i in 0..unknown_count {
3502 control[i + 1][d] = column[i];
3503 }
3504 }
3505 if let (Some(r), Some(e)) = (ratio, eliminated) {
3506 for d in 0..D {
3507 control[e][d] = points[m - 1][d] + r * (points[0][d] - control[1][d]);
3508 }
3509 }
3510 Ok(control)
3511}
3512
3513type Row = smallvec::SmallVec<[(usize, f64); 8]>;
3516
3517enum NormalMatrix {
3521 Banded {
3523 lower: Vec<Vec<f64>>,
3524 band: usize,
3525 },
3526 Dense(nalgebra::DMatrix<f64>),
3527}
3528
3529impl NormalMatrix {
3530 fn new(size: usize, band: usize, dense: bool) -> Self {
3531 if dense {
3532 Self::Dense(nalgebra::DMatrix::zeros(size, size))
3533 } else {
3534 Self::Banded {
3535 lower: vec![vec![0.0; band + 1]; size],
3536 band,
3537 }
3538 }
3539 }
3540
3541 fn add(&mut self, i: usize, j: usize, value: f64) {
3542 match self {
3543 Self::Dense(m) => m[(i, j)] += value,
3544 Self::Banded { lower, band } => {
3545 if j <= i && i - j <= *band {
3547 lower[i][i - j] += value;
3548 }
3549 }
3550 }
3551 }
3552
3553 fn solve(&self, rhs: &[Vec<f64>]) -> Option<Vec<Vec<f64>>> {
3561 match self {
3562 Self::Banded { lower, band } => {
3563 let factor = banded_cholesky(lower, *band)?;
3564 Some(
3565 rhs.iter()
3566 .map(|b| banded_substitute(&factor, *band, b))
3567 .collect(),
3568 )
3569 }
3570 Self::Dense(m) => {
3571 let largest = m.diagonal().iter().fold(0.0_f64, |a, d| a.max(d.abs()));
3572 let factor = m.clone().cholesky()?;
3573 let floor = largest * SINGULAR;
3574 if factor.l_dirty().diagonal().iter().any(|d| d * d <= floor) {
3575 return None;
3576 }
3577 Some(
3578 rhs.iter()
3579 .map(|b| {
3580 factor
3581 .solve(&nalgebra::DVector::from_column_slice(b))
3582 .as_slice()
3583 .to_vec()
3584 })
3585 .collect(),
3586 )
3587 }
3588 }
3589 }
3590}
3591
3592const SINGULAR: f64 = 1e-12;
3595
3596fn banded_cholesky(lower: &[Vec<f64>], band: usize) -> Option<Vec<Vec<f64>>> {
3599 let n = lower.len();
3600 let largest = lower.iter().fold(0.0_f64, |a, row| a.max(row[0].abs()));
3601 let floor = largest * SINGULAR;
3602 let mut factor = vec![vec![0.0; band + 1]; n];
3603 for i in 0..n {
3604 let first = i.saturating_sub(band);
3605 for j in first..=i {
3606 let mut sum = lower[i][i - j];
3607 for k in first.max(j.saturating_sub(band))..j {
3608 sum -= factor[i][i - k] * factor[j][j - k];
3609 }
3610 if i == j {
3611 if sum <= floor || !sum.is_finite() {
3612 return None;
3613 }
3614 factor[i][0] = sum.sqrt();
3615 } else {
3616 factor[i][i - j] = sum / factor[j][0];
3617 }
3618 }
3619 }
3620 Some(factor)
3621}
3622
3623fn banded_substitute(factor: &[Vec<f64>], band: usize, b: &[f64]) -> Vec<f64> {
3625 let n = factor.len();
3626 let mut y = vec![0.0; n];
3627 for i in 0..n {
3628 let mut sum = b[i];
3629 for k in i.saturating_sub(band)..i {
3630 sum -= factor[i][i - k] * y[k];
3631 }
3632 y[i] = sum / factor[i][0];
3633 }
3634 let mut x = vec![0.0; n];
3635 for i in (0..n).rev() {
3636 let mut sum = y[i];
3637 for k in (i + 1)..n.min(i + band + 1) {
3638 sum -= factor[k][k - i] * x[k];
3639 }
3640 x[i] = sum / factor[i][0];
3641 }
3642 x
3643}
3644
3645fn correct_parameters<const D: usize>(
3651 knots: &KnotVector,
3652 control: &[[f64; D]],
3653 points: &[[f64; D]],
3654 parameters: &mut [f64],
3655 looped: bool,
3656) {
3657 let (lo, hi) = knots.domain();
3658 let last = parameters.len() - 1;
3659 #[allow(clippy::cast_precision_loss)]
3667 let max_step = if looped {
3668 (hi - lo) * 8.0 / parameters.len() as f64
3669 } else {
3670 hi - lo
3671 };
3672 for (k, u) in parameters.iter_mut().enumerate() {
3673 if k == 0 || k == last {
3674 continue;
3675 }
3676 let (at, d1, d2) = evaluate::<D>(knots, control, *u);
3677 let gap: [f64; D] = core::array::from_fn(|d| at[d] - points[k][d]);
3678 let dot = |a: &[f64; D], b: &[f64; D]| a.iter().zip(b).map(|(x, y)| x * y).sum::<f64>();
3679 let numerator = dot(&gap, &d1);
3680 let denominator = dot(&d1, &d1) + dot(&gap, &d2);
3681 if denominator.abs() <= f64::MIN_POSITIVE {
3682 continue;
3683 }
3684 let stepped = *u - (numerator / denominator).clamp(-max_step, max_step);
3685 if stepped.is_finite() {
3686 *u = stepped.clamp(lo, hi);
3687 }
3688 }
3689 for k in 1..parameters.len() {
3692 if parameters[k] < parameters[k - 1] {
3693 parameters[k] = parameters[k - 1];
3694 }
3695 }
3696}
3697
3698fn evaluate<const D: usize>(
3700 knots: &KnotVector,
3701 control: &[[f64; D]],
3702 u: f64,
3703) -> ([f64; D], [f64; D], [f64; D]) {
3704 let degree = knots.degree();
3705 let span = knots.span_unchecked(u);
3706 let table = knots.basis_derivatives(span, u, 2);
3707 let first = span - degree;
3708 let mut out = [[0.0; D]; 3];
3709 for (order, row) in table.iter().enumerate().take(3) {
3710 for (j, b) in row.iter().enumerate() {
3711 for d in 0..D {
3712 out[order][d] += b * control[first + j][d];
3713 }
3714 }
3715 }
3716 (out[0], out[1], out[2])
3717}
3718
3719fn residuals<const D: usize>(
3721 knots: &KnotVector,
3722 control: &[[f64; D]],
3723 points: &[[f64; D]],
3724 parameters: &[f64],
3725) -> Vec<(f64, f64)> {
3726 parameters
3727 .iter()
3728 .zip(points)
3729 .map(|(&u, p)| (u, distance::<D>(&position::<D>(knots, control, u), p)))
3730 .collect()
3731}
3732
3733fn position<const D: usize>(knots: &KnotVector, control: &[[f64; D]], u: f64) -> [f64; D] {
3735 let span = knots.span_unchecked(u);
3736 let basis = knots.basis(span, u);
3737 let first = span - knots.degree();
3738 let mut at = [0.0; D];
3739 for (j, b) in basis.iter().enumerate() {
3740 for d in 0..D {
3741 at[d] += b * control[first + j][d];
3742 }
3743 }
3744 at
3745}
3746
3747fn wandering<const D: usize>(
3756 knots: &KnotVector,
3757 control: &[[f64; D]],
3758 points: &[[f64; D]],
3759 parameters: &[f64],
3760 errors: &mut [(f64, f64)],
3761) {
3762 for k in 0..parameters.len().saturating_sub(1) {
3763 let middle = f64::midpoint(parameters[k], parameters[k + 1]);
3764 let (at, _, _) = evaluate::<D>(knots, control, middle);
3765 let chord: [f64; D] =
3766 core::array::from_fn(|d| f64::midpoint(points[k][d], points[k + 1][d]));
3767 let excess = distance::<D>(&at, &chord) - distance::<D>(&points[k], &points[k + 1]) / 2.0;
3768 if excess > 0.0 {
3769 errors[k].1 = errors[k].1.max(excess);
3770 errors[k + 1].1 = errors[k + 1].1.max(excess);
3771 }
3772 }
3773}
3774
3775fn refined_where_bad(
3793 knots: &KnotVector,
3794 errors: &[(f64, f64)],
3795 tolerance: f64,
3796 within: bool,
3797) -> OgeomResult<Option<KnotVector>> {
3798 let distinct = knots.distinct();
3799 let mut refined = knots.clone();
3800 let mut changed = false;
3801 let ordered = errors.windows(2).all(|w| w[0].0 < w[1].0);
3802 for window in distinct.windows(2) {
3803 let (lo, hi) = (window[0].0, window[1].0);
3804 let inside: std::borrow::Cow<'_, [(f64, f64)]> = if ordered {
3805 let from = errors.partition_point(|(u, _)| *u < lo);
3806 let to = errors.partition_point(|(u, _)| *u < hi);
3807 std::borrow::Cow::Borrowed(&errors[from..to])
3808 } else {
3809 errors
3810 .iter()
3811 .filter(|(u, _)| *u >= lo && *u < hi)
3812 .copied()
3813 .collect()
3814 };
3815 let bad = inside.iter().any(|(_, e)| *e > tolerance);
3816 let inside = if within {
3817 std::borrow::Cow::Owned(inside.iter().filter(|(u, _)| *u > lo).copied().collect())
3818 } else {
3819 inside
3820 };
3821 if !bad || inside.len() < 2 {
3822 continue;
3823 }
3824 let at = f64::midpoint(inside[inside.len() / 2 - 1].0, inside[inside.len() / 2].0)
3826 .clamp(lo + (hi - lo) * 1e-6, hi - (hi - lo) * 1e-6);
3827 let left = inside.iter().any(|(u, _)| *u < at);
3829 let right = inside.iter().any(|(u, _)| *u >= at);
3830 if left && right {
3831 refined = refined.with_knot_inserted(at, 1)?;
3832 changed = true;
3833 }
3834 }
3835 Ok(if changed { Some(refined) } else { None })
3836}
3837
3838#[cfg(test)]
3839#[allow(clippy::unwrap_used)]
3840mod grid_tests {
3841 use super::*;
3842 use crate::traits::Surface as _;
3843 use ogeom_core::Tolerances;
3844
3845 const T: Tolerances = Tolerances::millimetres();
3846
3847 #[test]
3853 fn a_sampled_loop_fit_states_no_less_than_its_widest_miss() {
3854 use crate::traits::Curve3d as _;
3855 let square = |g: f64| -> Point {
3856 let s = g.rem_euclid(1.0) * 4.0;
3857 let r = s - s.floor();
3858 let (x, y) = match s.floor() {
3859 0.0 => (1.0, 2.0f64.mul_add(r, -1.0)),
3860 1.0 => (2.0f64.mul_add(-r, 1.0), 1.0),
3861 2.0 => (-1.0, 2.0f64.mul_add(-r, 1.0)),
3862 _ => (2.0f64.mul_add(r, -1.0), -1.0),
3863 };
3864 Point::new(x, y, 0.0)
3865 };
3866 let blend = |g: f64| {
3867 let (sin, cos) = g.mul_add(core::f64::consts::TAU, 0.3).sin_cos();
3868 let (c, q) = (Point::new(1.2 * cos, 1.2 * sin, 0.0), square(g));
3869 c + (q - c) * 0.9
3870 };
3871 let ts: Vec<f64> = (0..=64).map(|i| f64::from(i) / 64.0).collect();
3872 let fitted = fit_curve_sampled(|g| Ok(blend(g)), &ts, true, 3, 1e-4, T).unwrap();
3873 assert!(fitted.met);
3874 let n = 200_000;
3875 let mut worst = 0.0_f64;
3876 for i in 0..=n {
3877 let g = f64::from(i) / f64::from(n);
3878 worst = worst.max(blend(g).distance(fitted.curve.point_at(g, T).unwrap()));
3879 }
3880 assert!(
3881 worst <= fitted.error,
3882 "the fit misses by {worst:.4e} and states {:.4e}",
3883 fitted.error
3884 );
3885 }
3886
3887 #[test]
3892 fn a_sampled_curve_fit_states_no_less_than_its_miss_at_a_corner() {
3893 use crate::traits::Curve3d as _;
3894 let n = 25;
3895 let corners: Vec<Point> = (0..n)
3896 .map(|k| {
3897 let k = f64::from(k);
3898 let (sin, cos) = (k / f64::from(n) * core::f64::consts::TAU).sin_cos();
3899 let r = 3.0f64.mul_add((5.0 * k).sin(), 10.0);
3900 Point::new(r * cos, r * sin, (3.0 * k).cos())
3901 })
3902 .collect();
3903 let mut at_corner = vec![0.0_f64];
3904 for (k, c) in corners.iter().enumerate() {
3905 at_corner.push(at_corner[k] + c.distance(corners[(k + 1) % corners.len()]));
3906 }
3907 let round = at_corner[corners.len()];
3908 let polygon = |g: f64| -> Point {
3909 let s = round * g;
3910 let k = at_corner[1..]
3911 .partition_point(|l| *l < s)
3912 .min(corners.len() - 1);
3913 let f = (s - at_corner[k]) / (at_corner[k + 1] - at_corner[k]);
3914 let (a, b) = (corners[k], corners[(k + 1) % corners.len()]);
3915 a + (b - a) * f
3916 };
3917 let ts: Vec<f64> = (0..=16).map(|i| f64::from(i) / 16.0).collect();
3918 let fitted = fit_curve_sampled(|g| Ok(polygon(g)), &ts, true, 3, 1e-3, T).unwrap();
3919 assert!(fitted.met, "{}", fitted.error);
3920 let worst = at_corner[..corners.len()]
3921 .iter()
3922 .map(|l| {
3923 let g = l / round;
3924 polygon(g).distance(fitted.curve.point_at(g, T).unwrap())
3925 })
3926 .fold(0.0_f64, f64::max);
3927 assert!(
3928 worst <= fitted.error,
3929 "the fit misses by {worst:.4e} at a corner and states {:.4e}",
3930 fitted.error
3931 );
3932 }
3933
3934 #[test]
3940 fn a_sampled_surface_fit_states_no_less_than_its_miss_at_a_crease() {
3941 use crate::traits::Surface as _;
3942 let corners: [[f64; 2]; 3] = [[7.9, 8.2], [-11.2, 9.2], [-2.3, -10.0]];
3943 let mut at_corner = [0.0_f64; 4];
3944 for k in 0..3 {
3945 let (a, b) = (corners[k], corners[(k + 1) % 3]);
3946 at_corner[k + 1] = at_corner[k] + (a[0] - b[0]).hypot(a[1] - b[1]);
3947 }
3948 let round = at_corner[3];
3949 let twisted = |u: f64, v: f64| -> OgeomResult<Point> {
3950 let s = round * u;
3951 let k = at_corner[1..].partition_point(|l| *l < s).min(2);
3952 let f = (s - at_corner[k]) / (at_corner[k + 1] - at_corner[k]);
3953 let (a, b) = (corners[k], corners[(k + 1) % 3]);
3954 let (x, y) = (a[0] + (b[0] - a[0]) * f, a[1] + (b[1] - a[1]) * f);
3955 let (sin, cos) = (1.8 * v).sin_cos();
3956 Ok(Point::new(x * cos - y * sin, x * sin + y * cos, 31.0 * v))
3957 };
3958 let tolerance = 1e-3;
3959 let fitted = fit_surface_sampled(
3960 twisted,
3961 &Sampling::even(((0.0, 1.0), (0.0, 1.0)), 8),
3962 3,
3963 tolerance,
3964 T,
3965 )
3966 .unwrap();
3967 assert!(fitted.met, "{}", fitted.error);
3968 let mut worst = 0.0_f64;
3969 for corner in &at_corner[..3] {
3970 let u = corner / round;
3971 for j in 0..=1024 {
3972 let v = f64::from(j) / 1024.0;
3973 let off = twisted(u, v)
3974 .unwrap()
3975 .distance(fitted.curve.point_at(u, v, T).unwrap());
3976 worst = worst.max(off);
3977 }
3978 }
3979 assert!(
3980 worst <= fitted.error,
3981 "the fit misses by {worst:.4e} at a corner and states {:.4e}",
3982 fitted.error
3983 );
3984 }
3985
3986 #[test]
3987 fn a_torus_patch_grid_fits_to_tolerance_on_and_off_the_grid() {
3988 let torus = ogeom_math::Torus::new(ogeom_math::Frame::WORLD, 2.0, 0.5, T).unwrap();
3989 let surface = crate::TorusSurface::new(torus);
3990 let (nu, nv) = (25, 17);
3991 let span_u = 1.2_f64;
3992 let span_v = 0.9_f64;
3993 let sample = |fu: f64, fv: f64| surface.point_at(span_u * fu, span_v * fv, T).unwrap();
3994 let mut rows = Vec::new();
3995 for j in 0..nv {
3996 let mut row = Vec::new();
3997 for i in 0..nu {
3998 row.push(sample(
3999 f64::from(i) / f64::from(nu - 1),
4000 f64::from(j) / f64::from(nv - 1),
4001 ));
4002 }
4003 rows.push(row);
4004 }
4005 let fitted = fit_surface_grid(&rows, 3, 1e-4, T).unwrap();
4006 assert!(fitted.met, "error {} above the target", fitted.error);
4007
4008 let (ud, vd) = fitted.curve.domain();
4012 for i in 0..8 {
4013 for j in 0..8 {
4014 let u = ud.0 + (ud.1 - ud.0) * (0.07 + 0.9 * f64::from(i) / 7.0);
4015 let v = vd.0 + (vd.1 - vd.0) * (0.07 + 0.9 * f64::from(j) / 7.0);
4016 let p = fitted.curve.point_at(u, v, T).unwrap();
4017 let d = torus.distance_to(p);
4018 assert!(d < 5e-4, "off-grid deviation {d} at ({u}, {v})");
4019 }
4020 }
4021 }
4022
4023 #[test]
4024 fn a_grid_fitted_at_its_own_parameters_keeps_the_chart() {
4025 let torus = ogeom_math::Torus::new(ogeom_math::Frame::WORLD, 2.0, 0.5, T).unwrap();
4029 let surface = crate::TorusSurface::new(torus);
4030 let us: Vec<f64> = (0..21)
4031 .map(|i| 0.3 + 1.2 * (f64::from(i) / 20.0).powi(2))
4032 .collect();
4033 let vs: Vec<f64> = (0..17).map(|j| -0.4 + 0.9 * f64::from(j) / 16.0).collect();
4034 let rows: Vec<Vec<Point>> = vs
4035 .iter()
4036 .map(|v| {
4037 us.iter()
4038 .map(|u| surface.point_at(*u, *v, T).unwrap())
4039 .collect()
4040 })
4041 .collect();
4042 let fitted = fit_surface_grid_at(&us, &vs, &rows, 3, 1e-5, T).unwrap();
4043 assert!(fitted.met, "error {} above the target", fitted.error);
4044 let ((u0, u1), (v0, v1)) = fitted.curve.domain();
4045 assert!((u0 - 0.3).abs() < 1e-12 && (u1 - 1.5).abs() < 1e-12);
4046 assert!((v0 + 0.4).abs() < 1e-12 && (v1 - 0.5).abs() < 1e-12);
4047 for i in 0..9 {
4048 for j in 0..9 {
4049 let u = 0.3 + 1.2 * (0.05 + 0.9 * f64::from(i) / 8.0);
4050 let v = -0.4 + 0.9 * (0.05 + 0.9 * f64::from(j) / 8.0);
4051 let gap = fitted
4052 .curve
4053 .point_at(u, v, T)
4054 .unwrap()
4055 .distance(surface.point_at(u, v, T).unwrap());
4056 assert!(gap < 1e-4, "chart drifts by {gap} at ({u}, {v})");
4057 }
4058 }
4059 assert!(fit_surface_grid_at(&vs, &us, &rows, 3, 1e-5, T).is_err());
4060 }
4061
4062 #[test]
4063 fn a_grid_the_basis_can_represent_fits_to_rounding() {
4064 let corner = |x: f64, y: f64| Point::new(x, y, 0.3 * x - 0.2 * y);
4066 let mut rows = Vec::new();
4067 for j in 0..6 {
4068 let mut row = Vec::new();
4069 for i in 0..6 {
4070 row.push(corner(f64::from(i), 2.0 * f64::from(j)));
4071 }
4072 rows.push(row);
4073 }
4074 let fitted = fit_surface_grid(&rows, 1, 1e-9, T).unwrap();
4075 assert!(fitted.met, "error {} above rounding", fitted.error);
4076 }
4077
4078 fn height_gap(surface: &crate::BSplineSurface, height: impl Fn(f64, f64) -> f64) -> f64 {
4082 let ((u0, u1), (v0, v1)) = surface.domain();
4083 let mut worst = 0.0_f64;
4084 for i in 0..=100 {
4085 for j in 0..=100 {
4086 let u = (u1 - u0).mul_add(f64::from(i) / 100.0, u0);
4087 let v = (v1 - v0).mul_add(f64::from(j) / 100.0, v0);
4088 let p = surface.point_at(u, v, T).unwrap();
4089 worst = worst.max((p.z - height(p.x, p.y)).abs());
4090 }
4091 }
4092 worst
4093 }
4094
4095 fn graph_rows(xs: &[f64], ys: &[f64], height: impl Fn(f64, f64) -> f64) -> Vec<Vec<Point>> {
4097 ys.iter()
4098 .map(|&y| xs.iter().map(|&x| Point::new(x, y, height(x, y))).collect())
4099 .collect()
4100 }
4101
4102 #[test]
4109 fn a_saddle_grid_fit_follows_the_saddle_between_its_points() {
4110 let saddle = |x: f64, y: f64| x.mul_add(x, -y * y) / 20.0;
4111 let side: Vec<f64> = (0..=10).map(|i| f64::from(2 * i - 10)).collect();
4112 let rows = graph_rows(&side, &side, saddle);
4113 for fit in [fit_surface_grid, fit_surface_grid_chordal] {
4114 let fitted = fit(&rows, 3, 1e-6, T).unwrap();
4115 assert!(fitted.met, "error {} at the points", fitted.error);
4116 let gap = height_gap(&fitted.curve, saddle);
4117 assert!(gap < 2e-3, "the fit leaves the saddle by {gap:.3e}");
4118 }
4119 }
4120
4121 #[test]
4127 fn a_polynomial_the_degree_holds_is_reproduced_between_the_samples() {
4128 let even: Vec<f64> = (0..=10).map(|i| f64::from(i) / 5.0 - 1.0).collect();
4129 let uneven: Vec<f64> = (0..=10)
4130 .map(|i| 2.0 * (f64::from(i) / 10.0).powf(1.6) - 1.0)
4131 .collect();
4132 let saddle = |x: f64, y: f64| x.mul_add(x, -y * y) / 20.0;
4133 let cubic = |x: f64, y: f64| (x * x).mul_add(x, -2.0 * x * y * y) + 0.5 * y;
4134 for (xs, ys) in [(&even, &even), (&uneven, &even), (&even, &uneven)] {
4135 for (degree, height) in [
4136 (2, &saddle as &dyn Fn(f64, f64) -> f64),
4137 (3, &saddle),
4138 (3, &cubic),
4139 ] {
4140 let rows = graph_rows(xs, ys, height);
4141 let fitted = fit_surface_grid_at(xs, ys, &rows, degree, 1e-9, T).unwrap();
4142 assert!(fitted.met, "error {} at the points", fitted.error);
4143 let gap = height_gap(&fitted.curve, height);
4144 assert!(gap < 1e-9, "degree {degree} misses by {gap:.3e}");
4145 }
4146 }
4147 let ruled = |x: f64, y: f64| x * y / 5.0;
4148 let rows = graph_rows(&even, &even, ruled);
4149 let fitted = fit_surface_grid(&rows, 3, 1e-9, T).unwrap();
4150 let gap = height_gap(&fitted.curve, ruled);
4151 assert!(gap < 1e-9, "the ruled saddle misses by {gap:.3e}");
4152 }
4153
4154 #[test]
4155 fn a_ragged_grid_is_refused() {
4156 let rows = vec![
4157 vec![Point::ORIGIN, Point::new(1.0, 0.0, 0.0)],
4158 vec![Point::new(0.0, 1.0, 0.0)],
4159 ];
4160 assert!(fit_surface_grid(&rows, 2, 1e-6, T).is_err());
4161 }
4162}
4163
4164#[cfg(test)]
4165#[allow(clippy::unwrap_used)]
4166mod tests {
4167
4168 #[test]
4169 fn a_v_closed_grid_fits_a_ring_with_a_smooth_join() {
4170 use crate::traits::Surface as _;
4171 let tau = core::f64::consts::TAU;
4174 let (major, minor) = (5.0_f64, 1.5_f64);
4175 let rows: Vec<Vec<Point>> = (0..=24)
4176 .map(|j| {
4177 let a = tau * f64::from(j) / 24.0;
4178 (0..=16)
4179 .map(|i| {
4180 let b = tau * f64::from(i) / 16.0;
4181 let r = minor.mul_add(b.cos(), major);
4182 Point::new(r * a.cos(), r * a.sin(), minor * b.sin())
4183 })
4184 .collect()
4185 })
4186 .collect();
4187 let fitted = fit_surface_grid_closed_v(&rows, 3, 5e-3, T).unwrap();
4188 assert!(fitted.met, "the ring fit should meet its tolerance");
4189 let surface = fitted.curve;
4190
4191 let grid = surface.grid();
4194 let (k, l) = (grid.u_count(), grid.v_count());
4195 for i in 0..k {
4196 let first = grid.points()[i * l].point();
4197 let last = grid.points()[i * l + (l - 1)].point();
4198 assert!(
4199 first.distance(last) < 1e-12,
4200 "border control rows differ at u-index {i}"
4201 );
4202 }
4203
4204 let (u_dom, v_dom) = surface.domain();
4206 for i in 0..5 {
4207 let u = u_dom.0 + (u_dom.1 - u_dom.0) * f64::from(i) / 4.0;
4208 let (_, dv0) = surface.d1_at(u, v_dom.0, T).unwrap();
4209 let (_, dv1) = surface.d1_at(u, v_dom.1, T).unwrap();
4210 let gap = (dv0 / dv0.magnitude() - dv1 / dv1.magnitude()).magnitude();
4211 assert!(gap < 1e-6, "the join kinks at u = {u}: {gap}");
4212 }
4213 }
4214
4215 #[test]
4216 fn a_grid_that_is_not_a_loop_is_refused_by_the_closed_entry() {
4217 let rows: Vec<Vec<Point>> = (0..4)
4218 .map(|j| {
4219 (0..4)
4220 .map(|i| Point::new(f64::from(i), f64::from(j), 0.0))
4221 .collect()
4222 })
4223 .collect();
4224 assert!(fit_surface_grid_closed_v(&rows, 3, 1e-3, T).is_err());
4225 }
4226
4227 use super::*;
4228 use crate::traits::{Curve2d as _, Curve3d as _};
4229 use core::f64::consts::TAU;
4230
4231 #[test]
4235 fn a_curve_straying_between_its_samples_is_charged_for_it() {
4236 let knots = KnotVector::new(vec![0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0], 3).unwrap();
4237 let control = [[0.0, 0.0], [0.0, 10.0], [1.0, 10.0], [1.0, 0.0]];
4238 let points = [[0.0, 0.0], [1.0, 0.0]];
4239 let parameters = [0.0, 1.0];
4240 let mut errors = residuals::<2>(&knots, &control, &points, ¶meters);
4241 assert!(errors.iter().all(|e| e.1 < 1e-12));
4242 wandering::<2>(&knots, &control, &points, ¶meters, &mut errors);
4243 for e in &errors {
4244 assert!((e.1 - 7.0).abs() < 1e-12, "{e:?}");
4245 }
4246 }
4247
4248 #[test]
4255 fn a_spline_through_every_point_follows_the_curve_between_them() {
4256 let parabola: Vec<Point> = (0..=10)
4257 .map(|i| {
4258 let x = f64::from(i) - 5.0;
4259 Point::new(x, x * x / 20.0, 0.0)
4260 })
4261 .collect();
4262 let fitted = fit_points(¶bola, 3, 1e-9, T).unwrap();
4263 assert!(fitted.met, "error {}", fitted.error);
4264 let (a, b) = fitted.curve.domain();
4265 let mut worst = 0.0_f64;
4266 for i in 0..=2000 {
4267 let p = fitted
4268 .curve
4269 .point_at((b - a).mul_add(f64::from(i) / 2000.0, a), T)
4270 .unwrap();
4271 worst = worst.max((p.y - p.x * p.x / 20.0).abs());
4272 }
4273 assert!(worst < 5e-4, "the fit leaves the parabola by {worst:.3e}");
4274
4275 let ts: Vec<f64> = (0..=10).map(|i| f64::from(i) / 10.0).collect();
4276 let sine = |t: f64| Point::new(t, (2.0 * t).sin(), 0.0);
4277 let points: Vec<Point> = ts.iter().map(|t| sine(*t)).collect();
4278 let fitted = fit_points_at(&ts, &points, 3, 1e-12, T).unwrap();
4279 assert!(fitted.met, "error {}", fitted.error);
4280 let mut worst = 0.0_f64;
4281 for i in 0..=2000 {
4282 let t = f64::from(i) / 2000.0;
4283 worst = worst.max(fitted.curve.point_at(t, T).unwrap().distance(sine(t)));
4284 }
4285 assert!(worst < 5e-4, "the fit leaves the sine by {worst:.3e}");
4286
4287 let chart: Vec<Point2> = points.iter().map(|p| Point2::new(p.x, p.y)).collect();
4288 let fitted = fit_points_2d_at(&ts, &chart, 3, 1e-12, T).unwrap();
4289 assert!(fitted.met, "error {}", fitted.error);
4290 let mut worst = 0.0_f64;
4291 for i in 0..=2000 {
4292 let t = f64::from(i) / 2000.0;
4293 let p = crate::Curve2d::point_at(&fitted.curve, t, T).unwrap();
4294 let on = sine(t);
4295 worst = worst.max(p.distance(Point2::new(on.x, on.y)));
4296 }
4297 assert!(worst < 5e-4, "the chart fit leaves the sine by {worst:.3e}");
4298 }
4299
4300 #[test]
4301 fn scattered_points_fit_without_a_grid() {
4302 let mut state = 12345u64;
4306 let mut next = || {
4307 state = state
4308 .wrapping_mul(6364136223846793005)
4309 .wrapping_add(1442695040888963407);
4310 {
4311 #[allow(clippy::cast_precision_loss, reason = "31 bits fit exactly")]
4312 let r = (state >> 33) as f64;
4313 r / f64::from(u32::MAX) * 2.0 - 1.0
4314 }
4315 };
4316 let points: Vec<Point> = (0..200)
4317 .map(|_| {
4318 let (x, y) = (next() * 5.0, next() * 5.0);
4319 Point::new(x, y, 0.2 * (x * x + y * y))
4320 })
4321 .collect();
4322 let fitted = fit_surface_scattered(&points, 3, (8, 8), 1e-3, T).unwrap();
4323 assert!(
4324 fitted.error < 0.05,
4325 "the paraboloid fits to {}",
4326 fitted.error
4327 );
4328 }
4329
4330 #[test]
4335 fn scattered_points_at_their_parameters_fit_over_the_given_knots() {
4336 use crate::traits::Surface as _;
4337 let u_knots =
4338 KnotVector::new(vec![0.0, 0.0, 0.0, 0.0, 0.3, 0.6, 1.0, 1.0, 1.0, 1.0], 3).unwrap();
4339 let v_knots = KnotVector::clamped_uniform(3, 5).unwrap();
4340 let (nu, nv) = (6, 5);
4341 let mut net = Vec::new();
4342 for i in 0..nu {
4343 for j in 0..nv {
4344 let (x, y) = (precise(i), precise(j));
4345 net.push(Point::new(x, y, (x * 0.7).sin() + 0.1 * x * y));
4346 }
4347 }
4348 let grid = ogeom_math::ControlGrid::new(net, nu, nv).unwrap();
4349 let exact = crate::BSplineSurface::new(u_knots.clone(), v_knots.clone(), &grid, T).unwrap();
4350 let mut parameters = Vec::new();
4351 let mut points = Vec::new();
4352 for i in 0..=20 {
4353 for j in 0..=13 {
4354 let (u, v) = (precise(i) / 20.0, precise(j) / 13.0);
4355 parameters.push((u, v));
4356 points.push(exact.point_at(u, v, T).unwrap());
4357 }
4358 }
4359 let fitted =
4360 fit_surface_scattered_at(&points, ¶meters, &u_knots, &v_knots, 0.0, T).unwrap();
4361 assert!(fitted.error < 1e-9, "{}", fitted.error);
4362 let (v_wide, u_narrow) = (u_knots.clone(), v_knots.clone());
4364 let swapped: Vec<(f64, f64)> = parameters.iter().map(|&(u, v)| (v, u)).collect();
4365 let turned =
4366 fit_surface_scattered_at(&points, &swapped, &u_narrow, &v_wide, 0.0, T).unwrap();
4367 assert!(turned.error < 1e-9, "{}", turned.error);
4368
4369 let corner: Vec<(f64, f64)> = (0..=8)
4372 .flat_map(|i| (0..=8).map(move |j| (precise(i) / 20.0, precise(j) / 20.0)))
4373 .collect();
4374 let plane: Vec<Point> = corner
4375 .iter()
4376 .map(|&(u, v)| Point::new(u, v, 0.5 * u - 0.25 * v))
4377 .collect();
4378 let fitted =
4379 fit_surface_scattered_at(&plane, &corner, &u_knots, &v_knots, 1e-6, T).unwrap();
4380 assert!(fitted.error < 1e-6, "{}", fitted.error);
4381 let far = fitted.curve.point_at(1.0, 1.0, T).unwrap();
4382 assert!((far.z - 0.25).abs() < 1e-3, "{far:?}");
4383 assert!(
4384 fit_surface_scattered_at(&plane, &corner[1..], &u_knots, &v_knots, 0.0, T).is_err()
4385 );
4386 }
4387
4388 #[test]
4389 fn fairing_trades_closeness_for_straightness_and_says_the_price() {
4390 let points: Vec<Point> = (0..15)
4393 .map(|i| {
4394 let t = f64::from(i);
4395 let kick = if i % 2 == 0 { 0.1 } else { -0.1 };
4396 Point::new(t, t + kick, 0.0)
4397 })
4398 .collect();
4399 let chased = fit_points_faired(&points, 3, 10, 0.0, T).unwrap();
4400 let faired = fit_points_faired(&points, 3, 10, 50.0, T).unwrap();
4401
4402 let bend = |c: &BSplineCurve| -> f64 {
4404 let pts = c.control_points();
4405 (1..pts.len() - 1)
4406 .map(|i| {
4407 let p0 = pts[i - 1].point();
4408 let p1 = pts[i].point();
4409 let p2 = pts[i + 1].point();
4410 ((p2 - p1) - (p1 - p0)).magnitude().powi(2)
4411 })
4412 .sum()
4413 };
4414 assert!(
4415 bend(&faired.curve) < bend(&chased.curve) / 4.0,
4416 "fairing must straighten: {} vs {}",
4417 bend(&faired.curve),
4418 bend(&chased.curve)
4419 );
4420 assert!(faired.error >= chased.error);
4424 use crate::traits::Curve3d as _;
4425 let (lo, hi) = faired.curve.domain();
4426 for i in 0..=32 {
4427 let u = lo + (hi - lo) * f64::from(i) / 32.0;
4428 let p = faired.curve.point_at(u, T).unwrap();
4429 let off_line = (p.y - p.x).abs() / core::f64::consts::SQRT_2;
4430 assert!(
4431 off_line < 0.12,
4432 "the batten stays in the noise band: {off_line}"
4433 );
4434 }
4435 assert!(faired.curve.point_at(lo, T).unwrap().distance(points[0]) < 1e-9);
4437 assert!(faired.curve.point_at(hi, T).unwrap().distance(points[14]) < 1e-9);
4438 }
4439
4440 const T: Tolerances = Tolerances::millimetres();
4441
4442 fn nearest(curve: &BSplineCurve, p: Point) -> f64 {
4444 let at = |u: f64| curve.point_at(u, T).map_or(f64::MAX, |q| p.distance(q));
4445 let mut best = (0.0, f64::MAX);
4446 for i in 0..=4000 {
4447 #[allow(clippy::cast_precision_loss)]
4448 let u = i as f64 / 4000.0;
4449 let d = at(u);
4450 if d < best.1 {
4451 best = (u, d);
4452 }
4453 }
4454 let (mut lo, mut hi) = ((best.0 - 5e-4).max(0.0), (best.0 + 5e-4).min(1.0));
4455 for _ in 0..100 {
4456 let one = lo + (hi - lo) / 3.0;
4457 let two = hi - (hi - lo) / 3.0;
4458 if at(one) < at(two) {
4459 hi = two;
4460 } else {
4461 lo = one;
4462 }
4463 }
4464 at(f64::midpoint(lo, hi)).min(best.1)
4465 }
4466
4467 fn circle_points(n: usize, radius: f64) -> Vec<Point> {
4470 (0..=n)
4471 .map(|i| {
4472 #[allow(clippy::cast_precision_loss)]
4473 let a = TAU * i as f64 / n as f64;
4474 Point::new(radius * a.cos(), radius * a.sin(), 0.0)
4475 })
4476 .collect()
4477 }
4478
4479 #[test]
4480 fn a_fit_meets_the_tolerance_it_was_asked_for_and_says_what_it_reached() {
4481 let points = circle_points(200, 5.0);
4482 for tolerance in [1e-2, 1e-4, 1e-6] {
4483 let fitted = fit_points(&points, 3, tolerance, T).unwrap();
4484 assert!(
4485 fitted.met,
4486 "target {tolerance:e} not met, got {:e}",
4487 fitted.error
4488 );
4489 assert!(
4490 fitted.error <= tolerance,
4491 "reported {:e} over target {tolerance:e}",
4492 fitted.error
4493 );
4494 let mut worst = 0.0_f64;
4500 for p in &points {
4501 worst = worst.max(nearest(&fitted.curve, *p));
4502 }
4503 assert!(
4504 worst <= tolerance * 1.5,
4505 "independent measurement found {worst:e} against {tolerance:e}"
4506 );
4507 }
4508 }
4509
4510 #[test]
4511 fn a_tighter_tolerance_never_uses_fewer_control_points() {
4512 let points = circle_points(300, 3.0);
4513 let coarse = fit_points(&points, 3, 1e-2, T).unwrap();
4514 let fine = fit_points(&points, 3, 1e-6, T).unwrap();
4515 assert!(
4516 fine.curve.control_points().len() > coarse.curve.control_points().len(),
4517 "{} then {}",
4518 coarse.curve.control_points().len(),
4519 fine.curve.control_points().len()
4520 );
4521 assert!(coarse.curve.control_points().len() < 30);
4524 }
4525
4526 #[test]
4527 fn knots_go_where_the_error_is() {
4528 let mut points = Vec::new();
4532 for i in 0..=100 {
4533 points.push(Point::new(f64::from(i) * 0.1, 0.0, 0.0));
4534 }
4535 for i in 1..=50 {
4536 let a = f64::from(i) / 50.0 * core::f64::consts::FRAC_PI_2;
4537 points.push(Point::new(10.0 + a.sin() * 0.5, (1.0 - a.cos()) * 0.5, 0.0));
4538 }
4539 for i in 1..=100 {
4540 points.push(Point::new(10.5, 0.5 + f64::from(i) * 0.1, 0.0));
4541 }
4542
4543 let fitted = fit_points(&points, 3, 1e-4, T).unwrap();
4544 assert!(fitted.met);
4545
4546 let distinct = fitted.curve.knots().distinct();
4549 let interior: Vec<f64> = distinct[1..distinct.len() - 1]
4550 .iter()
4551 .map(|(u, _)| *u)
4552 .collect();
4553 let near_corner = interior
4554 .iter()
4555 .filter(|u| (0.40..0.60).contains(*u))
4556 .count();
4557 assert!(
4558 near_corner * 2 > interior.len(),
4559 "only {near_corner} of {} interior knots are near the corner",
4560 interior.len()
4561 );
4562 }
4563
4564 #[test]
4565 fn the_ends_are_honoured_exactly_and_a_closed_loop_stays_closed() {
4566 let points = circle_points(64, 2.0);
4567 let fitted = fit_points(&points, 3, 1e-3, T).unwrap();
4568 let (a, b) = fitted.curve.knots().domain();
4569 let start = fitted.curve.point_at(a, T).unwrap();
4570 let end = fitted.curve.point_at(b, T).unwrap();
4571 assert!(start.is_equal(points[0], T), "the start drifted");
4572 assert!(end.is_equal(*points.last().unwrap(), T), "the end drifted");
4573 assert!(start.is_equal(end, T), "the loop opened");
4574 }
4575
4576 #[test]
4577 fn a_smooth_loop_closes_with_matching_tangents_at_the_join() {
4578 use crate::traits::Curve3d as _;
4579 let points = circle_points(128, 3.0);
4580 let fitted = fit_points_closed(&points, 3, 1e-4, T).unwrap();
4581 assert!(fitted.met, "target not met, reached {:e}", fitted.error);
4582
4583 let curve: crate::curve::Curve = fitted.curve.clone().into();
4584 let (a, b) = fitted.curve.knots().domain();
4585 let start = curve.point_at(a, T).unwrap();
4586 let end = curve.point_at(b, T).unwrap();
4587 assert!(start.is_equal(end, T), "the loop opened");
4588
4589 let out = curve.d1_at(a, T).unwrap();
4592 let back = curve.d1_at(b, T).unwrap();
4593 assert!(
4594 (out - back).magnitude() <= 1e-9 * out.magnitude(),
4595 "the join creases: {out:?} vs {back:?}"
4596 );
4597
4598 let open = fit_points(&points, 3, 1e-4, T).unwrap();
4601 let ocurve: crate::curve::Curve = open.curve.into();
4602 let (oa, ob) = (a, b);
4603 let _ = (ocurve.d1_at(oa, T).unwrap(), ocurve.d1_at(ob, T).unwrap());
4604 }
4605
4606 #[test]
4607 fn a_joint_closed_fit_is_smooth_in_all_seven_coordinates() {
4608 use crate::traits::{Curve2d as _, Curve3d as _};
4609 let n = 96;
4610 let mut points = Vec::new();
4611 let mut on_a = Vec::new();
4612 let mut on_b = Vec::new();
4613 for i in 0..=n {
4614 #[allow(clippy::cast_precision_loss)]
4615 let t = core::f64::consts::TAU * i as f64 / n as f64;
4616 points.push(Point::new(4.0 * t.cos(), 4.0 * t.sin(), 1.0));
4617 on_a.push(Point2::new(t.cos(), t.sin()));
4619 on_b.push(Point2::new(2.0 * t.sin(), t.cos() - 3.0));
4620 }
4621 let (space, pa, pb) = fit_points_joint_closed(&points, &on_a, &on_b, 3, 1e-4, T).unwrap();
4622 assert!(space.met, "reached {:e}", space.error);
4623
4624 let curve: crate::curve::Curve = space.curve.into();
4625 let (lo, hi) = curve.domain();
4626 let out3 = curve.d1_at(lo, T).unwrap();
4627 let back3 = curve.d1_at(hi, T).unwrap();
4628 assert!((out3 - back3).magnitude() <= 1e-9 * out3.magnitude());
4629 for plane in [&pa, &pb] {
4630 let planar: crate::curve2d::PlanarCurve = plane.clone().into();
4631 let out2 = planar.d1_at(lo, T).unwrap();
4632 let back2 = planar.d1_at(hi, T).unwrap();
4633 assert!(
4634 (out2 - back2).magnitude() <= 1e-9 * out2.magnitude(),
4635 "a pcurve creases at its seam"
4636 );
4637 }
4638 }
4639
4640 #[test]
4641 fn a_fit_that_is_not_a_loop_is_refused_by_the_closed_entry() {
4642 let mut points = circle_points(32, 1.0);
4643 points.pop();
4644 assert!(fit_points_closed(&points, 3, 1e-3, T).is_err());
4645 }
4646
4647 #[test]
4648 fn an_impossible_target_is_reported_not_rounded_up_to_success() {
4649 let points = vec![
4653 Point::new(0.0, 0.0, 0.0),
4654 Point::new(1.0, 1.0, 0.0),
4655 Point::new(2.0, -1.0, 0.0),
4656 Point::new(3.0, 1.0, 0.0),
4657 Point::new(4.0, 0.0, 0.0),
4658 ];
4659 let fitted = fit_points(&points, 3, 1e-15, T).unwrap();
4660 assert_eq!(fitted.met, fitted.error <= 1e-15);
4663 }
4664
4665 #[test]
4666 fn the_2d_fit_is_the_same_machinery() {
4667 let points: Vec<Point2> = (0..=100)
4668 .map(|i| {
4669 #[allow(clippy::cast_precision_loss)]
4670 let a = TAU * f64::from(i) / 100.0;
4671 Point2::new(3.0 * a.cos(), 3.0 * a.sin())
4672 })
4673 .collect();
4674 let fitted = fit_points_2d(&points, 3, 1e-4, T).unwrap();
4675 assert!(fitted.met);
4676 assert!(fitted.error <= 1e-4);
4677 for p in points.iter().step_by(17) {
4680 let scan = |u: f64| {
4681 fitted
4682 .curve
4683 .point_at(u, T)
4684 .map_or(f64::MAX, |q| p.distance(q))
4685 };
4686 let mut best = (0.0, f64::MAX);
4687 for i in 0..=2000 {
4688 #[allow(clippy::cast_precision_loss)]
4689 let u = i as f64 / 2000.0;
4690 let d = scan(u);
4691 if d < best.1 {
4692 best = (u, d);
4693 }
4694 }
4695 let (mut lo, mut hi) = ((best.0 - 1e-3).max(0.0), (best.0 + 1e-3).min(1.0));
4696 for _ in 0..100 {
4697 let one = lo + (hi - lo) / 3.0;
4698 let two = hi - (hi - lo) / 3.0;
4699 if scan(one) < scan(two) {
4700 hi = two;
4701 } else {
4702 lo = one;
4703 }
4704 }
4705 let found = scan(f64::midpoint(lo, hi)).min(best.1);
4706 assert!(found < 2e-4, "a 2d point is {found:e} off the fit");
4707 }
4708 }
4709
4710 #[test]
4711 fn inputs_that_describe_nothing_are_refused() {
4712 let p = Point::ORIGIN;
4713 assert!(fit_points(&[], 3, 1e-3, T).is_err());
4714 assert!(fit_points(&[p], 3, 1e-3, T).is_err());
4715 assert!(
4716 fit_points(&[p, p, p], 3, 1e-3, T).is_err(),
4717 "all duplicates"
4718 );
4719 let two = [p, Point::new(1.0, 0.0, 0.0)];
4720 assert!(fit_points(&two, 3, 0.0, T).is_err());
4721 assert!(fit_points(&two, 3, -1.0, T).is_err());
4722 assert!(fit_points(&two, 3, f64::NAN, T).is_err());
4723 assert!(fit_points(&two, 0, 1e-3, T).is_err());
4724 assert!(fit_points(&two, 3, 1e-9, T).unwrap().met);
4726 }
4727
4728 #[test]
4736 fn a_joint_fit_holds_a_straight_run_sampled_geometrically() {
4737 let mut points = Vec::new();
4738 let mut on_a = Vec::new();
4739 let mut on_b = Vec::new();
4740 let mut s = 0.0_f64;
4741 let mut step = 1e-5;
4742 while s < 16.0 {
4743 points.push(Point::new(-6.5, 9.0 - s, 8.0));
4744 on_a.push(Point2::new(-8.0 + s, -1.0));
4745 on_b.push(Point2::new(-0.2, 0.7 - s / 16.0));
4746 s += step;
4747 step = (step * 2.0).min(1.0);
4748 }
4749 points.push(Point::new(-6.5, -7.0, 8.0));
4750 on_a.push(Point2::new(8.0, -1.0));
4751 on_b.push(Point2::new(-0.2, -0.3));
4752 let (space, pa, pb) = fit_points_joint(&points, &on_a, &on_b, 3, 1e-6, T).unwrap();
4753 assert!(space.error < 1e-6, "the run is a line: {}", space.error);
4754 let (lo, hi) = space.curve.domain();
4755 for i in 0..=200 {
4756 let t = lo + (hi - lo) * f64::from(i) / 200.0;
4757 let p = space.curve.point_at(t, T).unwrap();
4758 assert!(
4759 (p.x + 6.5).abs() < 1e-6 && (p.z - 8.0).abs() < 1e-6,
4760 "off the line at {p:?}"
4761 );
4762 let a = pa.point_at(t, T).unwrap();
4763 assert!((a.y + 1.0).abs() < 1e-6 && (a.x - (-8.0 + (9.0 - p.y))).abs() < 1e-6);
4764 let b = pb.point_at(t, T).unwrap();
4765 assert!((b.x + 0.2).abs() < 1e-6);
4766 }
4767 }
4768
4769 #[test]
4772 fn a_sampled_planar_loop_closes_and_holds_between_its_samples() {
4773 use crate::traits::Curve2d as _;
4774 let tau = core::f64::consts::TAU;
4775 let at = |t: f64| {
4776 let r = 0.1f64.mul_add((9.0 * tau * t).sin(), 1.0);
4777 Point2::new(r * (tau * t).cos(), r * (tau * t).sin())
4778 };
4779 let start: Vec<f64> = (0..=12).map(|i| f64::from(i) / 12.0).collect();
4780 let fitted = fit_curve_2d_sampled(|t| Ok(at(t)), &start, true, 3, 1e-6, T).unwrap();
4781 assert!(fitted.met, "{}", fitted.error);
4782 let curve = &fitted.curve;
4783 let (a, b) = (
4784 curve.point_at(0.0, T).unwrap(),
4785 curve.point_at(1.0, T).unwrap(),
4786 );
4787 assert!(
4788 a.distance(b) < 1e-12,
4789 "the loop is open by {}",
4790 a.distance(b)
4791 );
4792 let (da, db) = (
4793 curve.derivatives_at(0.0, 1, T).unwrap()[1],
4794 curve.derivatives_at(1.0, 1, T).unwrap()[1],
4795 );
4796 assert!(
4797 (da - db).magnitude() < 1e-6 * da.magnitude(),
4798 "{da:?} against {db:?}"
4799 );
4800 let mut worst = 0.0_f64;
4801 for i in 0..=7919 {
4802 let t = (f64::from(i) + 0.37) / 7920.0;
4803 worst = worst.max(curve.point_at(t, T).unwrap().distance(at(t)));
4804 }
4805 assert!(worst <= 1e-6, "{worst}");
4806 }
4807}