1use crate::{DiagnosticSeverity, KernelDiagnostics, KernelStage, KnotVector, NurbsCurve, Vec3, Vec4};
2
3pub fn solve_dense(mut matrix: Vec<Vec<f64>>, mut rhs: Vec<f64>) -> Result<Vec<f64>, String> {
4 let count = rhs.len();
5 if count == 0 || matrix.len() != count || matrix.iter().any(|row| row.len() != count) {
6 return Err("solve_dense: matrix must be square and match RHS".into());
7 }
8 let scale = matrix
9 .iter()
10 .flatten()
11 .fold(0.0_f64, |maximum, value| maximum.max(value.abs()));
12 if scale == 0.0 {
13 return Err("solve_dense: singular matrix".into());
14 }
15 for column in 0..count {
16 let pivot = (column..count)
17 .max_by(|&a, &b| matrix[a][column].abs().total_cmp(&matrix[b][column].abs()))
18 .unwrap();
19 if matrix[pivot][column].abs() <= 1e-13 * scale {
20 return Err("solve_dense: singular matrix".into());
21 }
22 matrix.swap(column, pivot);
23 rhs.swap(column, pivot);
24 let diagonal = matrix[column][column];
25 for row in column + 1..count {
26 let factor = matrix[row][column] / diagonal;
27 matrix[row][column] = 0.0;
28 for entry in column + 1..count {
29 matrix[row][entry] -= factor * matrix[column][entry];
30 }
31 rhs[row] -= factor * rhs[column];
32 }
33 }
34 let mut result = vec![0.0; count];
35 for row in (0..count).rev() {
36 let remainder: f64 = (row + 1..count)
37 .map(|column| matrix[row][column] * result[column])
38 .sum();
39 result[row] = (rhs[row] - remainder) / matrix[row][row];
40 }
41 Ok(result)
42}
43
44pub fn solve_small<const N: usize>(
48 mut matrix: [[f64; N]; N],
49 mut rhs: [f64; N],
50 count: usize,
51) -> Result<[f64; N], String> {
52 if count == 0 || count > N {
53 return Err("solve_small: invalid system size".into());
54 }
55 let scale = matrix
56 .iter()
57 .take(count)
58 .flat_map(|row| row.iter().take(count))
59 .fold(0.0_f64, |maximum, value| maximum.max(value.abs()));
60 if scale == 0.0 {
61 return Err("solve_small: singular matrix".into());
62 }
63 for column in 0..count {
64 let pivot = (column..count)
65 .max_by(|&a, &b| matrix[a][column].abs().total_cmp(&matrix[b][column].abs()))
66 .unwrap();
67 if matrix[pivot][column].abs() <= 1e-13 * scale {
68 return Err("solve_small: singular matrix".into());
69 }
70 matrix.swap(column, pivot);
71 rhs.swap(column, pivot);
72 let diagonal = matrix[column][column];
73 for row in column + 1..count {
74 let factor = matrix[row][column] / diagonal;
75 matrix[row][column] = 0.0;
76 for entry in column + 1..count {
77 matrix[row][entry] -= factor * matrix[column][entry];
78 }
79 rhs[row] -= factor * rhs[column];
80 }
81 }
82 let mut result = [0.0; N];
83 for row in (0..count).rev() {
84 let remainder: f64 = (row + 1..count)
85 .map(|column| matrix[row][column] * result[column])
86 .sum();
87 result[row] = (rhs[row] - remainder) / matrix[row][row];
88 }
89 Ok(result)
90}
91
92pub fn solve_collocation(
103 matrix: &[Vec<f64>],
104 rhs: &[f64],
105 degree: usize,
106) -> Result<Vec<f64>, String> {
107 let count = rhs.len();
108 let bandwidth = degree.max(1).min(count.saturating_sub(1).max(1));
109 if let Ok(solution) = solve_banded(matrix, rhs, bandwidth) {
110 let scale = rhs
111 .iter()
112 .fold(0.0_f64, |maximum, value| maximum.max(value.abs()));
113 let tolerance = 1e-9 * scale.max(1.0);
114 let mut residual = 0.0_f64;
115 for row in 0..count {
116 let lo = row.saturating_sub(bandwidth);
117 let hi = (row + bandwidth).min(count - 1);
118 let mut accumulated = 0.0;
119 for col in lo..=hi {
120 accumulated += matrix[row][col] * solution[col];
121 }
122 residual = residual.max((accumulated - rhs[row]).abs());
123 }
124 if residual <= tolerance {
125 return Ok(solution);
126 }
127 }
128 solve_dense(matrix.to_vec(), rhs.to_vec())
129}
130
131pub(crate) struct CollocationBand {
140 count: usize,
141 bandwidth: usize,
142 data: Vec<f64>,
143}
144
145impl CollocationBand {
146 pub(crate) fn new(count: usize, degree: usize) -> Self {
149 let bandwidth = degree.max(1).min(count.saturating_sub(1).max(1));
150 CollocationBand { count, bandwidth, data: vec![0.0; count * (2 * bandwidth + 1)] }
151 }
152
153 fn slot(&self, row: usize, column: usize) -> Option<usize> {
154 let offset = column as isize - row as isize + self.bandwidth as isize;
155 (offset >= 0 && offset <= 2 * self.bandwidth as isize)
156 .then(|| row * (2 * self.bandwidth + 1) + offset as usize)
157 }
158
159 pub(crate) fn set(&mut self, row: usize, column: usize, value: f64) -> bool {
162 match self.slot(row, column) {
163 Some(index) => {
164 self.data[index] = value;
165 true
166 }
167 None => value == 0.0,
168 }
169 }
170
171 fn get(&self, row: usize, column: usize) -> f64 {
172 self.slot(row, column).map_or(0.0, |index| self.data[index])
173 }
174
175 fn to_dense(&self) -> Vec<Vec<f64>> {
176 (0..self.count)
177 .map(|row| (0..self.count).map(|column| self.get(row, column)).collect())
178 .collect()
179 }
180}
181
182pub(crate) fn solve_collocation_banded(band: &CollocationBand, rhs: &[f64]) -> Result<Vec<f64>, String> {
187 let count = rhs.len();
188 if count == 0 || band.count != count {
189 return Err("solve_banded: matrix must be square and match RHS".into());
190 }
191 let bandwidth = band.bandwidth;
192 if let Ok(solution) = solve_band_storage(band, rhs) {
193 let scale = rhs
194 .iter()
195 .fold(0.0_f64, |maximum, value| maximum.max(value.abs()));
196 let tolerance = 1e-9 * scale.max(1.0);
197 let mut residual = 0.0_f64;
198 for row in 0..count {
199 let lo = row.saturating_sub(bandwidth);
200 let hi = (row + bandwidth).min(count - 1);
201 let mut accumulated = 0.0;
202 for col in lo..=hi {
203 accumulated += band.get(row, col) * solution[col];
204 }
205 residual = residual.max((accumulated - rhs[row]).abs());
206 }
207 if residual <= tolerance {
208 return Ok(solution);
209 }
210 }
211 solve_dense(band.to_dense(), rhs.to_vec())
212}
213
214fn solve_band_storage(band: &CollocationBand, rhs: &[f64]) -> Result<Vec<f64>, String> {
215 let count = rhs.len();
216 let bandwidth = band.bandwidth;
217 let width = 2 * bandwidth + 1;
218 let mut matrix = band.data.clone();
219 let mut rhs = rhs.to_vec();
220 let at = |row: usize, column: usize| row * width + column + bandwidth - row;
222 for column in 0..count {
223 let pivot = matrix[at(column, column)];
224 if pivot.abs() <= 1e-300 {
225 return Err("solve_banded: singular matrix".into());
226 }
227 let maximum_row = (column + bandwidth).min(count - 1);
228 for row in column + 1..=maximum_row {
229 let factor = matrix[at(row, column)] / pivot;
230 if factor == 0.0 {
231 continue;
232 }
233 let maximum_column = (column + bandwidth).min(count - 1);
234 for entry in column..=maximum_column {
235 matrix[at(row, entry)] -= factor * matrix[at(column, entry)];
236 }
237 rhs[row] -= factor * rhs[column];
238 }
239 }
240 let mut result = vec![0.0; count];
241 for row in (0..count).rev() {
242 let mut value = rhs[row];
243 let maximum_column = (row + bandwidth).min(count - 1);
244 for column in row + 1..=maximum_column {
245 value -= matrix[at(row, column)] * result[column];
246 }
247 if matrix[at(row, row)].abs() <= 1e-300 {
248 return Err("solve_banded: singular matrix".into());
249 }
250 result[row] = value / matrix[at(row, row)];
251 }
252 Ok(result)
253}
254
255pub fn solve_banded(
256 matrix: &[Vec<f64>],
257 rhs: &[f64],
258 bandwidth: usize,
259) -> Result<Vec<f64>, String> {
260 let count = rhs.len();
261 if count == 0 || matrix.len() != count || matrix.iter().any(|row| row.len() != count) {
262 return Err("solve_banded: matrix must be square and match RHS".into());
263 }
264 let mut matrix = matrix.to_vec();
265 let mut rhs = rhs.to_vec();
266 for column in 0..count {
267 let pivot = matrix[column][column];
268 if pivot.abs() <= 1e-300 {
269 return Err("solve_banded: singular matrix".into());
270 }
271 let maximum_row = (column + bandwidth).min(count - 1);
272 for row in column + 1..=maximum_row {
273 let factor = matrix[row][column] / pivot;
274 if factor == 0.0 {
275 continue;
276 }
277 let maximum_column = (column + bandwidth).min(count - 1);
278 for entry in column..=maximum_column {
279 matrix[row][entry] -= factor * matrix[column][entry];
280 }
281 rhs[row] -= factor * rhs[column];
282 }
283 }
284 let mut result = vec![0.0; count];
285 for row in (0..count).rev() {
286 let mut value = rhs[row];
287 let maximum_column = (row + bandwidth).min(count - 1);
288 for column in row + 1..=maximum_column {
289 value -= matrix[row][column] * result[column];
290 }
291 if matrix[row][row].abs() <= 1e-300 {
292 return Err("solve_banded: singular matrix".into());
293 }
294 result[row] = value / matrix[row][row];
295 }
296 Ok(result)
297}
298
299pub fn interpolate_homogeneous(
306 points: &[Vec4],
307 degree: usize,
308 parameters: &[f64],
309) -> Result<NurbsCurve, String> {
310 if points.len() < 2 || parameters.len() != points.len() {
311 return Err(
312 "interpolate_homogeneous: points and parameters must have matching length >= 2".into(),
313 );
314 }
315 if parameters.windows(2).any(|pair| pair[1] <= pair[0]) {
316 return Err("interpolate_homogeneous: parameters must be strictly increasing".into());
317 }
318 let n = points.len() - 1;
319 let degree = degree.min(n);
320 let m = n + degree + 1;
321 let mut knots = vec![0.0; m + 1];
322 for knot in &mut knots[m - degree..=m] {
323 *knot = 1.0;
324 }
325 for j in 1..=n.saturating_sub(degree) {
326 knots[j + degree] = parameters[j..j + degree].iter().sum::<f64>() / degree as f64;
327 }
328 let knot_vector = KnotVector::new(knots.clone(), degree)?;
329 let mut matrix = vec![vec![0.0; n + 1]; n + 1];
330 for (row, ¶meter) in parameters.iter().enumerate() {
331 let span = knot_vector.find_span(parameter);
332 let basis = knot_vector.basis_functions(span, parameter);
333 for (offset, value) in basis.into_iter().enumerate() {
334 matrix[row][span - degree + offset] = value;
335 }
336 }
337 let solve_axis = |axis: fn(&Vec4) -> f64| {
338 solve_collocation(
339 &matrix,
340 &points.iter().map(axis).collect::<Vec<_>>(),
341 degree,
342 )
343 };
344 let xs = solve_axis(|point| point.x)?;
345 let ys = solve_axis(|point| point.y)?;
346 let zs = solve_axis(|point| point.z)?;
347 let ws = solve_axis(|point| point.w)?;
348 NurbsCurve::new(
349 degree,
350 knots,
351 (0..=n)
352 .map(|index| Vec4 {
353 x: xs[index],
354 y: ys[index],
355 z: zs[index],
356 w: ws[index],
357 })
358 .collect(),
359 )
360}
361
362pub fn interpolate_curve(
378 points: &[Vec3],
379 degree: usize,
380 parameters: &[f64],
381) -> Result<NurbsCurve, String> {
382 if points.len() < 2 || parameters.len() != points.len() {
383 return Err(
384 "interpolate_curve: points and parameters must have matching length >= 2".into(),
385 );
386 }
387 if parameters.windows(2).any(|pair| pair[1] <= pair[0]) {
388 return Err("interpolate_curve: parameters must be strictly increasing".into());
389 }
390 let n = points.len() - 1;
391 let degree = degree.min(n);
392 let m = n + degree + 1;
393 let mut knots = vec![parameters[0]; m + 1];
394 for knot in &mut knots[m - degree..=m] {
395 *knot = parameters[n];
396 }
397 for j in 1..=n.saturating_sub(degree) {
398 knots[j + degree] = parameters[j..j + degree].iter().sum::<f64>() / degree as f64;
399 }
400 let knot_vector = KnotVector::new(knots.clone(), degree)?;
401 let mut band = CollocationBand::new(n + 1, degree);
404 let mut dense: Option<Vec<Vec<f64>>> = None;
405 for (row, ¶meter) in parameters.iter().enumerate() {
406 let span = knot_vector.find_span(parameter);
407 let basis = knot_vector.basis_functions(span, parameter);
408 for (offset, value) in basis.into_iter().enumerate() {
409 let column = span - degree + offset;
410 if !band.set(row, column, value) {
411 dense.get_or_insert_with(|| band.to_dense())[row][column] = value;
412 } else if let Some(dense) = dense.as_mut() {
413 dense[row][column] = value;
414 }
415 }
416 }
417 let solve_axis = |axis: fn(Vec3) -> f64| {
418 let rhs = points.iter().copied().map(axis).collect::<Vec<_>>();
419 match &dense {
420 Some(matrix) => solve_collocation(matrix, &rhs, degree),
421 None => solve_collocation_banded(&band, &rhs),
422 }
423 };
424 let xs = solve_axis(|point| point.x)?;
425 let ys = solve_axis(|point| point.y)?;
426 let zs = solve_axis(|point| point.z)?;
427 NurbsCurve::new(
428 degree,
429 knots,
430 (0..=n)
431 .map(|index| Vec4::from_point(Vec3::new(xs[index], ys[index], zs[index]), 1.0))
432 .collect(),
433 )
434}
435
436pub fn interpolate_curve_with_end_tangents(
443 points: &[Vec3],
444 parameters: &[f64],
445 start_tangent: Vec3,
446 end_tangent: Vec3,
447) -> Result<NurbsCurve, String> {
448 if points.len() < 2 || parameters.len() != points.len() {
449 return Err(
450 "interpolate_curve_with_end_tangents: points and parameters must match, >= 2".into(),
451 );
452 }
453 if parameters.windows(2).any(|pair| pair[1] <= pair[0]) {
454 return Err("interpolate_curve_with_end_tangents: parameters must increase".into());
455 }
456 let degree = 3usize;
457 let n = points.len() - 1;
458 let control_count = n + 3;
459 let t0 = parameters[0];
460 let t1 = parameters[n];
461 let mut knots = vec![t0; degree + 1];
464 for j in 0..n.saturating_sub(1) {
465 let window = ¶meters[j + 1..(j + degree).min(n) + 1];
466 knots.push(window.iter().sum::<f64>() / window.len() as f64);
467 }
468 knots.extend(std::iter::repeat(t1).take(degree + 1));
469 if knots.len() != control_count + degree + 1 {
470 return Err(format!(
471 "interpolate_curve_with_end_tangents: internal knot count {} for {} controls",
472 knots.len(),
473 control_count
474 ));
475 }
476 let knot_vector = KnotVector::new(knots.clone(), degree)?;
477 let mut matrix = vec![vec![0.0; control_count]; control_count];
478 let mut rhs_points = vec![Vec3::default(); control_count];
479 matrix[0][0] = 1.0;
483 rhs_points[0] = points[0];
484 let start_span = knots[degree + 1] - t0;
485 matrix[1][0] = -(degree as f64) / start_span;
486 matrix[1][1] = (degree as f64) / start_span;
487 rhs_points[1] = start_tangent;
488 for (index, ¶meter) in parameters.iter().enumerate().take(n).skip(1) {
489 let row = index + 1;
490 let span = knot_vector.find_span(parameter);
491 let basis = knot_vector.basis_functions(span, parameter);
492 for (offset, value) in basis.into_iter().enumerate() {
493 matrix[row][span - degree + offset] = value;
494 }
495 rhs_points[row] = points[index];
496 }
497 let end_span = t1 - knots[control_count - 1];
498 matrix[control_count - 2][control_count - 2] = -(degree as f64) / end_span;
499 matrix[control_count - 2][control_count - 1] = (degree as f64) / end_span;
500 rhs_points[control_count - 2] = end_tangent;
501 matrix[control_count - 1][control_count - 1] = 1.0;
502 rhs_points[control_count - 1] = points[n];
503 let solve_axis = |axis: fn(Vec3) -> f64| {
504 solve_dense(
505 matrix.clone(),
506 rhs_points.iter().copied().map(axis).collect::<Vec<_>>(),
507 )
508 };
509 let xs = solve_axis(|point| point.x)?;
510 let ys = solve_axis(|point| point.y)?;
511 let zs = solve_axis(|point| point.z)?;
512 NurbsCurve::new(
513 degree,
514 knots,
515 (0..control_count)
516 .map(|index| Vec4::from_point(Vec3::new(xs[index], ys[index], zs[index]), 1.0))
517 .collect(),
518 )
519}
520
521pub fn interpolate_curve_thinned(
522 points: &[Vec3],
523 degree: usize,
524 maximum_points: usize,
525) -> Result<NurbsCurve, String> {
526 if maximum_points < 2 {
527 return Err("interpolate_curve_thinned: maximum point count must be at least 2".into());
528 }
529 if points.len() <= maximum_points {
530 let parameters = chord_parameters(points);
531 return interpolate_curve(points, degree, ¶meters);
532 }
533 let step = (points.len() - 1) as f64 / (maximum_points - 1) as f64;
534 let thinned = (0..maximum_points)
535 .map(|index| points[(index as f64 * step).round() as usize])
536 .collect::<Vec<_>>();
537 let parameters = chord_parameters(&thinned);
538 interpolate_curve(&thinned, degree, ¶meters)
539}
540
541fn chord_parameters(points: &[Vec3]) -> Vec<f64> {
542 if points.len() < 2 {
543 return vec![0.0; points.len()];
544 }
545 let mut parameters = vec![0.0; points.len()];
546 for index in 1..points.len() {
547 parameters[index] = parameters[index - 1] + points[index].sub(points[index - 1]).length();
548 }
549 let length = parameters[points.len() - 1];
550 if length <= 1e-15 {
551 for (index, parameter) in parameters.iter_mut().enumerate() {
552 *parameter = index as f64 / (points.len() - 1) as f64;
553 }
554 } else {
555 for parameter in &mut parameters {
556 *parameter /= length;
557 }
558 }
559 parameters
560}
561
562pub fn simplify_polyline_indices(points: &[Vec3], tolerance: f64) -> Vec<usize> {
571 if points.len() <= 2 {
572 return (0..points.len()).collect();
573 }
574 let mut keep = vec![false; points.len()];
575 keep[0] = true;
576 keep[points.len() - 1] = true;
577 let mut stack = vec![(0usize, points.len() - 1)];
578 while let Some((start, end)) = stack.pop() {
579 if end - start < 2 {
580 continue;
581 }
582 let a = points[start];
583 let direction = points[end].sub(a);
584 let length_squared = direction.length_squared().max(1e-300);
585 let mut worst = None;
586 let mut worst_distance = tolerance;
587 for (index, point) in points.iter().enumerate().take(end).skip(start + 1) {
588 let fraction = point.sub(a).dot(direction) / length_squared;
589 let fraction = fraction.clamp(0.0, 1.0);
590 let distance = point.sub(a.add(direction.scale(fraction))).length();
591 if distance > worst_distance {
592 worst_distance = distance;
593 worst = Some(index);
594 }
595 }
596 if let Some(index) = worst {
597 keep[index] = true;
598 stack.push((start, index));
599 stack.push((index, end));
600 }
601 }
602 keep.into_iter()
603 .enumerate()
604 .filter_map(|(index, keep)| keep.then_some(index))
605 .collect()
606}
607
608pub fn simplify_polyline(points: &[Vec3], tolerance: f64) -> Vec<Vec3> {
609 simplify_polyline_indices(points, tolerance)
610 .into_iter()
611 .map(|index| points[index])
612 .collect()
613}
614
615pub fn interpolate_curve_local(
616 points: &[Vec3],
617 parameters: &[f64],
618 tension: f64,
619) -> Result<NurbsCurve, String> {
620 if points.len() != parameters.len() || points.len() < 2 {
621 return Err(
622 "interpolate_curve_local: points and parameters must have matching length >= 2".into(),
623 );
624 }
625 if points.len() == 2 {
626 return interpolate_curve(points, 1, parameters);
627 }
628 let count = points.len();
629 let mut tangents = vec![Vec3::default(); count];
630 tangents[0] = points[1]
631 .sub(points[0])
632 .scale(1.0 / (parameters[1] - parameters[0]));
633 tangents[count - 1] = points[count - 1]
634 .sub(points[count - 2])
635 .scale(1.0 / (parameters[count - 1] - parameters[count - 2]));
636 for index in 1..count - 1 {
637 let previous_interval = parameters[index] - parameters[index - 1];
638 let next_interval = parameters[index + 1] - parameters[index];
639 let total = previous_interval + next_interval;
640 let previous_secant = points[index]
641 .sub(points[index - 1])
642 .scale(1.0 / previous_interval);
643 let next_secant = points[index + 1]
644 .sub(points[index])
645 .scale(1.0 / next_interval);
646 let mut tangent =
647 points[index - 1]
648 .scale(-next_interval / (previous_interval * total))
649 .add(points[index].scale(
650 (next_interval - previous_interval) / (previous_interval * next_interval),
651 ))
652 .add(points[index + 1].scale(previous_interval / (next_interval * total)));
653 if previous_secant.dot(next_secant) <= 0.0
654 || tangent.dot(previous_secant) <= 0.0
655 || tangent.dot(next_secant) <= 0.0
656 {
657 tangent = Vec3::default();
658 } else {
659 let maximum = 3.0 * previous_secant.length().min(next_secant.length());
660 if tangent.length() > maximum {
661 tangent = tangent.normalized()?.scale(maximum);
662 }
663 }
664 tangents[index] = tangent;
665 }
666 if tension != 1.0 {
667 for tangent in &mut tangents {
668 *tangent = tangent.scale(tension);
669 }
670 }
671 let mut control_points = vec![Vec4::from_point(points[0], 1.0)];
672 for index in 0..count - 1 {
673 let interval = parameters[index + 1] - parameters[index];
674 control_points.extend([
675 Vec4::from_point(
676 points[index].add(tangents[index].scale(interval / 3.0)),
677 1.0,
678 ),
679 Vec4::from_point(
680 points[index + 1].sub(tangents[index + 1].scale(interval / 3.0)),
681 1.0,
682 ),
683 Vec4::from_point(points[index + 1], 1.0),
684 ]);
685 }
686 let mut knots = vec![parameters[0]; 4];
687 for parameter in ¶meters[1..count - 1] {
688 knots.extend([*parameter; 3]);
689 }
690 knots.extend([parameters[count - 1]; 4]);
691 NurbsCurve::new(3, knots, control_points)
692}
693
694const LADDER_BUDGET_FACTOR: usize = 2;
710
711pub const MAX_FIT_STATIONS: usize = 1024;
733
734const MEASURE_NEWTON_STEPS: usize = 3;
740
741const MEASURE_SPAN_FRACTIONS: [f64; 3] = [0.25, 0.5, 0.75];
747
748const STALL_FACTOR: f64 = 0.9;
753
754#[derive(Clone, Copy, Debug, PartialEq)]
772pub struct PolylineFitReport {
773 pub deviation: f64,
777 pub rms: f64,
779 pub between_stations: f64,
783 pub tolerance: f64,
785 pub floor: f64,
797 pub stations: usize,
799 pub controls: usize,
801 pub input_points: usize,
803 pub pool_points: usize,
806 pub dropped_band: f64,
813 pub rungs: usize,
816 pub local_lane: bool,
821 pub lane_switched: bool,
824 pub exit: PolylineFitExit,
826}
827
828impl PolylineFitReport {
829 pub fn met_tolerance(&self) -> bool {
833 self.deviation <= self.floor
834 }
835
836 pub fn met_requested_tolerance(&self) -> bool {
838 self.deviation <= self.tolerance
839 }
840
841 pub fn saturated(&self) -> bool {
852 self.stations >= self.pool_points
853 }
854}
855
856#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
861pub enum PolylineFitExit {
862 Converged,
864 StationCeiling,
867 InputExhausted,
871 Stalled,
875 Unrefined,
878}
879
880#[derive(Clone, Debug)]
881pub struct PolylineFit {
882 pub curve: NurbsCurve,
883 pub parameters: Vec<f64>,
884 pub kept: Vec<Vec3>,
885 pub report: PolylineFitReport,
887}
888
889fn station_pool(points: &[Vec3], tolerance: f64) -> Vec<usize> {
898 let simplified = simplify_polyline_indices(points, tolerance);
899 if simplified.len() > 2 {
900 let total: f64 = simplified
901 .windows(2)
902 .map(|pair| points[pair[1]].sub(points[pair[0]]).length())
903 .sum();
904 let floor = (tolerance * 0.01).max(total * 1e-4);
905 let first = points[simplified[0]];
906 let last = points[simplified[simplified.len() - 1]];
907 let mut conditioned = vec![simplified[0]];
908 for index in &simplified[1..simplified.len() - 1] {
909 let point = points[*index];
910 if point.sub(first).length() > floor
911 && point.sub(last).length() > floor
912 && point.sub(points[*conditioned.last().unwrap()]).length() > floor
913 {
914 conditioned.push(*index);
915 }
916 }
917 conditioned.push(simplified[simplified.len() - 1]);
918 conditioned
919 } else {
920 let mut distinct: Vec<usize> = Vec::new();
921 for index in simplified {
922 if distinct.last().is_none_or(|previous: &usize| {
923 points[index].sub(points[*previous]).length() > tolerance * 0.01
924 }) {
925 distinct.push(index);
926 }
927 }
928 distinct
929 }
930}
931
932fn decimate_stations(pool: &[usize], maximum_points: usize) -> Vec<usize> {
934 if pool.len() <= maximum_points {
935 return pool.to_vec();
936 }
937 let step = (pool.len() - 1) as f64 / (maximum_points - 1) as f64;
938 (0..maximum_points)
939 .map(|index| pool[(index as f64 * step).round() as usize])
940 .collect()
941}
942
943fn station_parameters(points: &[Vec3], stations: &[usize]) -> Option<Vec<f64>> {
946 let total: f64 = stations
947 .windows(2)
948 .map(|pair| points[pair[1]].sub(points[pair[0]]).length())
949 .sum();
950 if total <= 0.0 {
951 return None;
952 }
953 let mut parameters = vec![0.0; stations.len()];
954 let mut accumulated = 0.0;
955 for index in 1..stations.len() {
956 accumulated += points[stations[index]]
957 .sub(points[stations[index - 1]])
958 .length();
959 parameters[index] = accumulated / total;
960 }
961 *parameters.last_mut().unwrap() = 1.0;
962 Some(parameters)
963}
964
965fn distance_to_segment(point: Vec3, from: Vec3, to: Vec3) -> f64 {
966 let direction = to.sub(from);
967 let length_squared = direction.length_squared();
968 if length_squared <= 0.0 {
969 return point.sub(from).length();
970 }
971 let fraction = (point.sub(from).dot(direction) / length_squared).clamp(0.0, 1.0);
972 point.sub(from.add(direction.scale(fraction))).length()
973}
974
975fn distance_to_curve_near(
984 curve: &NurbsCurve,
985 point: Vec3,
986 seed: f64,
987 lower: f64,
988 upper: f64,
989) -> Result<f64, String> {
990 let mut parameter = seed.clamp(lower, upper);
991 let mut best = f64::INFINITY;
992 for _ in 0..MEASURE_NEWTON_STEPS {
993 let derivatives = curve.derivatives_small(parameter, 2)?;
994 let residual = derivatives[0].sub(point);
995 best = best.min(residual.length_squared());
996 let slope = derivatives[1].dot(residual);
997 let curvature = derivatives[2].dot(residual) + derivatives[1].length_squared();
998 if curvature.abs() <= f64::MIN_POSITIVE {
999 break;
1000 }
1001 let next = (parameter - slope / curvature).clamp(lower, upper);
1002 if next == parameter {
1003 break;
1004 }
1005 parameter = next;
1006 }
1007 let residual = curve.evaluate(parameter)?.sub(point).length_squared();
1008 Ok(best.min(residual).sqrt())
1009}
1010
1011fn measure_polyline_fit(
1033 curve: &NurbsCurve,
1034 points: &[Vec3],
1035 stations: &[usize],
1036 parameters: &[f64],
1037 arc: &[f64],
1038) -> Result<(f64, f64, f64), String> {
1039 let mut worst = 0.0_f64;
1040 let mut sum_squares = 0.0_f64;
1041 let mut samples = 0usize;
1042 let mut between = 0.0_f64;
1043 let last = stations.len() - 1;
1044 for span in 0..last {
1045 let (from, to) = (stations[span], stations[span + 1]);
1046 let (t0, t1) = (parameters[span], parameters[span + 1]);
1047 let lower = parameters[span.saturating_sub(1)];
1050 let upper = parameters[(span + 2).min(last)];
1051 let length = arc[to] - arc[from];
1055 for index in from..to {
1056 let seed = if length > 0.0 {
1057 t0 + (t1 - t0) * (arc[index] - arc[from]) / length
1058 } else {
1059 t0
1060 };
1061 let distance = distance_to_curve_near(curve, points[index], seed, lower, upper)?;
1062 worst = worst.max(distance);
1063 sum_squares += distance * distance;
1064 samples += 1;
1065 }
1066 let window_start = from.saturating_sub(1);
1069 let window_end = (to + 1).min(points.len() - 1);
1070 for fraction in MEASURE_SPAN_FRACTIONS {
1071 let sample = curve.evaluate(t0 + (t1 - t0) * fraction)?;
1072 let mut nearest = f64::INFINITY;
1073 for index in window_start..window_end {
1074 nearest = nearest.min(distance_to_segment(
1075 sample,
1076 points[index],
1077 points[index + 1],
1078 ));
1079 }
1080 if nearest.is_finite() {
1081 between = between.max(nearest);
1082 }
1083 }
1084 }
1085 let distance = distance_to_curve_near(
1086 curve,
1087 points[stations[last]],
1088 parameters[last],
1089 parameters[last.saturating_sub(1)],
1090 parameters[last],
1091 )?;
1092 worst = worst.max(distance);
1093 sum_squares += distance * distance;
1094 samples += 1;
1095 let rms = (sum_squares / samples as f64).sqrt();
1096 Ok((worst, rms, between))
1097}
1098
1099fn dropped_band(points: &[Vec3], pool: &[usize]) -> f64 {
1103 let mut worst = 0.0_f64;
1104 for span in 0..pool.len() - 1 {
1105 let (from, to) = (pool[span], pool[span + 1]);
1106 for index in from + 1..to {
1107 worst = worst.max(distance_to_segment(
1108 points[index],
1109 points[from],
1110 points[to],
1111 ));
1112 }
1113 }
1114 worst
1115}
1116
1117fn cumulative_chord(points: &[Vec3]) -> Vec<f64> {
1119 let mut arc = vec![0.0; points.len()];
1120 for index in 1..points.len() {
1121 arc[index] = arc[index - 1] + points[index].sub(points[index - 1]).length();
1122 }
1123 arc
1124}
1125
1126fn fit_rung(
1128 points: &[Vec3],
1129 pool: &[usize],
1130 arc: &[f64],
1131 maximum_points: usize,
1132 local_interpolation: bool,
1133 degree: usize,
1134) -> Result<(NurbsCurve, Vec<usize>, Vec<f64>, f64, f64, f64), String> {
1135 let stations = decimate_stations(pool, maximum_points);
1136 fit_station_set(points, stations, arc, local_interpolation, degree)
1137}
1138
1139fn fit_station_set(
1140 points: &[Vec3],
1141 stations: Vec<usize>,
1142 arc: &[f64],
1143 local_interpolation: bool,
1144 degree: usize,
1145) -> Result<(NurbsCurve, Vec<usize>, Vec<f64>, f64, f64, f64), String> {
1146 let parameters = station_parameters(points, &stations)
1147 .ok_or_else(|| "fit_polyline: degenerate polyline".to_string())?;
1148 let kept = stations.iter().map(|index| points[*index]).collect::<Vec<_>>();
1149 let closed = kept.len() > degree + 1
1153 && kept[0].sub(kept[kept.len() - 1]).length() <= 1e-12 * (1.0 + kept[0].length());
1154 let curve = if local_interpolation {
1155 interpolate_curve_local(&kept, ¶meters, 1.0)?
1156 } else if closed
1157 && degree > 3
1158 && degree % 2 == 1
1159 && std::env::var("BREP_SECTION_FIT_PERIODIC").as_deref() != Ok("0")
1160 {
1161 interpolate_curve_closed_of_degree(&kept[..kept.len() - 1], ¶meters, degree)?
1162 } else {
1163 interpolate_curve(&kept, degree.min(kept.len() - 1), ¶meters)?
1164 };
1165 let (deviation, rms, between) =
1166 measure_polyline_fit(&curve, points, &stations, ¶meters, arc)?;
1167 Ok((curve, stations, parameters, deviation, rms, between))
1168}
1169
1170pub fn fit_polyline_at(
1178 points: &[Vec3],
1179 tolerance: f64,
1180 maximum_points: usize,
1181 local_interpolation: bool,
1182) -> Result<PolylineFit, String> {
1183 let fit = fit_polyline_inner(points, tolerance, maximum_points, local_interpolation, false, 3)?;
1184 record_polyline_fit(&fit.report);
1185 Ok(fit)
1186}
1187
1188pub fn fit_polyline(
1237 points: &[Vec3],
1238 tolerance: f64,
1239 maximum_points: usize,
1240 local_interpolation: bool,
1241) -> Result<PolylineFit, String> {
1242 fit_polyline_of_degree(points, tolerance, maximum_points, local_interpolation, 3)
1243}
1244
1245pub fn fit_polyline_of_degree(
1254 points: &[Vec3],
1255 tolerance: f64,
1256 maximum_points: usize,
1257 local_interpolation: bool,
1258 degree: usize,
1259) -> Result<PolylineFit, String> {
1260 fit_polyline_of_degree_with_recovery(points, tolerance, maximum_points, local_interpolation, degree)
1261 .map(|(fit, _)| fit)
1262}
1263
1264pub(crate) fn fit_polyline_of_degree_with_recovery(
1268 points: &[Vec3],
1269 tolerance: f64,
1270 maximum_points: usize,
1271 local_interpolation: bool,
1272 degree: usize,
1273) -> Result<(PolylineFit, bool), String> {
1274 let refine = std::env::var("BREP_POLYLINE_FIT_REFINE").as_deref() != Ok("0");
1275 let asked = fit_polyline_inner(points, tolerance, maximum_points, local_interpolation, refine, degree)?;
1276 if asked.report.met_tolerance() || !refine {
1277 record_polyline_fit(&asked.report);
1278 return Ok((asked, false));
1279 }
1280 let mut other =
1281 fit_polyline_inner(points, tolerance, maximum_points, !local_interpolation, refine, degree)?;
1282 let spent = asked.report.rungs + other.report.rungs;
1283 let mut best = if other.report.deviation < asked.report.deviation {
1284 other.report.lane_switched = true;
1285 other
1286 } else {
1287 asked
1288 };
1289 let mut spent = spent;
1294 let mut recovered = false;
1295 if !best.report.met_tolerance() {
1296 let (adaptive, rounds) = fit_polyline_adaptive(points, tolerance, maximum_points, degree)?;
1297 spent += rounds;
1298 if let Some(mut adaptive) = adaptive {
1299 adaptive.report.lane_switched = local_interpolation;
1300 best = adaptive;
1301 recovered = true;
1302 }
1303 }
1304 best.report.rungs = spent;
1306 record_polyline_fit(&best.report);
1307 Ok((best, recovered))
1308}
1309
1310fn fit_polyline_adaptive(
1314 points: &[Vec3],
1315 tolerance: f64,
1316 maximum_points: usize,
1317 degree: usize,
1318) -> Result<(Option<PolylineFit>, usize), String> {
1319 let pool = station_pool(points, tolerance);
1320 let arc = cumulative_chord(points);
1321 let band = dropped_band(points, &pool);
1322 let floor = tolerance.max(band);
1323 let budget = maximum_points.max(2);
1324 let ceiling = pool.len().min(MAX_FIT_STATIONS)
1325 .min(budget.saturating_mul(LADDER_BUDGET_FACTOR)).max(budget);
1326 let mut stations = decimate_stations(&pool, budget);
1327 for round in 0..=8 {
1330 let Ok((curve, current, parameters, deviation, rms, between)) =
1331 fit_station_set(points, stations, &arc, false, degree)
1332 else { return Ok((None, round + 1)); };
1333 stations = current;
1334 if deviation <= floor {
1335 let report = PolylineFitReport {
1336 deviation, rms, between_stations: between, tolerance, floor,
1337 stations: stations.len(), controls: curve.control_points.len(),
1338 input_points: points.len(), pool_points: pool.len(), dropped_band: band,
1339 rungs: round + 1, local_lane: false, lane_switched: false,
1340 exit: PolylineFitExit::Converged,
1341 };
1342 return Ok((Some(PolylineFit {
1343 curve, parameters, kept: stations.iter().map(|&i| points[i]).collect(), report,
1344 }), round + 1));
1345 }
1346 if stations.len() >= ceiling || round == 8 { return Ok((None, round + 1)); }
1347 let mut candidates = Vec::new();
1348 for span in 0..stations.len() - 1 {
1349 let (from, to) = (stations[span], stations[span + 1]);
1350 let (t0, t1) = (parameters[span], parameters[span + 1]);
1351 let lower = parameters[span.saturating_sub(1)];
1352 let upper = parameters[(span + 2).min(stations.len() - 1)];
1353 let mut candidate: Option<(usize, f64)> = None;
1354 for &index in pool.iter().filter(|&&i| i > from && i < to) {
1355 let length = arc[to] - arc[from];
1356 let seed = if length > 0.0 {
1357 t0 + (t1 - t0) * (arc[index] - arc[from]) / length
1358 } else { t0 };
1359 let error = distance_to_curve_near(&curve, points[index], seed, lower, upper)?;
1360 if candidate.is_none_or(|(_, previous)| error > previous) {
1361 candidate = Some((index, error));
1362 }
1363 }
1364 if let Some((index, error)) = candidate {
1365 if error > floor { candidates.push((index, error)); }
1366 }
1367 }
1368 candidates.sort_by(|a, b| b.1.total_cmp(&a.1).then_with(|| a.0.cmp(&b.0)));
1369 let added = candidates.len().min(16).min(ceiling - stations.len());
1370 if added == 0 { return Ok((None, round + 1)); }
1371 stations.extend(candidates.iter().take(added).map(|&(index, _)| index));
1372 stations.sort_unstable();
1373 }
1374 unreachable!("the bounded refinement returns on its last round")
1375}
1376
1377fn fit_polyline_inner(
1378 points: &[Vec3],
1379 tolerance: f64,
1380 maximum_points: usize,
1381 local_interpolation: bool,
1382 refine: bool,
1383 degree: usize,
1384) -> Result<PolylineFit, String> {
1385 let pool = station_pool(points, tolerance);
1386 if pool.len() < 2 {
1387 return Err("fit_polyline: degenerate polyline".into());
1388 }
1389 let arc = cumulative_chord(points);
1390 let band = dropped_band(points, &pool);
1391 let floor = tolerance.max(band);
1392 let mut budget = maximum_points.max(2);
1393 let ceiling = pool
1394 .len()
1395 .min(MAX_FIT_STATIONS)
1396 .min(budget.saturating_mul(LADDER_BUDGET_FACTOR))
1397 .max(budget);
1398 let (mut curve, mut stations, mut parameters, mut deviation, mut rms, mut between) =
1399 fit_rung(points, &pool, &arc, budget, local_interpolation, degree)?;
1400 let mut rungs = 1usize;
1401 let mut exit = if deviation <= floor {
1402 PolylineFitExit::Converged
1403 } else {
1404 PolylineFitExit::Unrefined
1405 };
1406 while refine && deviation > floor {
1407 if budget >= ceiling {
1408 exit = if ceiling >= pool.len() {
1409 PolylineFitExit::InputExhausted
1410 } else {
1411 PolylineFitExit::StationCeiling
1412 };
1413 break;
1414 }
1415 budget = (budget * 2).min(ceiling);
1416 let rung = fit_rung(points, &pool, &arc, budget, local_interpolation, degree)?;
1417 rungs += 1;
1418 if rung.3 >= deviation * STALL_FACTOR {
1419 if rung.3 < deviation {
1420 (curve, stations, parameters, deviation, rms, between) = rung;
1421 }
1422 exit = if deviation <= floor {
1426 PolylineFitExit::Converged
1427 } else {
1428 PolylineFitExit::Stalled
1429 };
1430 break;
1431 }
1432 (curve, stations, parameters, deviation, rms, between) = rung;
1433 if deviation <= floor {
1434 exit = PolylineFitExit::Converged;
1435 }
1436 }
1437 let report = PolylineFitReport {
1438 deviation,
1439 rms,
1440 between_stations: between,
1441 tolerance,
1442 floor,
1443 stations: stations.len(),
1444 controls: curve.control_points.len(),
1445 input_points: points.len(),
1446 pool_points: pool.len(),
1447 dropped_band: band,
1448 rungs,
1449 local_lane: local_interpolation,
1450 lane_switched: false,
1451 exit,
1452 };
1453 Ok(PolylineFit {
1454 curve,
1455 parameters,
1456 kept: stations.iter().map(|index| points[*index]).collect(),
1457 report,
1458 })
1459}
1460
1461#[derive(Clone, Copy, Debug, Default, PartialEq)]
1474pub struct PolylineFitLedger {
1475 pub fits: u64,
1477 pub unmet: u64,
1479 pub station_ceiling: u64,
1481 pub input_exhausted: u64,
1483 pub stalled: u64,
1485 pub unrefined: u64,
1487 pub refined: u64,
1489 pub met_at_pool: u64,
1492 pub lane_switched: u64,
1495 pub extra_rungs: u64,
1497 pub worst_unmet_deviation: f64,
1499 pub worst_dropped_band: f64,
1501}
1502
1503impl PolylineFitLedger {
1504 fn record(&mut self, report: &PolylineFitReport) {
1505 self.fits += 1;
1506 self.extra_rungs += (report.rungs - 1) as u64;
1507 if report.rungs > 1 {
1508 self.refined += 1;
1509 }
1510 self.worst_dropped_band = self.worst_dropped_band.max(report.dropped_band);
1511 if report.lane_switched {
1512 self.lane_switched += 1;
1513 }
1514 if report.met_tolerance() {
1515 if report.saturated() {
1516 self.met_at_pool += 1;
1517 }
1518 return;
1519 }
1520 self.unmet += 1;
1521 match report.exit {
1522 PolylineFitExit::Converged => {}
1523 PolylineFitExit::StationCeiling => self.station_ceiling += 1,
1524 PolylineFitExit::InputExhausted => self.input_exhausted += 1,
1525 PolylineFitExit::Stalled => self.stalled += 1,
1526 PolylineFitExit::Unrefined => self.unrefined += 1,
1527 }
1528 self.worst_unmet_deviation = self.worst_unmet_deviation.max(report.deviation);
1529 }
1530
1531 fn fold(&mut self, child: &PolylineFitLedger) {
1532 self.fits += child.fits;
1533 self.unmet += child.unmet;
1534 self.station_ceiling += child.station_ceiling;
1535 self.input_exhausted += child.input_exhausted;
1536 self.stalled += child.stalled;
1537 self.unrefined += child.unrefined;
1538 self.refined += child.refined;
1539 self.met_at_pool += child.met_at_pool;
1540 self.lane_switched += child.lane_switched;
1541 self.extra_rungs += child.extra_rungs;
1542 self.worst_unmet_deviation = self.worst_unmet_deviation.max(child.worst_unmet_deviation);
1543 self.worst_dropped_band = self.worst_dropped_band.max(child.worst_dropped_band);
1544 }
1545
1546 pub fn report_into(&self, diagnostics: &mut KernelDiagnostics) {
1553 diagnostics.count_n("fit.polyline.fits", self.fits);
1554 diagnostics.count_n("fit.polyline.unmet_tolerance", self.unmet);
1555 diagnostics.count_n("fit.polyline.exit.station_ceiling", self.station_ceiling);
1556 diagnostics.count_n("fit.polyline.exit.input_exhausted", self.input_exhausted);
1557 diagnostics.count_n("fit.polyline.exit.stalled", self.stalled);
1558 diagnostics.count_n("fit.polyline.exit.unrefined", self.unrefined);
1559 diagnostics.count_n("fit.polyline.refined", self.refined);
1560 diagnostics.count_n("fit.polyline.met_at_pool", self.met_at_pool);
1561 diagnostics.count_n("fit.polyline.lane_switched", self.lane_switched);
1562 diagnostics.count_n("fit.polyline.extra_rungs", self.extra_rungs);
1563 diagnostics.measure_max("fit.polyline.worst_unmet_deviation", self.worst_unmet_deviation);
1564 diagnostics.measure_max("fit.polyline.worst_dropped_band", self.worst_dropped_band);
1565 if self.unmet > 0 {
1566 diagnostics.event(
1567 DiagnosticSeverity::Degraded,
1568 KernelStage::Intersect,
1569 "fit.polyline.unmet_tolerance",
1570 format!(
1571 "{} of {} polyline fits did not reach the tolerance they were given \
1572 (station ceiling {}, input exhausted {}, stalled {}, unrefined {}); \
1573 worst deviation {:.3e}; the simplification's own band reaches {:.3e}",
1574 self.unmet,
1575 self.fits,
1576 self.station_ceiling,
1577 self.input_exhausted,
1578 self.stalled,
1579 self.unrefined,
1580 self.worst_unmet_deviation,
1581 self.worst_dropped_band,
1582 ),
1583 );
1584 }
1585 }
1586}
1587
1588thread_local! {
1589 static POLYLINE_FIT_SCOPES: std::cell::RefCell<Vec<PolylineFitLedger>> =
1591 const { std::cell::RefCell::new(Vec::new()) };
1592}
1593
1594pub struct PolylineFitScope {
1601 _thread_bound: std::marker::PhantomData<*const ()>,
1603}
1604
1605impl PolylineFitScope {
1606 pub fn open() -> Self {
1607 POLYLINE_FIT_SCOPES.with(|scopes| scopes.borrow_mut().push(PolylineFitLedger::default()));
1608 Self {
1609 _thread_bound: std::marker::PhantomData,
1610 }
1611 }
1612
1613 pub fn close(self) -> PolylineFitLedger {
1615 let ledger = POLYLINE_FIT_SCOPES.with(|scopes| {
1616 let mut scopes = scopes.borrow_mut();
1617 let ledger = scopes.pop().unwrap_or_default();
1618 if let Some(parent) = scopes.last_mut() {
1619 parent.fold(&ledger);
1620 }
1621 ledger
1622 });
1623 std::mem::forget(self);
1624 ledger
1625 }
1626}
1627
1628impl Drop for PolylineFitScope {
1629 fn drop(&mut self) {
1630 POLYLINE_FIT_SCOPES.with(|scopes| {
1631 scopes.borrow_mut().pop();
1632 });
1633 }
1634}
1635
1636fn record_polyline_fit(report: &PolylineFitReport) {
1637 if std::env::var_os("BREP_DEBUG_POLYLINE_FIT").is_some() {
1640 eprintln!("POLYLINE-FIT {report:?}");
1641 }
1642 POLYLINE_FIT_SCOPES.with(|scopes| {
1643 if let Some(open) = scopes.borrow_mut().last_mut() {
1644 open.record(report);
1645 }
1646 });
1647}
1648
1649
1650
1651fn raw_basis(knots: &[f64], degree: usize, span: usize, parameter: f64) -> Vec<f64> {
1655 let mut basis = vec![0.0; degree + 1];
1656 let mut left = vec![0.0; degree + 1];
1657 let mut right = vec![0.0; degree + 1];
1658 basis[0] = 1.0;
1659 for j in 1..=degree {
1660 left[j] = parameter - knots[span + 1 - j];
1661 right[j] = knots[span + j] - parameter;
1662 let mut saved = 0.0;
1663 for r in 0..j {
1664 let denominator = right[r + 1] + left[j - r];
1665 let temp = if denominator.abs() > 0.0 {
1666 basis[r] / denominator
1667 } else {
1668 0.0
1669 };
1670 basis[r] = saved + right[r + 1] * temp;
1671 saved = left[j - r] * temp;
1672 }
1673 basis[j] = saved;
1674 }
1675 basis
1676}
1677
1678fn raw_insert_knot(knots: &mut Vec<f64>, controls: &mut Vec<Vec3>, degree: usize, parameter: f64) {
1680 let span = knots
1684 .iter()
1685 .rposition(|&knot| knot <= parameter + 1e-14)
1686 .unwrap()
1687 .min(controls.len() - 1);
1688 let mut fresh = Vec::with_capacity(controls.len() + 1);
1689 fresh.extend_from_slice(&controls[..=span - degree]);
1690 for i in span - degree + 1..=span {
1691 let denominator = knots[i + degree] - knots[i];
1692 let alpha = if denominator.abs() > 0.0 {
1693 (parameter - knots[i]) / denominator
1694 } else {
1695 0.0
1696 };
1697 fresh.push(
1698 controls[i - 1]
1699 .scale(1.0 - alpha)
1700 .add(controls[i].scale(alpha)),
1701 );
1702 }
1703 fresh.extend_from_slice(&controls[span..]);
1704 *controls = fresh;
1705 knots.insert(span + 1, parameter);
1706}
1707
1708pub fn interpolate_curve_closed(points: &[Vec3], parameters: &[f64]) -> Result<NurbsCurve, String> {
1716 interpolate_curve_closed_of_degree(points, parameters, 3)
1717}
1718
1719pub fn interpolate_curve_closed_of_degree(
1725 points: &[Vec3],
1726 parameters: &[f64],
1727 degree: usize,
1728) -> Result<NurbsCurve, String> {
1729 if degree % 2 == 0 || degree < 3 {
1730 return Err("interpolate_curve_closed: the degree must be odd and at least 3".into());
1731 }
1732 let station_count = points.len();
1733 if station_count < degree + 1 {
1734 return Err("interpolate_curve_closed: need more stations than the degree".into());
1735 }
1736 if parameters.len() != station_count + 1 {
1737 return Err(
1738 "interpolate_curve_closed: parameters must have one more entry than points".into(),
1739 );
1740 }
1741 if parameters.windows(2).any(|pair| pair[1] <= pair[0]) {
1742 return Err("interpolate_curve_closed: parameters must increase".into());
1743 }
1744 let period = parameters[station_count] - parameters[0];
1745 let cyclic = |j: i64| -> f64 {
1748 let s = station_count as i64;
1749 let wrap = j.div_euclid(s);
1750 parameters[j.rem_euclid(s) as usize] + wrap as f64 * period
1751 };
1752 let raw_knots: Vec<f64> = (-(degree as i64)..=(station_count as i64 + degree as i64)).map(cyclic).collect();
1753 let mut matrix = vec![vec![0.0; station_count]; station_count];
1756 for i in 0..station_count {
1757 let span = i + degree;
1758 let basis = raw_basis(&raw_knots, degree, span, parameters[i]);
1759 for (offset, value) in basis.iter().enumerate() {
1760 let index = (i as i64 + offset as i64 - degree as i64).rem_euclid(station_count as i64) as usize;
1763 matrix[i][index] += value;
1764 }
1765 }
1766 let solve_axis = |axis: fn(Vec3) -> f64| {
1767 solve_dense(
1768 matrix.clone(),
1769 points.iter().copied().map(axis).collect::<Vec<_>>(),
1770 )
1771 };
1772 let xs = solve_axis(|point| point.x)?;
1773 let ys = solve_axis(|point| point.y)?;
1774 let zs = solve_axis(|point| point.z)?;
1775 let cyclic_controls: Vec<Vec3> = (0..station_count)
1776 .map(|index| Vec3::new(xs[index], ys[index], zs[index]))
1777 .collect();
1778 let mut window_controls: Vec<Vec3> = (-(degree as i64)..(station_count as i64))
1780 .map(|j| cyclic_controls[j.rem_euclid(station_count as i64) as usize])
1781 .collect();
1782 let mut window_knots = raw_knots.clone();
1783 for _ in 0..degree {
1786 raw_insert_knot(
1787 &mut window_knots,
1788 &mut window_controls,
1789 degree,
1790 parameters[0],
1791 );
1792 }
1793 for _ in 0..degree {
1794 raw_insert_knot(
1795 &mut window_knots,
1796 &mut window_controls,
1797 degree,
1798 parameters[station_count],
1799 );
1800 }
1801 let first = window_knots
1805 .iter()
1806 .position(|&knot| (knot - parameters[0]).abs() < 1e-12)
1807 .ok_or("interpolate_curve_closed: clamp lost the start knot")?;
1808 let last = window_knots
1809 .iter()
1810 .rposition(|&knot| (knot - parameters[station_count]).abs() < 1e-12)
1811 .ok_or("interpolate_curve_closed: clamp lost the end knot")?;
1812 let clamped_knots: Vec<f64> = window_knots[first..=last].to_vec();
1813 let control_count = clamped_knots.len() - degree - 1;
1814 let clamped_controls: Vec<Vec4> = window_controls[first..first + control_count]
1815 .iter()
1816 .map(|point| Vec4::from_point(*point, 1.0))
1817 .collect();
1818 NurbsCurve::new(degree, clamped_knots, clamped_controls)
1819}
1820