1use geop_core_math::{
10 geop_error::{GeopError, GeopResult},
11 scalars::Scalar,
12 vector::{Vector, Vector2, Vector3},
13};
14
15use super::{NurbCurve, ParameterRefinable};
16
17fn uniform_params<S: Scalar>(m: usize) -> Vec<S> {
20 let mut t = vec![S::ZERO; m];
21 t[m - 1] = S::ONE;
22 for (k, tk) in t.iter_mut().enumerate().take(m - 1).skip(1) {
23 *tk = S::from_ratio(k as i64, (m - 1) as i64).unwrap();
24 }
25 t
26}
27
28fn chord_length_params<S: Scalar, const C: usize>(points: &[Vector<S, C>]) -> Vec<S> {
49 let m = points.len();
50 let mut cumulative = vec![S::ZERO; m];
51 for k in 1..m {
52 let chord = points[k].sub(&points[k - 1]).norm();
53 if chord.could_be_equal(S::ZERO) {
54 return uniform_params(m);
55 }
56 cumulative[k] = cumulative[k - 1].add(chord);
57 }
58 let total = cumulative[m - 1];
59 if total.could_be_equal(S::ZERO) {
60 return uniform_params(m);
61 }
62 let mut t = vec![S::ZERO; m];
63 t[m - 1] = S::ONE;
64 for k in 1..(m - 1) {
65 t[k] = match cumulative[k].div(total) {
70 Ok(x) => x.sharpen(),
71 Err(_) => return uniform_params(m),
72 };
73 }
74 t
75}
76
77fn averaging_knots<S: Scalar>(t: &[S], degree: usize) -> Vec<S> {
83 let m = t.len();
84 let n = m - 1;
85 let p = degree;
86 let mut knots = vec![S::ZERO; m + p + 1];
87 for i in 0..=p {
88 knots[i] = S::ZERO;
89 let last = knots.len() - 1 - i;
90 knots[last] = S::ONE;
91 }
92 let p_s = S::from_i64(p as i64);
93 for j in 1..=(n - p) {
94 let mut sum = S::ZERO;
95 for &tk in &t[j..(j + p)] {
96 sum = sum.add(tk);
97 }
98 knots[j + p] = sum.div(p_s).unwrap();
99 }
100 knots
101}
102
103fn find_span<S: Scalar>(degree: usize, knots: &[S], n: usize, t: S) -> GeopResult<usize> {
108 let p = degree;
109 if t.definitely_less(knots[p]) || t.definitely_greater(knots[n + 1]) {
110 return Err(GeopError::new(format!(
111 "parameter t={} out of domain [{}, {}]",
112 t,
113 knots[p],
114 knots[n + 1]
115 )));
116 }
117 if !t.definitely_less(knots[n + 1]) {
118 for k in (p..=n).rev() {
119 if knots[k].definitely_less(knots[n + 1]) {
120 return Ok(k);
121 }
122 }
123 return Ok(p);
124 }
125 for k in p..=n {
126 if !t.definitely_less(knots[k]) && t.definitely_less(knots[k + 1]) {
127 return Ok(k);
128 }
129 }
130 Err(GeopError::new("could not find knot span"))
131}
132
133fn basis_funs<S: Scalar>(span: usize, t: S, degree: usize, knots: &[S]) -> Vec<S> {
136 let p = degree;
137 let mut n = vec![S::ZERO; p + 1];
138 n[0] = S::ONE;
139 let mut left = vec![S::ZERO; p + 1];
140 let mut right = vec![S::ZERO; p + 1];
141
142 for j in 1..=p {
143 left[j] = t.sub(knots[span + 1 - j]);
144 right[j] = knots[span + j].sub(t);
145 let mut saved = S::ZERO;
146 for r in 0..j {
147 let denom = right[r + 1].add(left[j - r]);
148 let temp = if denom.could_be_equal(S::ZERO) {
149 S::ZERO
150 } else {
151 n[r].div(denom).unwrap_or(S::ZERO)
152 };
153 n[r] = saved.add(right[r + 1].mul(temp));
154 saved = left[j - r].mul(temp);
155 }
156 n[j] = saved;
157 }
158 n
159}
160
161fn solve_interpolation_system<S: Scalar, const C: usize, const D: usize>(
172 t: &[S],
173 knots: &[S],
174 degree: usize,
175 points: &[Vector<S, C>],
176) -> GeopResult<Vec<Vector<S, D>>> {
177 let m = points.len();
178 let p = degree;
179
180 let mut a = vec![vec![S::ZERO; m]; m];
181 for (k, &tk) in t.iter().enumerate() {
182 let span = find_span(p, knots, m - 1, tk)?;
183 let funs = basis_funs(span, tk, p, knots);
184 for (j, &val) in funs.iter().enumerate() {
185 a[k][span - p + j] = val;
186 }
187 }
188
189 let mut rhs = vec![vec![S::ZERO; C]; m];
190 for (k, pt) in points.iter().enumerate() {
191 for c in 0..C {
192 rhs[k][c] = pt[c];
193 }
194 }
195
196 for col in 0..m {
197 let pivot = a[col][col];
198 for row in (col + 1)..m {
199 let factor = a[row][col].div(pivot)?;
200 for c in col..m {
201 a[row][c] = a[row][c].sub(factor.mul(a[col][c]));
202 }
203 for c in 0..C {
204 rhs[row][c] = rhs[row][c].sub(factor.mul(rhs[col][c]));
205 }
206 }
207 }
208
209 let mut ctrl = vec![vec![S::ZERO; C]; m];
210 for row in (0..m).rev() {
211 let mut sum = rhs[row].clone();
212 for (col, ctrl_col) in ctrl.iter().enumerate().take(m).skip(row + 1) {
213 let coef = a[row][col];
214 for c in 0..C {
215 sum[c] = sum[c].sub(coef.mul(ctrl_col[c]));
216 }
217 }
218 for c in 0..C {
219 ctrl[row][c] = sum[c].div(a[row][row])?;
220 }
221 }
222
223 Ok(ctrl
224 .into_iter()
225 .map(|c| {
226 let mut v = Vector::<S, D>::zero();
227 for (i, &val) in c.iter().enumerate() {
228 v[i] = val;
229 }
230 v[C] = S::ONE;
231 v
232 })
233 .collect())
234}
235
236pub fn true_point_fractions(i: usize, intervals: usize) -> &'static [(i64, i64)] {
246 if i == 0 || i + 1 == intervals {
247 &[(1, 4), (1, 2), (3, 4)]
248 } else {
249 &[(1, 2)]
250 }
251}
252
253fn interpolate<S: Scalar, const C: usize, const D: usize>(
259 points: &[Vector<S, C>],
260 between: Option<&[Vec<Vector<S, C>>]>,
261 degree: usize,
262) -> GeopResult<NurbCurve<S, D>>
263where
264 NurbCurve<S, D>: ParameterRefinable<S, C>,
265{
266 if points.len() < 2 {
267 return Err(GeopError::new(
268 "NurbCurve::interpolate: need at least 2 points",
269 ));
270 }
271
272 let m = points.len();
273 let p = degree.max(1).min(m - 1);
274
275 let centres: Vec<Vector<S, C>>;
281 let through = if between.is_some() {
282 centres = points.iter().map(|q| q.sharpen()).collect();
283 ¢res[..]
284 } else {
285 points
286 };
287
288 let t = chord_length_params(through);
289 let knots = averaging_knots(&t, p);
290 let control_points = solve_interpolation_system::<S, C, D>(&t, &knots, p, through)?;
291
292 let mut curve = NurbCurve::try_new(p, control_points, knots)?;
293 if let Some(between) = between {
294 if between.len() != m - 1 {
295 return Err(GeopError::new(format!(
296 "NurbCurve::interpolate_enclosing: expected true points for each of the {} \
297 intervals between the {m} samples, got {}",
298 m - 1,
299 between.len()
300 )));
301 }
302 let mut checks: Vec<(S, Vector<S, C>)> =
307 t.iter().copied().zip(points.iter().copied()).collect();
308 for (w, qs) in t.windows(2).zip(between) {
309 for (j, &q) in qs.iter().enumerate() {
310 let frac = S::from_ratio(j as i64 + 1, qs.len() as i64 + 1)?;
311 checks.push((S::interpolate(w[0], w[1], frac).sharpen(), q));
312 }
313 }
314 widen_to_enclose(&mut curve, &checks)?;
315 }
316 Ok(curve)
317}
318
319fn widen_to_enclose<S: Scalar, const C: usize, const D: usize>(
340 curve: &mut NurbCurve<S, D>,
341 checks: &[(S, Vector<S, C>)],
342) -> GeopResult<()>
343where
344 NurbCurve<S, D>: ParameterRefinable<S, C>,
345{
346 let (lo, hi) = curve.domain();
347 let mut pad = [S::ZERO; C];
348 for &(guess, q) in checks {
349 let position = curve.evaluate_cartesian(guess)?;
350 let tangent = curve.tangent_cartesian(guess)?;
351 let step = position
352 .sub(&q)
353 .prod_dot(&tangent)
354 .div(tangent.prod_dot(&tangent))
355 .unwrap_or(S::ZERO);
356 let tau = guess.sub(step).sharpen();
357 let tau = if tau.definitely_less(lo) {
358 lo
359 } else if tau.definitely_greater(hi) {
360 hi
361 } else {
362 tau
363 };
364 let on_curve = curve.evaluate_cartesian(tau)?;
368 for k in 0..C {
369 let above = q[k].upper().sub(on_curve[k].upper()).upper();
370 let below = on_curve[k].lower().sub(q[k].lower()).upper();
371 pad[k] = pad[k].union(above).union(below).upper();
372 }
373 }
374 for cp in &mut curve.control_points {
375 let w = cp[D - 1];
376 for k in 0..C {
377 let d = pad[k].mul(w);
378 cp[k] = cp[k].sub(d).union(cp[k].add(d));
379 }
380 }
381 curve.recompute_aabb();
382 Ok(())
383}
384
385impl<S: Scalar> NurbCurve<S, 4> {
386 pub fn interpolate(points: &[Vector3<S>], degree: usize) -> GeopResult<Self> {
391 interpolate::<S, 3, 4>(points, None, degree)
392 }
393
394 pub fn interpolate_enclosing(
409 points: &[Vector3<S>],
410 between: &[Vec<Vector3<S>>],
411 degree: usize,
412 ) -> GeopResult<Self> {
413 interpolate::<S, 3, 4>(points, Some(between), degree)
414 }
415}
416
417impl<S: Scalar> NurbCurve<S, 3> {
418 pub fn interpolate(points: &[Vector2<S>], degree: usize) -> GeopResult<Self> {
421 interpolate::<S, 2, 3>(points, None, degree)
422 }
423
424 pub fn interpolate_enclosing(
426 points: &[Vector2<S>],
427 between: &[Vec<Vector2<S>>],
428 degree: usize,
429 ) -> GeopResult<Self> {
430 interpolate::<S, 2, 3>(points, Some(between), degree)
431 }
432}
433
434#[cfg(test)]
435mod tests {
436 use super::*;
437 use geop_core_math::for_all_scalars;
438
439 fn v3<S: Scalar>(x: f64, y: f64, z: f64) -> Vector3<S> {
440 Vector3::from_array([S::from_f64(x), S::from_f64(y), S::from_f64(z)])
441 }
442
443 fn v2<S: Scalar>(x: f64, y: f64) -> Vector2<S> {
444 Vector2::from_array([S::from_f64(x), S::from_f64(y)])
445 }
446
447 fn check_interpolate_reproduces_endpoints<S: Scalar>() {
455 let points = vec![
458 v3::<S>(0.5, 0.5, 0.5),
459 v3::<S>(0.4713, 0.5219, 0.4102),
460 v3::<S>(0.4402, 0.5411, 0.3301),
461 v3::<S>(0.4001, 0.5502, 0.2604),
462 v3::<S>(0.5, 0.5, 0.2),
463 ];
464 for degree in [1, 2, 3] {
465 let curve = NurbCurve::<S, 4>::interpolate(&points, degree).unwrap();
466 let (t0, t1) = curve.domain();
467 let start = curve.evaluate(t0).unwrap();
468 let end = curve.evaluate(t1).unwrap();
469 assert!(
470 start.could_be_equal(&points[0]),
471 "degree {degree}: start {start:?} != {:?}",
472 points[0]
473 );
474 assert!(
475 end.could_be_equal(points.last().unwrap()),
476 "degree {degree}: end {end:?} != {:?}",
477 points.last().unwrap()
478 );
479 }
480 }
481
482 #[test]
483 fn interpolate_reproduces_endpoints() {
484 for_all_scalars!(check_interpolate_reproduces_endpoints);
485 }
486
487 fn check_interpolate_2d_curve_matches_samples<S: Scalar>() {
490 let n = 20;
491 let points: Vec<_> = (0..=n)
492 .map(|i| {
493 let x = i as f64 / n as f64;
494 v2(x, x * x)
495 })
496 .collect();
497
498 let curve = NurbCurve::<S, 3>::interpolate(&points, 3).unwrap();
499
500 let (t0, t1) = curve.domain();
501 let mid_t = t0.add(t1.sub(t0).mul(S::from_f64(0.5)));
502 let p = curve.evaluate(mid_t).unwrap();
503 assert!(
504 p[1].sub(p[0].mul(p[0]))
505 .abs()
506 .could_be_less(S::from_f64(1e-3))
507 );
508 }
509 #[test]
510 fn interpolate_2d_curve_matches_samples() {
511 for_all_scalars!(check_interpolate_2d_curve_matches_samples);
512 }
513
514 fn check_interpolate_line<S: Scalar>() {
517 let n = 50;
518 let points: Vec<_> = (0..=n).map(|i| v3(i as f64 / n as f64, 0.0, 0.0)).collect();
519
520 let curve = NurbCurve::<S, 4>::interpolate(&points, 3).unwrap();
521
522 let p = curve.evaluate(S::from_f64(0.5)).unwrap();
523 assert!(p[0].could_be_equal(S::from_f64(0.5)));
524 assert!(p[1].could_be_equal(S::ZERO));
525 assert!(p[2].could_be_equal(S::ZERO));
526 }
527 #[test]
528 fn interpolate_line() {
529 for_all_scalars!(check_interpolate_line);
530 }
531
532 fn check_interpolate_passes_through_corner<S: Scalar>() {
535 let mut points = Vec::new();
536 for i in 0..=10 {
537 points.push(v3(i as f64 / 10.0, 0.0, 0.0));
538 }
539 for i in 1..=10 {
540 points.push(v3(1.0, i as f64 / 10.0, 0.0));
541 }
542
543 let curve = NurbCurve::<S, 4>::interpolate(&points, 3).unwrap();
544
545 let (t0, t1) = curve.domain();
546 let start = curve.evaluate(t0).unwrap();
547 let end = curve.evaluate(t1).unwrap();
548 assert!(start[0].could_be_equal(S::ZERO));
549 assert!(start[1].could_be_equal(S::ZERO));
550 assert!(end[0].could_be_equal(S::ONE));
551 assert!(end[1].could_be_equal(S::ONE));
552 }
553 #[test]
554 fn interpolate_passes_through_corner() {
555 for_all_scalars!(check_interpolate_passes_through_corner);
556 }
557
558 fn check_interpolate_curve_matches_samples<S: Scalar>() {
563 let n = 20;
564 let points: Vec<_> = (0..=n)
565 .map(|i| {
566 let x = i as f64 / n as f64;
567 v3(x, x * x, 0.0)
568 })
569 .collect();
570
571 let curve = NurbCurve::<S, 4>::interpolate(&points, 3).unwrap();
572
573 let (t0, t1) = curve.domain();
574 let mid_t = t0.add(t1.sub(t0).mul(S::from_f64(0.5)));
575 let p = curve.evaluate(mid_t).unwrap();
576 assert!(
578 p[1].sub(p[0].mul(p[0]))
579 .abs()
580 .could_be_less(S::from_f64(1e-3))
581 );
582 }
583 #[test]
584 fn interpolate_curve_matches_samples() {
585 for_all_scalars!(check_interpolate_curve_matches_samples);
586 }
587
588 fn check_interpolate_enclosing_contains_the_true_curve<S: Scalar>() {
592 use crate::contains::curve::curve_could_contain;
593 let circle = |a: f64| v3::<S>(a.cos(), a.sin(), 0.);
594 let n = 8;
595 let angle = |i: usize, of: usize| std::f64::consts::FRAC_PI_2 * i as f64 / of as f64;
596 let points: Vec<_> = (0..=n).map(|i| circle(angle(i, n))).collect();
597 let between: Vec<Vec<_>> = (0..n)
598 .map(|i| {
599 true_point_fractions(i, n)
600 .iter()
601 .map(|&(a, b)| circle(angle(i * b as usize + a as usize, n * b as usize)))
602 .collect()
603 })
604 .collect();
605
606 let exact = NurbCurve::<S, 4>::interpolate(&points, 3).unwrap();
607 let enclosing = NurbCurve::<S, 4>::interpolate_enclosing(&points, &between, 3).unwrap();
608 let eps = S::from_f64(1e-6);
609 let dense: Vec<_> = (0..=200).map(|i| circle(angle(i, 200))).collect();
610 let found = |c: &NurbCurve<S, 4>| {
611 dense
612 .iter()
613 .filter(|q| curve_could_contain(c, q, 5000, eps).unwrap().is_some())
614 .count()
615 };
616 assert!(
617 found(&exact) < dense.len(),
618 "the exact interpolant should drift"
619 );
620 let missed: Vec<(usize, Result<Option<S>, String>)> = dense
621 .iter()
622 .enumerate()
623 .map(|(i, q)| {
624 (
625 i,
626 curve_could_contain(&enclosing, q, 5000, eps).map_err(|e| format!("{e}")),
627 )
628 })
629 .filter(|(_, r)| !matches!(r, Ok(Some(_))))
630 .collect();
631 assert!(missed.is_empty(), "true points not enclosed: {missed:?}");
632 }
633 #[test]
634 fn interpolate_enclosing_contains_the_true_curve() {
635 for_all_scalars!(check_interpolate_enclosing_contains_the_true_curve);
636 }
637}