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
255fn solve_collocation_banded_axes(
259 band: &CollocationBand,
260 mut rhs: [Vec<f64>; 3],
261) -> Result<[Vec<f64>; 3], String> {
262 let original = rhs.clone();
263 let count = band.count;
264 let bandwidth = band.bandwidth;
265 let width = 2 * bandwidth + 1;
266 let at = |row: usize, column: usize| row * width + column + bandwidth - row;
267 let fallback = || Ok([
268 solve_collocation_banded(band, &original[0])?,
269 solve_collocation_banded(band, &original[1])?,
270 solve_collocation_banded(band, &original[2])?,
271 ]);
272 if count == 0 || rhs.iter().any(|axis| axis.len() != count) {
273 return fallback();
274 }
275 let mut matrix = band.data.clone();
276 for column in 0..count {
277 let pivot = matrix[at(column, column)];
278 if pivot.abs() <= 1e-300 { return fallback(); }
279 let maximum_row = (column + bandwidth).min(count - 1);
280 for row in column + 1..=maximum_row {
281 let factor = matrix[at(row, column)] / pivot;
282 if factor == 0.0 { continue; }
283 let maximum_column = (column + bandwidth).min(count - 1);
284 for entry in column..=maximum_column {
285 matrix[at(row, entry)] -= factor * matrix[at(column, entry)];
286 }
287 for axis in &mut rhs {
288 axis[row] -= factor * axis[column];
289 }
290 }
291 }
292 let mut solutions = std::array::from_fn(|_| vec![0.0; count]);
293 for axis in 0..3 {
294 for row in (0..count).rev() {
295 let mut value = rhs[axis][row];
296 let maximum_column = (row + bandwidth).min(count - 1);
297 for column in row + 1..=maximum_column {
298 value -= matrix[at(row, column)] * solutions[axis][column];
299 }
300 if matrix[at(row, row)].abs() <= 1e-300 { return fallback(); }
301 solutions[axis][row] = value / matrix[at(row, row)];
302 }
303 let scale = original[axis].iter().fold(0.0_f64, |m, v| m.max(v.abs()));
304 let tolerance = 1e-9 * scale.max(1.0);
305 let mut residual = 0.0_f64;
306 for row in 0..count {
307 let lo = row.saturating_sub(bandwidth);
308 let hi = (row + bandwidth).min(count - 1);
309 let mut accumulated = 0.0;
310 for col in lo..=hi {
311 accumulated += band.get(row, col) * solutions[axis][col];
312 }
313 residual = residual.max((accumulated - original[axis][row]).abs());
314 }
315 if residual > tolerance {
316 solutions[axis] = solve_dense(band.to_dense(), original[axis].clone())?;
317 }
318 }
319 Ok(solutions)
320}
321
322fn assert_interpolant_bits(before: &NurbsCurve, after: &NurbsCurve) {
323 let bits = |c: &NurbsCurve| (c.degree,
324 c.knots.iter().map(|v| v.to_bits()).collect::<Vec<_>>(),
325 c.control_points.iter().map(|p| (p.x.to_bits(), p.y.to_bits(), p.z.to_bits(), p.w.to_bits())).collect::<Vec<_>>());
326 assert_eq!(bits(before), bits(after), "shared interpolant elimination moved a coefficient");
327}
328
329pub fn solve_banded(
330 matrix: &[Vec<f64>],
331 rhs: &[f64],
332 bandwidth: usize,
333) -> Result<Vec<f64>, String> {
334 let count = rhs.len();
335 if count == 0 || matrix.len() != count || matrix.iter().any(|row| row.len() != count) {
336 return Err("solve_banded: matrix must be square and match RHS".into());
337 }
338 let mut matrix = matrix.to_vec();
339 let mut rhs = rhs.to_vec();
340 for column in 0..count {
341 let pivot = matrix[column][column];
342 if pivot.abs() <= 1e-300 {
343 return Err("solve_banded: singular matrix".into());
344 }
345 let maximum_row = (column + bandwidth).min(count - 1);
346 for row in column + 1..=maximum_row {
347 let factor = matrix[row][column] / pivot;
348 if factor == 0.0 {
349 continue;
350 }
351 let maximum_column = (column + bandwidth).min(count - 1);
352 for entry in column..=maximum_column {
353 matrix[row][entry] -= factor * matrix[column][entry];
354 }
355 rhs[row] -= factor * rhs[column];
356 }
357 }
358 let mut result = vec![0.0; count];
359 for row in (0..count).rev() {
360 let mut value = rhs[row];
361 let maximum_column = (row + bandwidth).min(count - 1);
362 for column in row + 1..=maximum_column {
363 value -= matrix[row][column] * result[column];
364 }
365 if matrix[row][row].abs() <= 1e-300 {
366 return Err("solve_banded: singular matrix".into());
367 }
368 result[row] = value / matrix[row][row];
369 }
370 Ok(result)
371}
372
373pub fn interpolate_homogeneous(
380 points: &[Vec4],
381 degree: usize,
382 parameters: &[f64],
383) -> Result<NurbsCurve, String> {
384 if points.len() < 2 || parameters.len() != points.len() {
385 return Err(
386 "interpolate_homogeneous: points and parameters must have matching length >= 2".into(),
387 );
388 }
389 if parameters.windows(2).any(|pair| pair[1] <= pair[0]) {
390 return Err("interpolate_homogeneous: parameters must be strictly increasing".into());
391 }
392 let n = points.len() - 1;
393 let degree = degree.min(n);
394 let m = n + degree + 1;
395 let mut knots = vec![0.0; m + 1];
396 for knot in &mut knots[m - degree..=m] {
397 *knot = 1.0;
398 }
399 for j in 1..=n.saturating_sub(degree) {
400 knots[j + degree] = parameters[j..j + degree].iter().sum::<f64>() / degree as f64;
401 }
402 let knot_vector = KnotVector::new(knots.clone(), degree)?;
403 let mut matrix = vec![vec![0.0; n + 1]; n + 1];
404 for (row, ¶meter) in parameters.iter().enumerate() {
405 let span = knot_vector.find_span(parameter);
406 let basis = knot_vector.basis_functions(span, parameter);
407 for (offset, value) in basis.into_iter().enumerate() {
408 matrix[row][span - degree + offset] = value;
409 }
410 }
411 let solve_axis = |axis: fn(&Vec4) -> f64| {
412 solve_collocation(
413 &matrix,
414 &points.iter().map(axis).collect::<Vec<_>>(),
415 degree,
416 )
417 };
418 let xs = solve_axis(|point| point.x)?;
419 let ys = solve_axis(|point| point.y)?;
420 let zs = solve_axis(|point| point.z)?;
421 let ws = solve_axis(|point| point.w)?;
422 NurbsCurve::new(
423 degree,
424 knots,
425 (0..=n)
426 .map(|index| Vec4 {
427 x: xs[index],
428 y: ys[index],
429 z: zs[index],
430 w: ws[index],
431 })
432 .collect(),
433 )
434}
435
436pub fn interpolate_curve(
452 points: &[Vec3],
453 degree: usize,
454 parameters: &[f64],
455) -> Result<NurbsCurve, String> {
456 let option = std::env::var("BREP_INTERPOLANT_AXIS_REUSE").unwrap_or_default();
457 let enabled = matches!(option.as_str(), "on" | "1" | "check");
458 if option == "check" {
459 let before = interpolate_curve_with_axis_reuse(points, degree, parameters, false);
460 let after = interpolate_curve_with_axis_reuse(points, degree, parameters, true);
461 match (&before, &after) {
462 (Ok(a), Ok(b)) => assert_interpolant_bits(a, b),
463 (Err(a), Err(b)) => assert_eq!(a, b),
464 _ => panic!("shared interpolant elimination changed the result"),
465 }
466 eprintln!("BREP_INTERPOLANT_AXIS_CENSUS stations={} matched=true", points.len());
467 return before;
468 }
469 interpolate_curve_with_axis_reuse(points, degree, parameters, enabled)
470}
471
472fn interpolate_curve_with_axis_reuse(
473 points: &[Vec3],
474 degree: usize,
475 parameters: &[f64],
476 reuse_axes: bool,
477) -> Result<NurbsCurve, String> {
478 if points.len() < 2 || parameters.len() != points.len() {
479 return Err(
480 "interpolate_curve: points and parameters must have matching length >= 2".into(),
481 );
482 }
483 if parameters.windows(2).any(|pair| pair[1] <= pair[0]) {
484 return Err("interpolate_curve: parameters must be strictly increasing".into());
485 }
486 let n = points.len() - 1;
487 let degree = degree.min(n);
488 let m = n + degree + 1;
489 let mut knots = vec![parameters[0]; m + 1];
490 for knot in &mut knots[m - degree..=m] {
491 *knot = parameters[n];
492 }
493 for j in 1..=n.saturating_sub(degree) {
494 knots[j + degree] = parameters[j..j + degree].iter().sum::<f64>() / degree as f64;
495 }
496 let knot_vector = KnotVector::new(knots.clone(), degree)?;
497 let mut band = CollocationBand::new(n + 1, degree);
500 let mut dense: Option<Vec<Vec<f64>>> = None;
501 for (row, ¶meter) in parameters.iter().enumerate() {
502 let span = knot_vector.find_span(parameter);
503 let basis = knot_vector.basis_functions(span, parameter);
504 for (offset, value) in basis.into_iter().enumerate() {
505 let column = span - degree + offset;
506 if !band.set(row, column, value) {
507 dense.get_or_insert_with(|| band.to_dense())[row][column] = value;
508 } else if let Some(dense) = dense.as_mut() {
509 dense[row][column] = value;
510 }
511 }
512 }
513 let solve_axis = |axis: fn(Vec3) -> f64| {
514 let rhs = points.iter().copied().map(axis).collect::<Vec<_>>();
515 match &dense {
516 Some(matrix) => solve_collocation(matrix, &rhs, degree),
517 None => solve_collocation_banded(&band, &rhs),
518 }
519 };
520 let [xs, ys, zs] = if reuse_axes && dense.is_none() {
521 solve_collocation_banded_axes(&band, [
522 points.iter().map(|p| p.x).collect(),
523 points.iter().map(|p| p.y).collect(),
524 points.iter().map(|p| p.z).collect(),
525 ])?
526 } else {
527 [solve_axis(|point| point.x)?, solve_axis(|point| point.y)?, solve_axis(|point| point.z)?]
528 };
529 NurbsCurve::new(
530 degree,
531 knots,
532 (0..=n)
533 .map(|index| Vec4::from_point(Vec3::new(xs[index], ys[index], zs[index]), 1.0))
534 .collect(),
535 )
536}
537
538pub fn interpolate_curve_with_end_tangents(
545 points: &[Vec3],
546 parameters: &[f64],
547 start_tangent: Vec3,
548 end_tangent: Vec3,
549) -> Result<NurbsCurve, String> {
550 if points.len() < 2 || parameters.len() != points.len() {
551 return Err(
552 "interpolate_curve_with_end_tangents: points and parameters must match, >= 2".into(),
553 );
554 }
555 if parameters.windows(2).any(|pair| pair[1] <= pair[0]) {
556 return Err("interpolate_curve_with_end_tangents: parameters must increase".into());
557 }
558 let degree = 3usize;
559 let n = points.len() - 1;
560 let control_count = n + 3;
561 let t0 = parameters[0];
562 let t1 = parameters[n];
563 let mut knots = vec![t0; degree + 1];
566 for j in 0..n.saturating_sub(1) {
567 let window = ¶meters[j + 1..(j + degree).min(n) + 1];
568 knots.push(window.iter().sum::<f64>() / window.len() as f64);
569 }
570 knots.extend(std::iter::repeat(t1).take(degree + 1));
571 if knots.len() != control_count + degree + 1 {
572 return Err(format!(
573 "interpolate_curve_with_end_tangents: internal knot count {} for {} controls",
574 knots.len(),
575 control_count
576 ));
577 }
578 let knot_vector = KnotVector::new(knots.clone(), degree)?;
579 let mut matrix = vec![vec![0.0; control_count]; control_count];
580 let mut rhs_points = vec![Vec3::default(); control_count];
581 matrix[0][0] = 1.0;
585 rhs_points[0] = points[0];
586 let start_span = knots[degree + 1] - t0;
587 matrix[1][0] = -(degree as f64) / start_span;
588 matrix[1][1] = (degree as f64) / start_span;
589 rhs_points[1] = start_tangent;
590 for (index, ¶meter) in parameters.iter().enumerate().take(n).skip(1) {
591 let row = index + 1;
592 let span = knot_vector.find_span(parameter);
593 let basis = knot_vector.basis_functions(span, parameter);
594 for (offset, value) in basis.into_iter().enumerate() {
595 matrix[row][span - degree + offset] = value;
596 }
597 rhs_points[row] = points[index];
598 }
599 let end_span = t1 - knots[control_count - 1];
600 matrix[control_count - 2][control_count - 2] = -(degree as f64) / end_span;
601 matrix[control_count - 2][control_count - 1] = (degree as f64) / end_span;
602 rhs_points[control_count - 2] = end_tangent;
603 matrix[control_count - 1][control_count - 1] = 1.0;
604 rhs_points[control_count - 1] = points[n];
605 let solve_axis = |axis: fn(Vec3) -> f64| {
606 solve_dense(
607 matrix.clone(),
608 rhs_points.iter().copied().map(axis).collect::<Vec<_>>(),
609 )
610 };
611 let xs = solve_axis(|point| point.x)?;
612 let ys = solve_axis(|point| point.y)?;
613 let zs = solve_axis(|point| point.z)?;
614 NurbsCurve::new(
615 degree,
616 knots,
617 (0..control_count)
618 .map(|index| Vec4::from_point(Vec3::new(xs[index], ys[index], zs[index]), 1.0))
619 .collect(),
620 )
621}
622
623pub fn interpolate_curve_thinned(
624 points: &[Vec3],
625 degree: usize,
626 maximum_points: usize,
627) -> Result<NurbsCurve, String> {
628 if maximum_points < 2 {
629 return Err("interpolate_curve_thinned: maximum point count must be at least 2".into());
630 }
631 if points.len() <= maximum_points {
632 let parameters = chord_parameters(points);
633 return interpolate_curve(points, degree, ¶meters);
634 }
635 let step = (points.len() - 1) as f64 / (maximum_points - 1) as f64;
636 let thinned = (0..maximum_points)
637 .map(|index| points[(index as f64 * step).round() as usize])
638 .collect::<Vec<_>>();
639 let parameters = chord_parameters(&thinned);
640 interpolate_curve(&thinned, degree, ¶meters)
641}
642
643fn chord_parameters(points: &[Vec3]) -> Vec<f64> {
644 if points.len() < 2 {
645 return vec![0.0; points.len()];
646 }
647 let mut parameters = vec![0.0; points.len()];
648 for index in 1..points.len() {
649 parameters[index] = parameters[index - 1] + points[index].sub(points[index - 1]).length();
650 }
651 let length = parameters[points.len() - 1];
652 if length <= 1e-15 {
653 for (index, parameter) in parameters.iter_mut().enumerate() {
654 *parameter = index as f64 / (points.len() - 1) as f64;
655 }
656 } else {
657 for parameter in &mut parameters {
658 *parameter /= length;
659 }
660 }
661 parameters
662}
663
664pub fn simplify_polyline_indices(points: &[Vec3], tolerance: f64) -> Vec<usize> {
673 if points.len() <= 2 {
674 return (0..points.len()).collect();
675 }
676 let mut keep = vec![false; points.len()];
677 keep[0] = true;
678 keep[points.len() - 1] = true;
679 let mut stack = vec![(0usize, points.len() - 1)];
680 while let Some((start, end)) = stack.pop() {
681 if end - start < 2 {
682 continue;
683 }
684 let a = points[start];
685 let direction = points[end].sub(a);
686 let length_squared = direction.length_squared().max(1e-300);
687 let mut worst = None;
688 let mut worst_distance = tolerance;
689 for (index, point) in points.iter().enumerate().take(end).skip(start + 1) {
690 let fraction = point.sub(a).dot(direction) / length_squared;
691 let fraction = fraction.clamp(0.0, 1.0);
692 let distance = point.sub(a.add(direction.scale(fraction))).length();
693 if distance > worst_distance {
694 worst_distance = distance;
695 worst = Some(index);
696 }
697 }
698 if let Some(index) = worst {
699 keep[index] = true;
700 stack.push((start, index));
701 stack.push((index, end));
702 }
703 }
704 keep.into_iter()
705 .enumerate()
706 .filter_map(|(index, keep)| keep.then_some(index))
707 .collect()
708}
709
710pub fn simplify_polyline(points: &[Vec3], tolerance: f64) -> Vec<Vec3> {
711 simplify_polyline_indices(points, tolerance)
712 .into_iter()
713 .map(|index| points[index])
714 .collect()
715}
716
717pub fn interpolate_curve_local(
718 points: &[Vec3],
719 parameters: &[f64],
720 tension: f64,
721) -> Result<NurbsCurve, String> {
722 if points.len() != parameters.len() || points.len() < 2 {
723 return Err(
724 "interpolate_curve_local: points and parameters must have matching length >= 2".into(),
725 );
726 }
727 if points.len() == 2 {
728 return interpolate_curve(points, 1, parameters);
729 }
730 let count = points.len();
731 let mut tangents = vec![Vec3::default(); count];
732 tangents[0] = points[1]
733 .sub(points[0])
734 .scale(1.0 / (parameters[1] - parameters[0]));
735 tangents[count - 1] = points[count - 1]
736 .sub(points[count - 2])
737 .scale(1.0 / (parameters[count - 1] - parameters[count - 2]));
738 for index in 1..count - 1 {
739 let previous_interval = parameters[index] - parameters[index - 1];
740 let next_interval = parameters[index + 1] - parameters[index];
741 let total = previous_interval + next_interval;
742 let previous_secant = points[index]
743 .sub(points[index - 1])
744 .scale(1.0 / previous_interval);
745 let next_secant = points[index + 1]
746 .sub(points[index])
747 .scale(1.0 / next_interval);
748 let mut tangent =
749 points[index - 1]
750 .scale(-next_interval / (previous_interval * total))
751 .add(points[index].scale(
752 (next_interval - previous_interval) / (previous_interval * next_interval),
753 ))
754 .add(points[index + 1].scale(previous_interval / (next_interval * total)));
755 if previous_secant.dot(next_secant) <= 0.0
756 || tangent.dot(previous_secant) <= 0.0
757 || tangent.dot(next_secant) <= 0.0
758 {
759 tangent = Vec3::default();
760 } else {
761 let maximum = 3.0 * previous_secant.length().min(next_secant.length());
762 if tangent.length() > maximum {
763 tangent = tangent.normalized()?.scale(maximum);
764 }
765 }
766 tangents[index] = tangent;
767 }
768 if tension != 1.0 {
769 for tangent in &mut tangents {
770 *tangent = tangent.scale(tension);
771 }
772 }
773 let mut control_points = vec![Vec4::from_point(points[0], 1.0)];
774 for index in 0..count - 1 {
775 let interval = parameters[index + 1] - parameters[index];
776 control_points.extend([
777 Vec4::from_point(
778 points[index].add(tangents[index].scale(interval / 3.0)),
779 1.0,
780 ),
781 Vec4::from_point(
782 points[index + 1].sub(tangents[index + 1].scale(interval / 3.0)),
783 1.0,
784 ),
785 Vec4::from_point(points[index + 1], 1.0),
786 ]);
787 }
788 let mut knots = vec![parameters[0]; 4];
789 for parameter in ¶meters[1..count - 1] {
790 knots.extend([*parameter; 3]);
791 }
792 knots.extend([parameters[count - 1]; 4]);
793 NurbsCurve::new(3, knots, control_points)
794}
795
796const LADDER_BUDGET_FACTOR: usize = 2;
812
813pub const MAX_FIT_STATIONS: usize = 1024;
835
836const MEASURE_NEWTON_STEPS: usize = 3;
842
843const MEASURE_SPAN_FRACTIONS: [f64; 3] = [0.25, 0.5, 0.75];
849
850const STALL_FACTOR: f64 = 0.9;
855
856#[derive(Clone, Copy, Debug, PartialEq)]
874pub struct PolylineFitReport {
875 pub deviation: f64,
879 pub rms: f64,
881 pub between_stations: f64,
885 pub tolerance: f64,
887 pub floor: f64,
899 pub stations: usize,
901 pub controls: usize,
903 pub input_points: usize,
905 pub pool_points: usize,
908 pub dropped_band: f64,
915 pub rungs: usize,
918 pub local_lane: bool,
923 pub lane_switched: bool,
926 pub exit: PolylineFitExit,
928}
929
930impl PolylineFitReport {
931 pub fn met_tolerance(&self) -> bool {
935 self.deviation <= self.floor
936 }
937
938 pub fn met_requested_tolerance(&self) -> bool {
940 self.deviation <= self.tolerance
941 }
942
943 pub fn saturated(&self) -> bool {
954 self.stations >= self.pool_points
955 }
956}
957
958#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
963pub enum PolylineFitExit {
964 Converged,
966 StationCeiling,
969 InputExhausted,
973 Stalled,
977 Unrefined,
980}
981
982#[derive(Clone, Debug)]
983pub struct PolylineFit {
984 pub curve: NurbsCurve,
985 pub parameters: Vec<f64>,
986 pub kept: Vec<Vec3>,
987 pub report: PolylineFitReport,
989}
990
991pub(crate) fn polyline_station_pool(points: &[Vec3], tolerance: f64) -> Vec<usize> {
1003 station_pool(points, tolerance)
1004}
1005
1006fn station_pool(points: &[Vec3], tolerance: f64) -> Vec<usize> {
1007 let simplified = simplify_polyline_indices(points, tolerance);
1008 if simplified.len() > 2 {
1009 let total: f64 = simplified
1010 .windows(2)
1011 .map(|pair| points[pair[1]].sub(points[pair[0]]).length())
1012 .sum();
1013 let floor = (tolerance * 0.01).max(total * 1e-4);
1014 let first = points[simplified[0]];
1015 let last = points[simplified[simplified.len() - 1]];
1016 let mut conditioned = vec![simplified[0]];
1017 for index in &simplified[1..simplified.len() - 1] {
1018 let point = points[*index];
1019 if point.sub(first).length() > floor
1020 && point.sub(last).length() > floor
1021 && point.sub(points[*conditioned.last().unwrap()]).length() > floor
1022 {
1023 conditioned.push(*index);
1024 }
1025 }
1026 conditioned.push(simplified[simplified.len() - 1]);
1027 conditioned
1028 } else {
1029 let mut distinct: Vec<usize> = Vec::new();
1030 for index in simplified {
1031 if distinct.last().is_none_or(|previous: &usize| {
1032 points[index].sub(points[*previous]).length() > tolerance * 0.01
1033 }) {
1034 distinct.push(index);
1035 }
1036 }
1037 distinct
1038 }
1039}
1040
1041fn decimate_stations(pool: &[usize], maximum_points: usize) -> Vec<usize> {
1043 if pool.len() <= maximum_points {
1044 return pool.to_vec();
1045 }
1046 let step = (pool.len() - 1) as f64 / (maximum_points - 1) as f64;
1047 (0..maximum_points)
1048 .map(|index| pool[(index as f64 * step).round() as usize])
1049 .collect()
1050}
1051
1052fn station_parameters(points: &[Vec3], stations: &[usize]) -> Option<Vec<f64>> {
1055 let total: f64 = stations
1056 .windows(2)
1057 .map(|pair| points[pair[1]].sub(points[pair[0]]).length())
1058 .sum();
1059 if total <= 0.0 {
1060 return None;
1061 }
1062 let mut parameters = vec![0.0; stations.len()];
1063 let mut accumulated = 0.0;
1064 for index in 1..stations.len() {
1065 accumulated += points[stations[index]]
1066 .sub(points[stations[index - 1]])
1067 .length();
1068 parameters[index] = accumulated / total;
1069 }
1070 *parameters.last_mut().unwrap() = 1.0;
1071 Some(parameters)
1072}
1073
1074fn distance_to_segment(point: Vec3, from: Vec3, to: Vec3) -> f64 {
1075 let direction = to.sub(from);
1076 let length_squared = direction.length_squared();
1077 if length_squared <= 0.0 {
1078 return point.sub(from).length();
1079 }
1080 let fraction = (point.sub(from).dot(direction) / length_squared).clamp(0.0, 1.0);
1081 point.sub(from.add(direction.scale(fraction))).length()
1082}
1083
1084fn distance_to_curve_near(
1093 curve: &NurbsCurve,
1094 point: Vec3,
1095 seed: f64,
1096 lower: f64,
1097 upper: f64,
1098) -> Result<f64, String> {
1099 let mut parameter = seed.clamp(lower, upper);
1100 let mut best = f64::INFINITY;
1101 for _ in 0..MEASURE_NEWTON_STEPS {
1102 let derivatives = curve.derivatives_small(parameter, 2)?;
1103 let residual = derivatives[0].sub(point);
1104 best = best.min(residual.length_squared());
1105 let slope = derivatives[1].dot(residual);
1106 let curvature = derivatives[2].dot(residual) + derivatives[1].length_squared();
1107 if curvature.abs() <= f64::MIN_POSITIVE {
1108 break;
1109 }
1110 let next = (parameter - slope / curvature).clamp(lower, upper);
1111 if next == parameter {
1112 break;
1113 }
1114 parameter = next;
1115 }
1116 let residual = curve.evaluate(parameter)?.sub(point).length_squared();
1117 Ok(best.min(residual).sqrt())
1118}
1119
1120fn measure_polyline_fit(
1142 curve: &NurbsCurve,
1143 points: &[Vec3],
1144 stations: &[usize],
1145 parameters: &[f64],
1146 arc: &[f64],
1147) -> Result<(f64, f64, f64), String> {
1148 let mut worst = 0.0_f64;
1149 let mut sum_squares = 0.0_f64;
1150 let mut samples = 0usize;
1151 let mut between = 0.0_f64;
1152 let last = stations.len() - 1;
1153 for span in 0..last {
1154 let (from, to) = (stations[span], stations[span + 1]);
1155 let (t0, t1) = (parameters[span], parameters[span + 1]);
1156 let lower = parameters[span.saturating_sub(1)];
1159 let upper = parameters[(span + 2).min(last)];
1160 let length = arc[to] - arc[from];
1164 for index in from..to {
1165 let seed = if length > 0.0 {
1166 t0 + (t1 - t0) * (arc[index] - arc[from]) / length
1167 } else {
1168 t0
1169 };
1170 let distance = distance_to_curve_near(curve, points[index], seed, lower, upper)?;
1171 worst = worst.max(distance);
1172 sum_squares += distance * distance;
1173 samples += 1;
1174 }
1175 let window_start = from.saturating_sub(1);
1178 let window_end = (to + 1).min(points.len() - 1);
1179 for fraction in MEASURE_SPAN_FRACTIONS {
1180 let sample = curve.evaluate(t0 + (t1 - t0) * fraction)?;
1181 let mut nearest = f64::INFINITY;
1182 for index in window_start..window_end {
1183 nearest = nearest.min(distance_to_segment(
1184 sample,
1185 points[index],
1186 points[index + 1],
1187 ));
1188 }
1189 if nearest.is_finite() {
1190 between = between.max(nearest);
1191 }
1192 }
1193 }
1194 let distance = distance_to_curve_near(
1195 curve,
1196 points[stations[last]],
1197 parameters[last],
1198 parameters[last.saturating_sub(1)],
1199 parameters[last],
1200 )?;
1201 worst = worst.max(distance);
1202 sum_squares += distance * distance;
1203 samples += 1;
1204 let rms = (sum_squares / samples as f64).sqrt();
1205 Ok((worst, rms, between))
1206}
1207
1208fn dropped_band(points: &[Vec3], pool: &[usize]) -> f64 {
1212 let mut worst = 0.0_f64;
1213 for span in 0..pool.len() - 1 {
1214 let (from, to) = (pool[span], pool[span + 1]);
1215 for index in from + 1..to {
1216 worst = worst.max(distance_to_segment(
1217 points[index],
1218 points[from],
1219 points[to],
1220 ));
1221 }
1222 }
1223 worst
1224}
1225
1226fn cumulative_chord(points: &[Vec3]) -> Vec<f64> {
1228 let mut arc = vec![0.0; points.len()];
1229 for index in 1..points.len() {
1230 arc[index] = arc[index - 1] + points[index].sub(points[index - 1]).length();
1231 }
1232 arc
1233}
1234
1235fn fit_rung(
1237 points: &[Vec3],
1238 pool: &[usize],
1239 arc: &[f64],
1240 maximum_points: usize,
1241 local_interpolation: bool,
1242 degree: usize,
1243) -> Result<(NurbsCurve, Vec<usize>, Vec<f64>, f64, f64, f64), String> {
1244 let stations = decimate_stations(pool, maximum_points);
1245 fit_station_set(points, stations, arc, local_interpolation, degree)
1246}
1247
1248fn fit_station_set(
1249 points: &[Vec3],
1250 stations: Vec<usize>,
1251 arc: &[f64],
1252 local_interpolation: bool,
1253 degree: usize,
1254) -> Result<(NurbsCurve, Vec<usize>, Vec<f64>, f64, f64, f64), String> {
1255 let parameters = station_parameters(points, &stations)
1256 .ok_or_else(|| "fit_polyline: degenerate polyline".to_string())?;
1257 let kept = stations.iter().map(|index| points[*index]).collect::<Vec<_>>();
1258 let closed = kept.len() > degree + 1
1262 && kept[0].sub(kept[kept.len() - 1]).length() <= 1e-12 * (1.0 + kept[0].length());
1263 let curve = if local_interpolation {
1264 interpolate_curve_local(&kept, ¶meters, 1.0)?
1265 } else if closed
1266 && degree > 3
1267 && degree % 2 == 1
1268 && std::env::var("BREP_SECTION_FIT_PERIODIC").as_deref() != Ok("0")
1269 {
1270 interpolate_curve_closed_of_degree(&kept[..kept.len() - 1], ¶meters, degree)?
1271 } else {
1272 interpolate_curve(&kept, degree.min(kept.len() - 1), ¶meters)?
1273 };
1274 let (deviation, rms, between) =
1275 measure_polyline_fit(&curve, points, &stations, ¶meters, arc)?;
1276 Ok((curve, stations, parameters, deviation, rms, between))
1277}
1278
1279pub fn fit_polyline_at(
1287 points: &[Vec3],
1288 tolerance: f64,
1289 maximum_points: usize,
1290 local_interpolation: bool,
1291) -> Result<PolylineFit, String> {
1292 let fit = fit_polyline_inner(points, tolerance, maximum_points, local_interpolation, false, 3)?;
1293 record_polyline_fit(&fit.report);
1294 Ok(fit)
1295}
1296
1297pub fn fit_polyline(
1346 points: &[Vec3],
1347 tolerance: f64,
1348 maximum_points: usize,
1349 local_interpolation: bool,
1350) -> Result<PolylineFit, String> {
1351 fit_polyline_of_degree(points, tolerance, maximum_points, local_interpolation, 3)
1352}
1353
1354pub fn fit_polyline_of_degree(
1363 points: &[Vec3],
1364 tolerance: f64,
1365 maximum_points: usize,
1366 local_interpolation: bool,
1367 degree: usize,
1368) -> Result<PolylineFit, String> {
1369 fit_polyline_of_degree_with_recovery(points, tolerance, maximum_points, local_interpolation, degree)
1370 .map(|(fit, _)| fit)
1371}
1372
1373pub(crate) fn fit_polyline_of_degree_with_recovery(
1377 points: &[Vec3],
1378 tolerance: f64,
1379 maximum_points: usize,
1380 local_interpolation: bool,
1381 degree: usize,
1382) -> Result<(PolylineFit, bool), String> {
1383 let refine = std::env::var("BREP_POLYLINE_FIT_REFINE").as_deref() != Ok("0");
1384 let asked = fit_polyline_inner(points, tolerance, maximum_points, local_interpolation, refine, degree)?;
1385 if asked.report.met_tolerance() || !refine {
1386 record_polyline_fit(&asked.report);
1387 return Ok((asked, false));
1388 }
1389 let mut other =
1390 fit_polyline_inner(points, tolerance, maximum_points, !local_interpolation, refine, degree)?;
1391 let spent = asked.report.rungs + other.report.rungs;
1392 let mut best = if other.report.deviation < asked.report.deviation {
1393 other.report.lane_switched = true;
1394 other
1395 } else {
1396 asked
1397 };
1398 let mut spent = spent;
1403 let mut recovered = false;
1404 if !best.report.met_tolerance() {
1405 let (adaptive, rounds) = fit_polyline_adaptive(points, tolerance, maximum_points, degree)?;
1406 spent += rounds;
1407 if let Some(mut adaptive) = adaptive {
1408 adaptive.report.lane_switched = local_interpolation;
1409 best = adaptive;
1410 recovered = true;
1411 }
1412 }
1413 best.report.rungs = spent;
1415 record_polyline_fit(&best.report);
1416 Ok((best, recovered))
1417}
1418
1419fn fit_polyline_adaptive(
1423 points: &[Vec3],
1424 tolerance: f64,
1425 maximum_points: usize,
1426 degree: usize,
1427) -> Result<(Option<PolylineFit>, usize), String> {
1428 let pool = station_pool(points, tolerance);
1429 let arc = cumulative_chord(points);
1430 let band = dropped_band(points, &pool);
1431 let floor = tolerance.max(band);
1432 let budget = maximum_points.max(2);
1433 let ceiling = pool.len().min(MAX_FIT_STATIONS)
1434 .min(budget.saturating_mul(LADDER_BUDGET_FACTOR)).max(budget);
1435 let mut stations = decimate_stations(&pool, budget);
1436 for round in 0..=8 {
1439 let Ok((curve, current, parameters, deviation, rms, between)) =
1440 fit_station_set(points, stations, &arc, false, degree)
1441 else { return Ok((None, round + 1)); };
1442 stations = current;
1443 if deviation <= floor {
1444 let report = PolylineFitReport {
1445 deviation, rms, between_stations: between, tolerance, floor,
1446 stations: stations.len(), controls: curve.control_points.len(),
1447 input_points: points.len(), pool_points: pool.len(), dropped_band: band,
1448 rungs: round + 1, local_lane: false, lane_switched: false,
1449 exit: PolylineFitExit::Converged,
1450 };
1451 return Ok((Some(PolylineFit {
1452 curve, parameters, kept: stations.iter().map(|&i| points[i]).collect(), report,
1453 }), round + 1));
1454 }
1455 if stations.len() >= ceiling || round == 8 { return Ok((None, round + 1)); }
1456 let mut candidates = Vec::new();
1457 for span in 0..stations.len() - 1 {
1458 let (from, to) = (stations[span], stations[span + 1]);
1459 let (t0, t1) = (parameters[span], parameters[span + 1]);
1460 let lower = parameters[span.saturating_sub(1)];
1461 let upper = parameters[(span + 2).min(stations.len() - 1)];
1462 let mut candidate: Option<(usize, f64)> = None;
1463 for &index in pool.iter().filter(|&&i| i > from && i < to) {
1464 let length = arc[to] - arc[from];
1465 let seed = if length > 0.0 {
1466 t0 + (t1 - t0) * (arc[index] - arc[from]) / length
1467 } else { t0 };
1468 let error = distance_to_curve_near(&curve, points[index], seed, lower, upper)?;
1469 if candidate.is_none_or(|(_, previous)| error > previous) {
1470 candidate = Some((index, error));
1471 }
1472 }
1473 if let Some((index, error)) = candidate {
1474 if error > floor { candidates.push((index, error)); }
1475 }
1476 }
1477 candidates.sort_by(|a, b| b.1.total_cmp(&a.1).then_with(|| a.0.cmp(&b.0)));
1478 let added = candidates.len().min(16).min(ceiling - stations.len());
1479 if added == 0 { return Ok((None, round + 1)); }
1480 stations.extend(candidates.iter().take(added).map(|&(index, _)| index));
1481 stations.sort_unstable();
1482 }
1483 unreachable!("the bounded refinement returns on its last round")
1484}
1485
1486#[derive(Clone, Copy, Debug, PartialEq, Eq)]
1517pub(crate) enum SeededPath {
1518 Accepted { rounds: usize },
1520 Fallback(SeededFallback),
1522}
1523
1524#[derive(Clone, Copy, Debug, PartialEq, Eq)]
1526pub(crate) enum SeededFallback {
1527 NonFiniteInput,
1528 Degenerate,
1529 RefineDisabled,
1530 PoolTooSmall,
1531 NonFiniteBandOrArc,
1532 NonFiniteFloor,
1533 SeedsOverCeiling,
1534 SeedsOutsideStationPool,
1538 StationSetFailed,
1539 NonFiniteFitReading,
1540 SpanUnreadable,
1541 CeilingReached,
1542 RoundsSpent,
1543 NothingToAdd,
1544 EndpointUnreadable,
1547 TightStationOverTolerance,
1551}
1552
1553#[derive(Clone, Copy, Debug, Default, PartialEq)]
1556pub(crate) struct RepresentativeCost {
1557 pub(crate) representatives: usize,
1561 pub(crate) witnesses: usize,
1563 pub(crate) candidate_fits: usize,
1565 pub(crate) full_refit: bool,
1567 pub(crate) start_offset: f64,
1576 pub(crate) end_offset: f64,
1577}
1578
1579pub(crate) fn polyline_station_pool_floor(points: &[Vec3], tolerance: f64) -> f64 {
1586 let simplified = simplify_polyline_indices(points, tolerance);
1587 if simplified.len() > 2 {
1588 let total: f64 = simplified.windows(2).map(|pair| points[pair[1]].sub(points[pair[0]]).length()).sum();
1589 (tolerance * 0.01).max(total * 1e-4)
1590 } else {
1591 tolerance * 0.01
1592 }
1593}
1594
1595pub(crate) fn fit_polyline_seeded_with_path(
1596 points: &[Vec3],
1597 seeds: &[usize],
1598 tolerance: f64,
1599 full_budget: usize,
1600 local_interpolation: bool,
1601 degree: usize,
1602) -> Result<(PolylineFit, SeededPath), String> {
1603 let full_refit = |reason: SeededFallback| {
1604 fit_polyline_of_degree(points, tolerance, full_budget, local_interpolation, degree)
1605 .map(|fit| (fit, SeededPath::Fallback(reason)))
1606 };
1607 if !tolerance.is_finite() || points.iter().any(|p| !p.x.is_finite() || !p.y.is_finite() || !p.z.is_finite()) {
1609 return full_refit(SeededFallback::NonFiniteInput);
1610 }
1611 if points.len() < 3 {
1612 return full_refit(SeededFallback::Degenerate);
1613 }
1614 if std::env::var("BREP_POLYLINE_FIT_REFINE").as_deref() == Ok("0") {
1615 return full_refit(SeededFallback::RefineDisabled);
1616 }
1617 let pool = station_pool(points, tolerance);
1618 if pool.len() < 2 {
1619 return full_refit(SeededFallback::PoolTooSmall);
1620 }
1621 let arc = cumulative_chord(points);
1622 let band = dropped_band(points, &pool);
1623 if !band.is_finite() || arc.iter().any(|value| !value.is_finite()) {
1624 return full_refit(SeededFallback::NonFiniteBandOrArc);
1625 }
1626 let floor = tolerance.max(band);
1627 if !floor.is_finite() {
1628 return full_refit(SeededFallback::NonFiniteFloor);
1629 }
1630 let budget = full_budget.max(2);
1633 let ceiling = pool
1634 .len()
1635 .min(MAX_FIT_STATIONS)
1636 .min(budget.saturating_mul(LADDER_BUDGET_FACTOR))
1637 .max(budget);
1638 let mut stations = seeds
1639 .iter()
1640 .copied()
1641 .filter(|&index| index < points.len())
1642 .chain([0, points.len() - 1])
1643 .collect::<Vec<_>>();
1644 stations.sort_unstable();
1645 stations.dedup();
1646 if stations.len() > ceiling {
1647 return full_refit(SeededFallback::SeedsOverCeiling);
1648 }
1649 if stations.iter().any(|index| pool.binary_search(index).is_err()) {
1667 return full_refit(SeededFallback::SeedsOutsideStationPool);
1668 }
1669 for round in 0..=8 {
1670 let Ok((curve, current, parameters, deviation, rms, between)) =
1671 fit_station_set(points, stations, &arc, local_interpolation, degree)
1672 else {
1673 return full_refit(SeededFallback::StationSetFailed);
1674 };
1675 stations = current;
1676 if !deviation.is_finite() || !rms.is_finite() || !between.is_finite() {
1677 return full_refit(SeededFallback::NonFiniteFitReading);
1678 }
1679 if deviation <= floor {
1680 let report = PolylineFitReport {
1681 deviation,
1682 rms,
1683 between_stations: between,
1684 tolerance,
1685 floor,
1686 stations: stations.len(),
1687 controls: curve.control_points.len(),
1688 input_points: points.len(),
1689 pool_points: pool.len(),
1690 dropped_band: band,
1691 rungs: round + 1,
1692 local_lane: local_interpolation,
1693 lane_switched: false,
1694 exit: PolylineFitExit::Converged,
1695 };
1696 record_polyline_fit(&report);
1697 let kept = stations.iter().map(|&index| points[index]).collect();
1698 return Ok((PolylineFit { curve, parameters, kept, report }, SeededPath::Accepted { rounds: round + 1 }));
1699 }
1700 if stations.len() >= ceiling {
1701 return full_refit(SeededFallback::CeilingReached);
1702 }
1703 if round == 8 {
1704 return full_refit(SeededFallback::RoundsSpent);
1705 }
1706 let mut candidates = Vec::new();
1707 for span in 0..stations.len() - 1 {
1708 let (from, to) = (stations[span], stations[span + 1]);
1709 let (t0, t1) = (parameters[span], parameters[span + 1]);
1710 let lower = parameters[span.saturating_sub(1)];
1711 let upper = parameters[(span + 2).min(stations.len() - 1)];
1712 let mut candidate: Option<(usize, f64)> = None;
1713 for &index in pool.iter().filter(|&&i| i > from && i < to) {
1714 let length = arc[to] - arc[from];
1715 let seed = if length > 0.0 { t0 + (t1 - t0) * (arc[index] - arc[from]) / length } else { t0 };
1716 let Ok(error) = distance_to_curve_near(&curve, points[index], seed, lower, upper) else {
1719 return full_refit(SeededFallback::SpanUnreadable);
1720 };
1721 if !error.is_finite() {
1722 return full_refit(SeededFallback::SpanUnreadable);
1723 }
1724 if candidate.is_none_or(|(_, previous)| error > previous) {
1725 candidate = Some((index, error));
1726 }
1727 }
1728 if let Some((index, error)) = candidate {
1729 if error > floor {
1730 candidates.push((index, error));
1731 }
1732 }
1733 }
1734 candidates.sort_by(|a, b| b.1.total_cmp(&a.1).then_with(|| a.0.cmp(&b.0)));
1735 let added = candidates.len().min(16).min(ceiling - stations.len());
1736 if added == 0 {
1737 return full_refit(SeededFallback::NothingToAdd);
1738 }
1739 stations.extend(candidates.iter().take(added).map(|&(index, _)| index));
1740 stations.sort_unstable();
1741 }
1742 full_refit(SeededFallback::RoundsSpent)
1744}
1745
1746pub(crate) fn fit_polyline_representatives_with_path(
1766 points: &[Vec3],
1767 seeds: &[usize],
1768 tolerance: f64,
1769 full_budget: usize,
1770 local_interpolation: bool,
1771 degree: usize,
1772) -> Result<(PolylineFit, SeededPath, RepresentativeCost), String> {
1773 fit_polyline_representatives_with_witnesses(points, seeds, &[], tolerance, tolerance, full_budget, local_interpolation, degree)
1774}
1775
1776#[allow(clippy::too_many_arguments)]
1790pub(crate) fn fit_polyline_representatives_with_witnesses(
1791 points: &[Vec3],
1792 seeds: &[usize],
1793 tight: &[usize],
1794 tight_tolerance: f64,
1795 tolerance: f64,
1796 full_budget: usize,
1797 local_interpolation: bool,
1798 degree: usize,
1799) -> Result<(PolylineFit, SeededPath, RepresentativeCost), String> {
1800 let mut cost = RepresentativeCost { witnesses: points.len(), start_offset: f64::NAN, end_offset: f64::NAN, ..RepresentativeCost::default() };
1801 let full_refit = |reason: SeededFallback, mut cost: RepresentativeCost| {
1802 cost.full_refit = true;
1803 fit_polyline_of_degree(points, tolerance, full_budget, local_interpolation, degree)
1804 .map(|fit| (fit, SeededPath::Fallback(reason), cost))
1805 };
1806 if !tolerance.is_finite()
1807 || !tight_tolerance.is_finite()
1808 || points.iter().any(|p| !p.x.is_finite() || !p.y.is_finite() || !p.z.is_finite())
1809 {
1810 return full_refit(SeededFallback::NonFiniteInput, cost);
1811 }
1812 if points.len() < 3 {
1813 return full_refit(SeededFallback::Degenerate, cost);
1814 }
1815 if std::env::var("BREP_POLYLINE_FIT_REFINE").as_deref() == Ok("0") {
1816 return full_refit(SeededFallback::RefineDisabled, cost);
1817 }
1818 let mut tight: Vec<usize> = tight.iter().copied().filter(|&index| index < points.len()).collect();
1819 tight.sort_unstable();
1820 tight.dedup();
1821 let pool = station_pool(points, tolerance);
1822 if pool.len() < 2 || pool[0] != 0 || pool[pool.len() - 1] != points.len() - 1 {
1823 return full_refit(SeededFallback::PoolTooSmall, cost);
1824 }
1825 let arc = cumulative_chord(points);
1826 let band = dropped_band(points, &pool);
1827 if !band.is_finite() || arc.iter().any(|value| !value.is_finite()) {
1828 return full_refit(SeededFallback::NonFiniteBandOrArc, cost);
1829 }
1830 let budget = full_budget.max(2);
1831 let ceiling = pool
1832 .len()
1833 .min(MAX_FIT_STATIONS)
1834 .min(budget.saturating_mul(LADDER_BUDGET_FACTOR))
1835 .max(budget);
1836 let mut stations = seeds
1837 .iter()
1838 .copied()
1839 .filter(|index| pool.binary_search(index).is_ok())
1840 .chain([0, points.len() - 1])
1841 .collect::<Vec<_>>();
1842 stations.sort_unstable();
1843 stations.dedup();
1844 cost.representatives = stations.len();
1845 if stations.len() > ceiling {
1846 return full_refit(SeededFallback::SeedsOverCeiling, cost);
1847 }
1848 for round in 0..=8 {
1849 cost.candidate_fits += 1;
1850 let Ok((curve, current, parameters, deviation, rms, between)) =
1851 fit_station_set(points, stations, &arc, local_interpolation, degree)
1852 else {
1853 return full_refit(SeededFallback::StationSetFailed, cost);
1854 };
1855 stations = current;
1856 if !deviation.is_finite() || !rms.is_finite() || !between.is_finite() {
1857 return full_refit(SeededFallback::NonFiniteFitReading, cost);
1858 }
1859 let mut tight_over: Vec<usize> = Vec::new();
1862 for &index in &tight {
1863 let span = match stations.binary_search(&index) {
1864 Ok(position) => {
1865 let distance = curve.evaluate(parameters[position]).map(|p| p.sub(points[index]).length());
1868 match distance {
1869 Ok(distance) if distance.is_finite() => {
1870 if distance > tight_tolerance {
1871 return full_refit(SeededFallback::TightStationOverTolerance, cost);
1872 }
1873 }
1874 _ => return full_refit(SeededFallback::SpanUnreadable, cost),
1875 }
1876 continue;
1877 }
1878 Err(at) if at == 0 || at >= stations.len() => return full_refit(SeededFallback::SpanUnreadable, cost),
1881 Err(at) => at - 1,
1882 };
1883 let (from, to) = (stations[span], stations[span + 1]);
1884 let (t0, t1) = (parameters[span], parameters[span + 1]);
1885 let length = arc[to] - arc[from];
1886 let seed = if length > 0.0 { t0 + (t1 - t0) * (arc[index] - arc[from]) / length } else { t0 };
1887 let lower = parameters[span.saturating_sub(1)];
1888 let upper = parameters[(span + 2).min(stations.len() - 1)];
1889 let Ok(error) = distance_to_curve_near(&curve, points[index], seed, lower, upper) else {
1890 return full_refit(SeededFallback::SpanUnreadable, cost);
1891 };
1892 if !error.is_finite() {
1893 return full_refit(SeededFallback::SpanUnreadable, cost);
1894 }
1895 if error > tight_tolerance {
1896 tight_over.push(index);
1897 }
1898 }
1899 if deviation <= tolerance && tight_over.is_empty() {
1900 let ends = curve
1905 .domain()
1906 .and_then(|[t0, t1]| Ok((curve.evaluate(t0)?, curve.evaluate(t1)?)));
1907 let Ok((start, end)) = ends else {
1908 return full_refit(SeededFallback::EndpointUnreadable, cost);
1909 };
1910 cost.start_offset = start.sub(points[0]).length();
1911 cost.end_offset = end.sub(points[points.len() - 1]).length();
1912 if !(cost.start_offset.is_finite() && cost.end_offset.is_finite()) {
1913 cost.start_offset = f64::NAN;
1914 cost.end_offset = f64::NAN;
1915 return full_refit(SeededFallback::EndpointUnreadable, cost);
1916 }
1917 let report = PolylineFitReport {
1918 deviation,
1919 rms,
1920 between_stations: between,
1921 tolerance,
1922 floor: tolerance,
1923 stations: stations.len(),
1924 controls: curve.control_points.len(),
1925 input_points: points.len(),
1926 pool_points: pool.len(),
1927 dropped_band: band,
1928 rungs: round + 1,
1929 local_lane: local_interpolation,
1930 lane_switched: false,
1931 exit: PolylineFitExit::Converged,
1932 };
1933 record_polyline_fit(&report);
1934 let kept = stations.iter().map(|&index| points[index]).collect();
1935 return Ok((PolylineFit { curve, parameters, kept, report }, SeededPath::Accepted { rounds: round + 1 }, cost));
1936 }
1937 if stations.len() >= ceiling {
1938 return full_refit(SeededFallback::CeilingReached, cost);
1939 }
1940 if round == 8 {
1941 return full_refit(SeededFallback::RoundsSpent, cost);
1942 }
1943 let mut candidates = Vec::new();
1944 for span in 0..stations.len() - 1 {
1945 let (from, to) = (stations[span], stations[span + 1]);
1946 let (t0, t1) = (parameters[span], parameters[span + 1]);
1947 let lower = parameters[span.saturating_sub(1)];
1948 let upper = parameters[(span + 2).min(stations.len() - 1)];
1949 let mut candidate: Option<(usize, f64)> = None;
1950 for &index in pool.iter().filter(|&&i| i > from && i < to) {
1951 let length = arc[to] - arc[from];
1952 let seed = if length > 0.0 { t0 + (t1 - t0) * (arc[index] - arc[from]) / length } else { t0 };
1953 let Ok(error) = distance_to_curve_near(&curve, points[index], seed, lower, upper) else {
1954 return full_refit(SeededFallback::SpanUnreadable, cost);
1955 };
1956 if !error.is_finite() {
1957 return full_refit(SeededFallback::SpanUnreadable, cost);
1958 }
1959 if candidate.is_none_or(|(_, previous)| error > previous) {
1960 candidate = Some((index, error));
1961 }
1962 }
1963 if let Some((index, error)) = candidate {
1964 if error > tolerance {
1965 candidates.push((index, error));
1966 }
1967 }
1968 }
1969 for &index in &tight_over {
1972 let at = stations.binary_search(&index).unwrap_err();
1973 let (from, to) = (stations[at - 1], stations[at]);
1974 let nearest = pool
1975 .iter()
1976 .copied()
1977 .filter(|&member| member > from && member < to)
1978 .min_by(|&a, &b| (arc[a] - arc[index]).abs().total_cmp(&(arc[b] - arc[index]).abs()).then_with(|| a.cmp(&b)));
1979 if let Some(member) = nearest {
1980 match candidates.iter_mut().find(|(chosen, _)| *chosen == member) {
1983 Some(entry) => entry.1 = f64::INFINITY,
1984 None => candidates.push((member, f64::INFINITY)),
1985 }
1986 }
1987 }
1988 candidates.sort_by(|a, b| b.1.total_cmp(&a.1).then_with(|| a.0.cmp(&b.0)));
1989 let added = candidates.len().min(16).min(ceiling - stations.len());
1990 if added == 0 {
1991 return full_refit(SeededFallback::NothingToAdd, cost);
1994 }
1995 stations.extend(candidates.iter().take(added).map(|&(index, _)| index));
1996 stations.sort_unstable();
1997 }
1998 full_refit(SeededFallback::RoundsSpent, cost)
1999}
2000
2001
2002fn fit_polyline_inner(
2003 points: &[Vec3],
2004 tolerance: f64,
2005 maximum_points: usize,
2006 local_interpolation: bool,
2007 refine: bool,
2008 degree: usize,
2009) -> Result<PolylineFit, String> {
2010 let pool = station_pool(points, tolerance);
2011 if pool.len() < 2 {
2012 return Err("fit_polyline: degenerate polyline".into());
2013 }
2014 let arc = cumulative_chord(points);
2015 let band = dropped_band(points, &pool);
2016 let floor = tolerance.max(band);
2017 let mut budget = maximum_points.max(2);
2018 let ceiling = pool
2019 .len()
2020 .min(MAX_FIT_STATIONS)
2021 .min(budget.saturating_mul(LADDER_BUDGET_FACTOR))
2022 .max(budget);
2023 let (mut curve, mut stations, mut parameters, mut deviation, mut rms, mut between) =
2024 fit_rung(points, &pool, &arc, budget, local_interpolation, degree)?;
2025 let mut rungs = 1usize;
2026 let mut exit = if deviation <= floor {
2027 PolylineFitExit::Converged
2028 } else {
2029 PolylineFitExit::Unrefined
2030 };
2031 while refine && deviation > floor {
2032 if budget >= ceiling {
2033 exit = if ceiling >= pool.len() {
2034 PolylineFitExit::InputExhausted
2035 } else {
2036 PolylineFitExit::StationCeiling
2037 };
2038 break;
2039 }
2040 budget = (budget * 2).min(ceiling);
2041 let rung = fit_rung(points, &pool, &arc, budget, local_interpolation, degree)?;
2042 rungs += 1;
2043 if rung.3 >= deviation * STALL_FACTOR {
2044 if rung.3 < deviation {
2045 (curve, stations, parameters, deviation, rms, between) = rung;
2046 }
2047 exit = if deviation <= floor {
2051 PolylineFitExit::Converged
2052 } else {
2053 PolylineFitExit::Stalled
2054 };
2055 break;
2056 }
2057 (curve, stations, parameters, deviation, rms, between) = rung;
2058 if deviation <= floor {
2059 exit = PolylineFitExit::Converged;
2060 }
2061 }
2062 let report = PolylineFitReport {
2063 deviation,
2064 rms,
2065 between_stations: between,
2066 tolerance,
2067 floor,
2068 stations: stations.len(),
2069 controls: curve.control_points.len(),
2070 input_points: points.len(),
2071 pool_points: pool.len(),
2072 dropped_band: band,
2073 rungs,
2074 local_lane: local_interpolation,
2075 lane_switched: false,
2076 exit,
2077 };
2078 Ok(PolylineFit {
2079 curve,
2080 parameters,
2081 kept: stations.iter().map(|index| points[*index]).collect(),
2082 report,
2083 })
2084}
2085
2086#[derive(Clone, Copy, Debug, Default, PartialEq)]
2099pub struct PolylineFitLedger {
2100 pub fits: u64,
2102 pub unmet: u64,
2104 pub station_ceiling: u64,
2106 pub input_exhausted: u64,
2108 pub stalled: u64,
2110 pub unrefined: u64,
2112 pub refined: u64,
2114 pub met_at_pool: u64,
2117 pub lane_switched: u64,
2120 pub extra_rungs: u64,
2122 pub worst_unmet_deviation: f64,
2124 pub worst_dropped_band: f64,
2126}
2127
2128impl PolylineFitLedger {
2129 fn record(&mut self, report: &PolylineFitReport) {
2130 self.fits += 1;
2131 self.extra_rungs += (report.rungs - 1) as u64;
2132 if report.rungs > 1 {
2133 self.refined += 1;
2134 }
2135 self.worst_dropped_band = self.worst_dropped_band.max(report.dropped_band);
2136 if report.lane_switched {
2137 self.lane_switched += 1;
2138 }
2139 if report.met_tolerance() {
2140 if report.saturated() {
2141 self.met_at_pool += 1;
2142 }
2143 return;
2144 }
2145 self.unmet += 1;
2146 match report.exit {
2147 PolylineFitExit::Converged => {}
2148 PolylineFitExit::StationCeiling => self.station_ceiling += 1,
2149 PolylineFitExit::InputExhausted => self.input_exhausted += 1,
2150 PolylineFitExit::Stalled => self.stalled += 1,
2151 PolylineFitExit::Unrefined => self.unrefined += 1,
2152 }
2153 self.worst_unmet_deviation = self.worst_unmet_deviation.max(report.deviation);
2154 }
2155
2156 fn fold(&mut self, child: &PolylineFitLedger) {
2157 self.fits += child.fits;
2158 self.unmet += child.unmet;
2159 self.station_ceiling += child.station_ceiling;
2160 self.input_exhausted += child.input_exhausted;
2161 self.stalled += child.stalled;
2162 self.unrefined += child.unrefined;
2163 self.refined += child.refined;
2164 self.met_at_pool += child.met_at_pool;
2165 self.lane_switched += child.lane_switched;
2166 self.extra_rungs += child.extra_rungs;
2167 self.worst_unmet_deviation = self.worst_unmet_deviation.max(child.worst_unmet_deviation);
2168 self.worst_dropped_band = self.worst_dropped_band.max(child.worst_dropped_band);
2169 }
2170
2171 pub fn report_into(&self, diagnostics: &mut KernelDiagnostics) {
2178 diagnostics.count_n("fit.polyline.fits", self.fits);
2179 diagnostics.count_n("fit.polyline.unmet_tolerance", self.unmet);
2180 diagnostics.count_n("fit.polyline.exit.station_ceiling", self.station_ceiling);
2181 diagnostics.count_n("fit.polyline.exit.input_exhausted", self.input_exhausted);
2182 diagnostics.count_n("fit.polyline.exit.stalled", self.stalled);
2183 diagnostics.count_n("fit.polyline.exit.unrefined", self.unrefined);
2184 diagnostics.count_n("fit.polyline.refined", self.refined);
2185 diagnostics.count_n("fit.polyline.met_at_pool", self.met_at_pool);
2186 diagnostics.count_n("fit.polyline.lane_switched", self.lane_switched);
2187 diagnostics.count_n("fit.polyline.extra_rungs", self.extra_rungs);
2188 diagnostics.measure_max("fit.polyline.worst_unmet_deviation", self.worst_unmet_deviation);
2189 diagnostics.measure_max("fit.polyline.worst_dropped_band", self.worst_dropped_band);
2190 if self.unmet > 0 {
2191 diagnostics.event(
2192 DiagnosticSeverity::Degraded,
2193 KernelStage::Intersect,
2194 "fit.polyline.unmet_tolerance",
2195 format!(
2196 "{} of {} polyline fits did not reach the tolerance they were given \
2197 (station ceiling {}, input exhausted {}, stalled {}, unrefined {}); \
2198 worst deviation {:.3e}; the simplification's own band reaches {:.3e}",
2199 self.unmet,
2200 self.fits,
2201 self.station_ceiling,
2202 self.input_exhausted,
2203 self.stalled,
2204 self.unrefined,
2205 self.worst_unmet_deviation,
2206 self.worst_dropped_band,
2207 ),
2208 );
2209 }
2210 }
2211}
2212
2213thread_local! {
2214 static POLYLINE_FIT_SCOPES: std::cell::RefCell<Vec<PolylineFitLedger>> =
2216 const { std::cell::RefCell::new(Vec::new()) };
2217}
2218
2219pub struct PolylineFitScope {
2226 _thread_bound: std::marker::PhantomData<*const ()>,
2228}
2229
2230impl PolylineFitScope {
2231 pub fn open() -> Self {
2232 POLYLINE_FIT_SCOPES.with(|scopes| scopes.borrow_mut().push(PolylineFitLedger::default()));
2233 Self {
2234 _thread_bound: std::marker::PhantomData,
2235 }
2236 }
2237
2238 pub fn close(self) -> PolylineFitLedger {
2240 let ledger = POLYLINE_FIT_SCOPES.with(|scopes| {
2241 let mut scopes = scopes.borrow_mut();
2242 let ledger = scopes.pop().unwrap_or_default();
2243 if let Some(parent) = scopes.last_mut() {
2244 parent.fold(&ledger);
2245 }
2246 ledger
2247 });
2248 std::mem::forget(self);
2249 ledger
2250 }
2251}
2252
2253impl Drop for PolylineFitScope {
2254 fn drop(&mut self) {
2255 POLYLINE_FIT_SCOPES.with(|scopes| {
2256 scopes.borrow_mut().pop();
2257 });
2258 }
2259}
2260
2261fn record_polyline_fit(report: &PolylineFitReport) {
2262 if std::env::var_os("BREP_DEBUG_POLYLINE_FIT").is_some() {
2265 eprintln!("POLYLINE-FIT {report:?}");
2266 }
2267 POLYLINE_FIT_SCOPES.with(|scopes| {
2268 if let Some(open) = scopes.borrow_mut().last_mut() {
2269 open.record(report);
2270 }
2271 });
2272}
2273
2274
2275
2276fn raw_basis(knots: &[f64], degree: usize, span: usize, parameter: f64) -> Vec<f64> {
2280 let mut basis = vec![0.0; degree + 1];
2281 let mut left = vec![0.0; degree + 1];
2282 let mut right = vec![0.0; degree + 1];
2283 basis[0] = 1.0;
2284 for j in 1..=degree {
2285 left[j] = parameter - knots[span + 1 - j];
2286 right[j] = knots[span + j] - parameter;
2287 let mut saved = 0.0;
2288 for r in 0..j {
2289 let denominator = right[r + 1] + left[j - r];
2290 let temp = if denominator.abs() > 0.0 {
2291 basis[r] / denominator
2292 } else {
2293 0.0
2294 };
2295 basis[r] = saved + right[r + 1] * temp;
2296 saved = left[j - r] * temp;
2297 }
2298 basis[j] = saved;
2299 }
2300 basis
2301}
2302
2303fn raw_insert_knot(knots: &mut Vec<f64>, controls: &mut Vec<Vec3>, degree: usize, parameter: f64) {
2305 let span = knots
2309 .iter()
2310 .rposition(|&knot| knot <= parameter + 1e-14)
2311 .unwrap()
2312 .min(controls.len() - 1);
2313 let mut fresh = Vec::with_capacity(controls.len() + 1);
2314 fresh.extend_from_slice(&controls[..=span - degree]);
2315 for i in span - degree + 1..=span {
2316 let denominator = knots[i + degree] - knots[i];
2317 let alpha = if denominator.abs() > 0.0 {
2318 (parameter - knots[i]) / denominator
2319 } else {
2320 0.0
2321 };
2322 fresh.push(
2323 controls[i - 1]
2324 .scale(1.0 - alpha)
2325 .add(controls[i].scale(alpha)),
2326 );
2327 }
2328 fresh.extend_from_slice(&controls[span..]);
2329 *controls = fresh;
2330 knots.insert(span + 1, parameter);
2331}
2332
2333pub fn interpolate_curve_closed(points: &[Vec3], parameters: &[f64]) -> Result<NurbsCurve, String> {
2341 interpolate_curve_closed_of_degree(points, parameters, 3)
2342}
2343
2344pub fn interpolate_curve_closed_of_degree(
2350 points: &[Vec3],
2351 parameters: &[f64],
2352 degree: usize,
2353) -> Result<NurbsCurve, String> {
2354 if degree % 2 == 0 || degree < 3 {
2355 return Err("interpolate_curve_closed: the degree must be odd and at least 3".into());
2356 }
2357 let station_count = points.len();
2358 if station_count < degree + 1 {
2359 return Err("interpolate_curve_closed: need more stations than the degree".into());
2360 }
2361 if parameters.len() != station_count + 1 {
2362 return Err(
2363 "interpolate_curve_closed: parameters must have one more entry than points".into(),
2364 );
2365 }
2366 if parameters.windows(2).any(|pair| pair[1] <= pair[0]) {
2367 return Err("interpolate_curve_closed: parameters must increase".into());
2368 }
2369 let period = parameters[station_count] - parameters[0];
2370 let cyclic = |j: i64| -> f64 {
2373 let s = station_count as i64;
2374 let wrap = j.div_euclid(s);
2375 parameters[j.rem_euclid(s) as usize] + wrap as f64 * period
2376 };
2377 let raw_knots: Vec<f64> = (-(degree as i64)..=(station_count as i64 + degree as i64)).map(cyclic).collect();
2378 let mut matrix = vec![vec![0.0; station_count]; station_count];
2381 for i in 0..station_count {
2382 let span = i + degree;
2383 let basis = raw_basis(&raw_knots, degree, span, parameters[i]);
2384 for (offset, value) in basis.iter().enumerate() {
2385 let index = (i as i64 + offset as i64 - degree as i64).rem_euclid(station_count as i64) as usize;
2388 matrix[i][index] += value;
2389 }
2390 }
2391 let solve_axis = |axis: fn(Vec3) -> f64| {
2392 solve_dense(
2393 matrix.clone(),
2394 points.iter().copied().map(axis).collect::<Vec<_>>(),
2395 )
2396 };
2397 let xs = solve_axis(|point| point.x)?;
2398 let ys = solve_axis(|point| point.y)?;
2399 let zs = solve_axis(|point| point.z)?;
2400 let cyclic_controls: Vec<Vec3> = (0..station_count)
2401 .map(|index| Vec3::new(xs[index], ys[index], zs[index]))
2402 .collect();
2403 let mut window_controls: Vec<Vec3> = (-(degree as i64)..(station_count as i64))
2405 .map(|j| cyclic_controls[j.rem_euclid(station_count as i64) as usize])
2406 .collect();
2407 let mut window_knots = raw_knots.clone();
2408 for _ in 0..degree {
2411 raw_insert_knot(
2412 &mut window_knots,
2413 &mut window_controls,
2414 degree,
2415 parameters[0],
2416 );
2417 }
2418 for _ in 0..degree {
2419 raw_insert_knot(
2420 &mut window_knots,
2421 &mut window_controls,
2422 degree,
2423 parameters[station_count],
2424 );
2425 }
2426 let first = window_knots
2430 .iter()
2431 .position(|&knot| (knot - parameters[0]).abs() < 1e-12)
2432 .ok_or("interpolate_curve_closed: clamp lost the start knot")?;
2433 let last = window_knots
2434 .iter()
2435 .rposition(|&knot| (knot - parameters[station_count]).abs() < 1e-12)
2436 .ok_or("interpolate_curve_closed: clamp lost the end knot")?;
2437 let clamped_knots: Vec<f64> = window_knots[first..=last].to_vec();
2438 let control_count = clamped_knots.len() - degree - 1;
2439 let clamped_controls: Vec<Vec4> = window_controls[first..first + control_count]
2440 .iter()
2441 .map(|point| Vec4::from_point(*point, 1.0))
2442 .collect();
2443 NurbsCurve::new(degree, clamped_knots, clamped_controls)
2444}
2445