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 const PT_KNOTS_DEFAULT: bool = true;
564 static PT_KNOTS: std::sync::OnceLock<bool> = std::sync::OnceLock::new();
565 let pt_knots = *PT_KNOTS.get_or_init(|| {
566 match std::env::var("BREP_FIT_PT_KNOTS").as_deref() {
567 Ok("0") => false,
568 Ok("1") => true,
569 _ => PT_KNOTS_DEFAULT,
570 }
571 });
572 let mut knots = vec![t0; degree + 1];
578 for j in 0..n.saturating_sub(1) {
579 let window = if pt_knots {
580 ¶meters[j..j + degree]
581 } else {
582 ¶meters[j + 1..(j + degree).min(n) + 1]
583 };
584 knots.push(window.iter().sum::<f64>() / window.len() as f64);
585 }
586 knots.extend(std::iter::repeat(t1).take(degree + 1));
587 if knots.len() != control_count + degree + 1 {
588 return Err(format!(
589 "interpolate_curve_with_end_tangents: internal knot count {} for {} controls",
590 knots.len(),
591 control_count
592 ));
593 }
594 let knot_vector = KnotVector::new(knots.clone(), degree)?;
595 let mut matrix = vec![vec![0.0; control_count]; control_count];
596 let mut rhs_points = vec![Vec3::default(); control_count];
597 matrix[0][0] = 1.0;
601 rhs_points[0] = points[0];
602 let start_span = knots[degree + 1] - t0;
603 matrix[1][0] = -(degree as f64) / start_span;
604 matrix[1][1] = (degree as f64) / start_span;
605 rhs_points[1] = start_tangent;
606 for (index, ¶meter) in parameters.iter().enumerate().take(n).skip(1) {
607 let row = index + 1;
608 let span = knot_vector.find_span(parameter);
609 let basis = knot_vector.basis_functions(span, parameter);
610 for (offset, value) in basis.into_iter().enumerate() {
611 matrix[row][span - degree + offset] = value;
612 }
613 rhs_points[row] = points[index];
614 }
615 let end_span = t1 - knots[control_count - 1];
616 matrix[control_count - 2][control_count - 2] = -(degree as f64) / end_span;
617 matrix[control_count - 2][control_count - 1] = (degree as f64) / end_span;
618 rhs_points[control_count - 2] = end_tangent;
619 matrix[control_count - 1][control_count - 1] = 1.0;
620 rhs_points[control_count - 1] = points[n];
621 let solve_axis = |axis: fn(Vec3) -> f64| {
622 solve_dense(
623 matrix.clone(),
624 rhs_points.iter().copied().map(axis).collect::<Vec<_>>(),
625 )
626 };
627 let xs = solve_axis(|point| point.x)?;
628 let ys = solve_axis(|point| point.y)?;
629 let zs = solve_axis(|point| point.z)?;
630 NurbsCurve::new(
631 degree,
632 knots,
633 (0..control_count)
634 .map(|index| Vec4::from_point(Vec3::new(xs[index], ys[index], zs[index]), 1.0))
635 .collect(),
636 )
637}
638
639pub fn interpolate_curve_thinned(
640 points: &[Vec3],
641 degree: usize,
642 maximum_points: usize,
643) -> Result<NurbsCurve, String> {
644 if maximum_points < 2 {
645 return Err("interpolate_curve_thinned: maximum point count must be at least 2".into());
646 }
647 if points.len() <= maximum_points {
648 let parameters = chord_parameters(points);
649 return interpolate_curve(points, degree, ¶meters);
650 }
651 let step = (points.len() - 1) as f64 / (maximum_points - 1) as f64;
652 let thinned = (0..maximum_points)
653 .map(|index| points[(index as f64 * step).round() as usize])
654 .collect::<Vec<_>>();
655 let parameters = chord_parameters(&thinned);
656 interpolate_curve(&thinned, degree, ¶meters)
657}
658
659fn chord_parameters(points: &[Vec3]) -> Vec<f64> {
660 if points.len() < 2 {
661 return vec![0.0; points.len()];
662 }
663 let mut parameters = vec![0.0; points.len()];
664 for index in 1..points.len() {
665 parameters[index] = parameters[index - 1] + points[index].sub(points[index - 1]).length();
666 }
667 let length = parameters[points.len() - 1];
668 if length <= 1e-15 {
669 for (index, parameter) in parameters.iter_mut().enumerate() {
670 *parameter = index as f64 / (points.len() - 1) as f64;
671 }
672 } else {
673 for parameter in &mut parameters {
674 *parameter /= length;
675 }
676 }
677 parameters
678}
679
680pub fn simplify_polyline_indices(points: &[Vec3], tolerance: f64) -> Vec<usize> {
689 if points.len() <= 2 {
690 return (0..points.len()).collect();
691 }
692 let mut keep = vec![false; points.len()];
693 keep[0] = true;
694 keep[points.len() - 1] = true;
695 let mut stack = vec![(0usize, points.len() - 1)];
696 while let Some((start, end)) = stack.pop() {
697 if end - start < 2 {
698 continue;
699 }
700 let a = points[start];
701 let direction = points[end].sub(a);
702 let length_squared = direction.length_squared().max(1e-300);
703 let mut worst = None;
704 let mut worst_distance = tolerance;
705 for (index, point) in points.iter().enumerate().take(end).skip(start + 1) {
706 let fraction = point.sub(a).dot(direction) / length_squared;
707 let fraction = fraction.clamp(0.0, 1.0);
708 let distance = point.sub(a.add(direction.scale(fraction))).length();
709 if distance > worst_distance {
710 worst_distance = distance;
711 worst = Some(index);
712 }
713 }
714 if let Some(index) = worst {
715 keep[index] = true;
716 stack.push((start, index));
717 stack.push((index, end));
718 }
719 }
720 keep.into_iter()
721 .enumerate()
722 .filter_map(|(index, keep)| keep.then_some(index))
723 .collect()
724}
725
726pub fn simplify_polyline(points: &[Vec3], tolerance: f64) -> Vec<Vec3> {
727 simplify_polyline_indices(points, tolerance)
728 .into_iter()
729 .map(|index| points[index])
730 .collect()
731}
732
733pub fn interpolate_curve_local(
734 points: &[Vec3],
735 parameters: &[f64],
736 tension: f64,
737) -> Result<NurbsCurve, String> {
738 if points.len() != parameters.len() || points.len() < 2 {
739 return Err(
740 "interpolate_curve_local: points and parameters must have matching length >= 2".into(),
741 );
742 }
743 if points.len() == 2 {
744 return interpolate_curve(points, 1, parameters);
745 }
746 let count = points.len();
747 let mut tangents = vec![Vec3::default(); count];
748 tangents[0] = points[1]
749 .sub(points[0])
750 .scale(1.0 / (parameters[1] - parameters[0]));
751 tangents[count - 1] = points[count - 1]
752 .sub(points[count - 2])
753 .scale(1.0 / (parameters[count - 1] - parameters[count - 2]));
754 for index in 1..count - 1 {
755 let previous_interval = parameters[index] - parameters[index - 1];
756 let next_interval = parameters[index + 1] - parameters[index];
757 let total = previous_interval + next_interval;
758 let previous_secant = points[index]
759 .sub(points[index - 1])
760 .scale(1.0 / previous_interval);
761 let next_secant = points[index + 1]
762 .sub(points[index])
763 .scale(1.0 / next_interval);
764 let mut tangent =
765 points[index - 1]
766 .scale(-next_interval / (previous_interval * total))
767 .add(points[index].scale(
768 (next_interval - previous_interval) / (previous_interval * next_interval),
769 ))
770 .add(points[index + 1].scale(previous_interval / (next_interval * total)));
771 if previous_secant.dot(next_secant) <= 0.0
772 || tangent.dot(previous_secant) <= 0.0
773 || tangent.dot(next_secant) <= 0.0
774 {
775 tangent = Vec3::default();
776 } else {
777 let maximum = 3.0 * previous_secant.length().min(next_secant.length());
778 if tangent.length() > maximum {
779 tangent = tangent.normalized()?.scale(maximum);
780 }
781 }
782 tangents[index] = tangent;
783 }
784 if tension != 1.0 {
785 for tangent in &mut tangents {
786 *tangent = tangent.scale(tension);
787 }
788 }
789 let mut control_points = vec![Vec4::from_point(points[0], 1.0)];
790 for index in 0..count - 1 {
791 let interval = parameters[index + 1] - parameters[index];
792 control_points.extend([
793 Vec4::from_point(
794 points[index].add(tangents[index].scale(interval / 3.0)),
795 1.0,
796 ),
797 Vec4::from_point(
798 points[index + 1].sub(tangents[index + 1].scale(interval / 3.0)),
799 1.0,
800 ),
801 Vec4::from_point(points[index + 1], 1.0),
802 ]);
803 }
804 let mut knots = vec![parameters[0]; 4];
805 for parameter in ¶meters[1..count - 1] {
806 knots.extend([*parameter; 3]);
807 }
808 knots.extend([parameters[count - 1]; 4]);
809 NurbsCurve::new(3, knots, control_points)
810}
811
812const LADDER_BUDGET_FACTOR: usize = 2;
828
829pub const MAX_FIT_STATIONS: usize = 1024;
851
852const MEASURE_NEWTON_STEPS: usize = 3;
858
859const MEASURE_SPAN_FRACTIONS: [f64; 3] = [0.25, 0.5, 0.75];
865
866const STALL_FACTOR: f64 = 0.9;
871
872#[derive(Clone, Copy, Debug, PartialEq)]
890pub struct PolylineFitReport {
891 pub deviation: f64,
895 pub rms: f64,
897 pub between_stations: f64,
901 pub tolerance: f64,
903 pub floor: f64,
915 pub stations: usize,
917 pub controls: usize,
919 pub input_points: usize,
921 pub pool_points: usize,
924 pub dropped_band: f64,
931 pub rungs: usize,
934 pub local_lane: bool,
939 pub lane_switched: bool,
942 pub exit: PolylineFitExit,
944}
945
946impl PolylineFitReport {
947 pub fn met_tolerance(&self) -> bool {
951 self.deviation <= self.floor
952 }
953
954 pub fn met_requested_tolerance(&self) -> bool {
956 self.deviation <= self.tolerance
957 }
958
959 pub fn saturated(&self) -> bool {
970 self.stations >= self.pool_points
971 }
972}
973
974#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
979pub enum PolylineFitExit {
980 Converged,
982 StationCeiling,
985 InputExhausted,
989 Stalled,
993 Unrefined,
996}
997
998#[derive(Clone, Debug)]
999pub struct PolylineFit {
1000 pub curve: NurbsCurve,
1001 pub parameters: Vec<f64>,
1002 pub kept: Vec<Vec3>,
1003 pub report: PolylineFitReport,
1005}
1006
1007pub(crate) fn polyline_station_pool(points: &[Vec3], tolerance: f64) -> Vec<usize> {
1019 station_pool(points, tolerance)
1020}
1021
1022fn station_pool(points: &[Vec3], tolerance: f64) -> Vec<usize> {
1023 let simplified = simplify_polyline_indices(points, tolerance);
1024 if simplified.len() > 2 {
1025 let total: f64 = simplified
1026 .windows(2)
1027 .map(|pair| points[pair[1]].sub(points[pair[0]]).length())
1028 .sum();
1029 let floor = (tolerance * 0.01).max(total * 1e-4);
1030 let first = points[simplified[0]];
1031 let last = points[simplified[simplified.len() - 1]];
1032 let mut conditioned = vec![simplified[0]];
1033 for index in &simplified[1..simplified.len() - 1] {
1034 let point = points[*index];
1035 if point.sub(first).length() > floor
1036 && point.sub(last).length() > floor
1037 && point.sub(points[*conditioned.last().unwrap()]).length() > floor
1038 {
1039 conditioned.push(*index);
1040 }
1041 }
1042 conditioned.push(simplified[simplified.len() - 1]);
1043 conditioned
1044 } else {
1045 let mut distinct: Vec<usize> = Vec::new();
1046 for index in simplified {
1047 if distinct.last().is_none_or(|previous: &usize| {
1048 points[index].sub(points[*previous]).length() > tolerance * 0.01
1049 }) {
1050 distinct.push(index);
1051 }
1052 }
1053 distinct
1054 }
1055}
1056
1057fn decimate_stations(pool: &[usize], maximum_points: usize) -> Vec<usize> {
1059 if pool.len() <= maximum_points {
1060 return pool.to_vec();
1061 }
1062 let step = (pool.len() - 1) as f64 / (maximum_points - 1) as f64;
1063 (0..maximum_points)
1064 .map(|index| pool[(index as f64 * step).round() as usize])
1065 .collect()
1066}
1067
1068fn station_parameters(points: &[Vec3], stations: &[usize]) -> Option<Vec<f64>> {
1071 let total: f64 = stations
1072 .windows(2)
1073 .map(|pair| points[pair[1]].sub(points[pair[0]]).length())
1074 .sum();
1075 if total <= 0.0 {
1076 return None;
1077 }
1078 let mut parameters = vec![0.0; stations.len()];
1079 let mut accumulated = 0.0;
1080 for index in 1..stations.len() {
1081 accumulated += points[stations[index]]
1082 .sub(points[stations[index - 1]])
1083 .length();
1084 parameters[index] = accumulated / total;
1085 }
1086 *parameters.last_mut().unwrap() = 1.0;
1087 Some(parameters)
1088}
1089
1090fn distance_to_segment(point: Vec3, from: Vec3, to: Vec3) -> f64 {
1091 let direction = to.sub(from);
1092 let length_squared = direction.length_squared();
1093 if length_squared <= 0.0 {
1094 return point.sub(from).length();
1095 }
1096 let fraction = (point.sub(from).dot(direction) / length_squared).clamp(0.0, 1.0);
1097 point.sub(from.add(direction.scale(fraction))).length()
1098}
1099
1100fn distance_to_curve_near(
1109 curve: &NurbsCurve,
1110 point: Vec3,
1111 seed: f64,
1112 lower: f64,
1113 upper: f64,
1114) -> Result<f64, String> {
1115 let mut parameter = seed.clamp(lower, upper);
1116 let mut best = f64::INFINITY;
1117 for _ in 0..MEASURE_NEWTON_STEPS {
1118 let derivatives = curve.derivatives_small(parameter, 2)?;
1119 let residual = derivatives[0].sub(point);
1120 best = best.min(residual.length_squared());
1121 let slope = derivatives[1].dot(residual);
1122 let curvature = derivatives[2].dot(residual) + derivatives[1].length_squared();
1123 if curvature.abs() <= f64::MIN_POSITIVE {
1124 break;
1125 }
1126 let next = (parameter - slope / curvature).clamp(lower, upper);
1127 if next == parameter {
1128 break;
1129 }
1130 parameter = next;
1131 }
1132 let residual = curve.evaluate(parameter)?.sub(point).length_squared();
1133 Ok(best.min(residual).sqrt())
1134}
1135
1136fn measure_polyline_fit(
1158 curve: &NurbsCurve,
1159 points: &[Vec3],
1160 stations: &[usize],
1161 parameters: &[f64],
1162 arc: &[f64],
1163) -> Result<(f64, f64, f64), String> {
1164 let mut worst = 0.0_f64;
1165 let mut sum_squares = 0.0_f64;
1166 let mut samples = 0usize;
1167 let mut between = 0.0_f64;
1168 let last = stations.len() - 1;
1169 for span in 0..last {
1170 let (from, to) = (stations[span], stations[span + 1]);
1171 let (t0, t1) = (parameters[span], parameters[span + 1]);
1172 let lower = parameters[span.saturating_sub(1)];
1175 let upper = parameters[(span + 2).min(last)];
1176 let length = arc[to] - arc[from];
1180 for index in from..to {
1181 let seed = if length > 0.0 {
1182 t0 + (t1 - t0) * (arc[index] - arc[from]) / length
1183 } else {
1184 t0
1185 };
1186 let distance = distance_to_curve_near(curve, points[index], seed, lower, upper)?;
1187 worst = worst.max(distance);
1188 sum_squares += distance * distance;
1189 samples += 1;
1190 }
1191 let window_start = from.saturating_sub(1);
1194 let window_end = (to + 1).min(points.len() - 1);
1195 for fraction in MEASURE_SPAN_FRACTIONS {
1196 let sample = curve.evaluate(t0 + (t1 - t0) * fraction)?;
1197 let mut nearest = f64::INFINITY;
1198 for index in window_start..window_end {
1199 nearest = nearest.min(distance_to_segment(
1200 sample,
1201 points[index],
1202 points[index + 1],
1203 ));
1204 }
1205 if nearest.is_finite() {
1206 between = between.max(nearest);
1207 }
1208 }
1209 }
1210 let distance = distance_to_curve_near(
1211 curve,
1212 points[stations[last]],
1213 parameters[last],
1214 parameters[last.saturating_sub(1)],
1215 parameters[last],
1216 )?;
1217 worst = worst.max(distance);
1218 sum_squares += distance * distance;
1219 samples += 1;
1220 let rms = (sum_squares / samples as f64).sqrt();
1221 Ok((worst, rms, between))
1222}
1223
1224fn dropped_band(points: &[Vec3], pool: &[usize]) -> f64 {
1228 let mut worst = 0.0_f64;
1229 for span in 0..pool.len() - 1 {
1230 let (from, to) = (pool[span], pool[span + 1]);
1231 for index in from + 1..to {
1232 worst = worst.max(distance_to_segment(
1233 points[index],
1234 points[from],
1235 points[to],
1236 ));
1237 }
1238 }
1239 worst
1240}
1241
1242fn cumulative_chord(points: &[Vec3]) -> Vec<f64> {
1244 let mut arc = vec![0.0; points.len()];
1245 for index in 1..points.len() {
1246 arc[index] = arc[index - 1] + points[index].sub(points[index - 1]).length();
1247 }
1248 arc
1249}
1250
1251fn fit_rung(
1253 points: &[Vec3],
1254 pool: &[usize],
1255 arc: &[f64],
1256 maximum_points: usize,
1257 local_interpolation: bool,
1258 degree: usize,
1259) -> Result<(NurbsCurve, Vec<usize>, Vec<f64>, f64, f64, f64), String> {
1260 let stations = decimate_stations(pool, maximum_points);
1261 fit_station_set(points, stations, arc, local_interpolation, degree)
1262}
1263
1264fn fit_station_set(
1265 points: &[Vec3],
1266 stations: Vec<usize>,
1267 arc: &[f64],
1268 local_interpolation: bool,
1269 degree: usize,
1270) -> Result<(NurbsCurve, Vec<usize>, Vec<f64>, f64, f64, f64), String> {
1271 let parameters = station_parameters(points, &stations)
1272 .ok_or_else(|| "fit_polyline: degenerate polyline".to_string())?;
1273 let kept = stations.iter().map(|index| points[*index]).collect::<Vec<_>>();
1274 let closed = kept.len() > degree + 1
1278 && kept[0].sub(kept[kept.len() - 1]).length() <= 1e-12 * (1.0 + kept[0].length());
1279 let curve = if local_interpolation {
1280 interpolate_curve_local(&kept, ¶meters, 1.0)?
1281 } else if closed
1282 && degree > 3
1283 && degree % 2 == 1
1284 && std::env::var("BREP_SECTION_FIT_PERIODIC").as_deref() != Ok("0")
1285 {
1286 interpolate_curve_closed_of_degree(&kept[..kept.len() - 1], ¶meters, degree)?
1287 } else {
1288 interpolate_curve(&kept, degree.min(kept.len() - 1), ¶meters)?
1289 };
1290 let (deviation, rms, between) =
1291 measure_polyline_fit(&curve, points, &stations, ¶meters, arc)?;
1292 Ok((curve, stations, parameters, deviation, rms, between))
1293}
1294
1295pub fn fit_polyline_at(
1303 points: &[Vec3],
1304 tolerance: f64,
1305 maximum_points: usize,
1306 local_interpolation: bool,
1307) -> Result<PolylineFit, String> {
1308 let fit = fit_polyline_inner(points, tolerance, maximum_points, local_interpolation, false, 3)?;
1309 record_polyline_fit(&fit.report);
1310 Ok(fit)
1311}
1312
1313pub fn fit_polyline(
1362 points: &[Vec3],
1363 tolerance: f64,
1364 maximum_points: usize,
1365 local_interpolation: bool,
1366) -> Result<PolylineFit, String> {
1367 fit_polyline_of_degree(points, tolerance, maximum_points, local_interpolation, 3)
1368}
1369
1370pub fn fit_polyline_of_degree(
1379 points: &[Vec3],
1380 tolerance: f64,
1381 maximum_points: usize,
1382 local_interpolation: bool,
1383 degree: usize,
1384) -> Result<PolylineFit, String> {
1385 fit_polyline_of_degree_with_recovery(points, tolerance, maximum_points, local_interpolation, degree)
1386 .map(|(fit, _)| fit)
1387}
1388
1389pub(crate) fn fit_polyline_of_degree_with_recovery(
1393 points: &[Vec3],
1394 tolerance: f64,
1395 maximum_points: usize,
1396 local_interpolation: bool,
1397 degree: usize,
1398) -> Result<(PolylineFit, bool), String> {
1399 let refine = std::env::var("BREP_POLYLINE_FIT_REFINE").as_deref() != Ok("0");
1400 let asked = fit_polyline_inner(points, tolerance, maximum_points, local_interpolation, refine, degree)?;
1401 if asked.report.met_tolerance() || !refine {
1402 record_polyline_fit(&asked.report);
1403 return Ok((asked, false));
1404 }
1405 let mut other =
1406 fit_polyline_inner(points, tolerance, maximum_points, !local_interpolation, refine, degree)?;
1407 let spent = asked.report.rungs + other.report.rungs;
1408 let mut best = if other.report.deviation < asked.report.deviation {
1409 other.report.lane_switched = true;
1410 other
1411 } else {
1412 asked
1413 };
1414 let mut spent = spent;
1419 let mut recovered = false;
1420 if !best.report.met_tolerance() {
1421 let (adaptive, rounds) = fit_polyline_adaptive(points, tolerance, maximum_points, degree)?;
1422 spent += rounds;
1423 if let Some(mut adaptive) = adaptive {
1424 adaptive.report.lane_switched = local_interpolation;
1425 best = adaptive;
1426 recovered = true;
1427 }
1428 }
1429 best.report.rungs = spent;
1431 record_polyline_fit(&best.report);
1432 Ok((best, recovered))
1433}
1434
1435fn fit_polyline_adaptive(
1439 points: &[Vec3],
1440 tolerance: f64,
1441 maximum_points: usize,
1442 degree: usize,
1443) -> Result<(Option<PolylineFit>, usize), String> {
1444 let pool = station_pool(points, tolerance);
1445 let arc = cumulative_chord(points);
1446 let band = dropped_band(points, &pool);
1447 let floor = tolerance.max(band);
1448 let budget = maximum_points.max(2);
1449 let ceiling = pool.len().min(MAX_FIT_STATIONS)
1450 .min(budget.saturating_mul(LADDER_BUDGET_FACTOR)).max(budget);
1451 let mut stations = decimate_stations(&pool, budget);
1452 for round in 0..=8 {
1455 let Ok((curve, current, parameters, deviation, rms, between)) =
1456 fit_station_set(points, stations, &arc, false, degree)
1457 else { return Ok((None, round + 1)); };
1458 stations = current;
1459 if deviation <= floor {
1460 let report = PolylineFitReport {
1461 deviation, rms, between_stations: between, tolerance, floor,
1462 stations: stations.len(), controls: curve.control_points.len(),
1463 input_points: points.len(), pool_points: pool.len(), dropped_band: band,
1464 rungs: round + 1, local_lane: false, lane_switched: false,
1465 exit: PolylineFitExit::Converged,
1466 };
1467 return Ok((Some(PolylineFit {
1468 curve, parameters, kept: stations.iter().map(|&i| points[i]).collect(), report,
1469 }), round + 1));
1470 }
1471 if stations.len() >= ceiling || round == 8 { return Ok((None, round + 1)); }
1472 let mut candidates = Vec::new();
1473 for span in 0..stations.len() - 1 {
1474 let (from, to) = (stations[span], stations[span + 1]);
1475 let (t0, t1) = (parameters[span], parameters[span + 1]);
1476 let lower = parameters[span.saturating_sub(1)];
1477 let upper = parameters[(span + 2).min(stations.len() - 1)];
1478 let mut candidate: Option<(usize, f64)> = None;
1479 for &index in pool.iter().filter(|&&i| i > from && i < to) {
1480 let length = arc[to] - arc[from];
1481 let seed = if length > 0.0 {
1482 t0 + (t1 - t0) * (arc[index] - arc[from]) / length
1483 } else { t0 };
1484 let error = distance_to_curve_near(&curve, points[index], seed, lower, upper)?;
1485 if candidate.is_none_or(|(_, previous)| error > previous) {
1486 candidate = Some((index, error));
1487 }
1488 }
1489 if let Some((index, error)) = candidate {
1490 if error > floor { candidates.push((index, error)); }
1491 }
1492 }
1493 candidates.sort_by(|a, b| b.1.total_cmp(&a.1).then_with(|| a.0.cmp(&b.0)));
1494 let added = candidates.len().min(16).min(ceiling - stations.len());
1495 if added == 0 { return Ok((None, round + 1)); }
1496 stations.extend(candidates.iter().take(added).map(|&(index, _)| index));
1497 stations.sort_unstable();
1498 }
1499 unreachable!("the bounded refinement returns on its last round")
1500}
1501
1502#[derive(Clone, Copy, Debug, PartialEq, Eq)]
1533pub(crate) enum SeededPath {
1534 Accepted { rounds: usize },
1536 Fallback(SeededFallback),
1538}
1539
1540#[derive(Clone, Copy, Debug, PartialEq, Eq)]
1542pub(crate) enum SeededFallback {
1543 NonFiniteInput,
1544 Degenerate,
1545 RefineDisabled,
1546 PoolTooSmall,
1547 NonFiniteBandOrArc,
1548 NonFiniteFloor,
1549 SeedsOverCeiling,
1550 SeedsOutsideStationPool,
1554 StationSetFailed,
1555 NonFiniteFitReading,
1556 SpanUnreadable,
1557 CeilingReached,
1558 RoundsSpent,
1559 NothingToAdd,
1560 EndpointUnreadable,
1563 TightStationOverTolerance,
1567}
1568
1569#[derive(Clone, Copy, Debug, Default, PartialEq)]
1572pub(crate) struct RepresentativeCost {
1573 pub(crate) representatives: usize,
1577 pub(crate) witnesses: usize,
1579 pub(crate) candidate_fits: usize,
1581 pub(crate) full_refit: bool,
1583 pub(crate) start_offset: f64,
1592 pub(crate) end_offset: f64,
1593}
1594
1595pub(crate) fn polyline_station_pool_floor(points: &[Vec3], tolerance: f64) -> f64 {
1602 let simplified = simplify_polyline_indices(points, tolerance);
1603 if simplified.len() > 2 {
1604 let total: f64 = simplified.windows(2).map(|pair| points[pair[1]].sub(points[pair[0]]).length()).sum();
1605 (tolerance * 0.01).max(total * 1e-4)
1606 } else {
1607 tolerance * 0.01
1608 }
1609}
1610
1611pub(crate) fn fit_polyline_seeded_with_path(
1612 points: &[Vec3],
1613 seeds: &[usize],
1614 tolerance: f64,
1615 full_budget: usize,
1616 local_interpolation: bool,
1617 degree: usize,
1618) -> Result<(PolylineFit, SeededPath), String> {
1619 let full_refit = |reason: SeededFallback| {
1620 fit_polyline_of_degree(points, tolerance, full_budget, local_interpolation, degree)
1621 .map(|fit| (fit, SeededPath::Fallback(reason)))
1622 };
1623 if !tolerance.is_finite() || points.iter().any(|p| !p.x.is_finite() || !p.y.is_finite() || !p.z.is_finite()) {
1625 return full_refit(SeededFallback::NonFiniteInput);
1626 }
1627 if points.len() < 3 {
1628 return full_refit(SeededFallback::Degenerate);
1629 }
1630 if std::env::var("BREP_POLYLINE_FIT_REFINE").as_deref() == Ok("0") {
1631 return full_refit(SeededFallback::RefineDisabled);
1632 }
1633 let pool = station_pool(points, tolerance);
1634 if pool.len() < 2 {
1635 return full_refit(SeededFallback::PoolTooSmall);
1636 }
1637 let arc = cumulative_chord(points);
1638 let band = dropped_band(points, &pool);
1639 if !band.is_finite() || arc.iter().any(|value| !value.is_finite()) {
1640 return full_refit(SeededFallback::NonFiniteBandOrArc);
1641 }
1642 let floor = tolerance.max(band);
1643 if !floor.is_finite() {
1644 return full_refit(SeededFallback::NonFiniteFloor);
1645 }
1646 let budget = full_budget.max(2);
1649 let ceiling = pool
1650 .len()
1651 .min(MAX_FIT_STATIONS)
1652 .min(budget.saturating_mul(LADDER_BUDGET_FACTOR))
1653 .max(budget);
1654 let mut stations = seeds
1655 .iter()
1656 .copied()
1657 .filter(|&index| index < points.len())
1658 .chain([0, points.len() - 1])
1659 .collect::<Vec<_>>();
1660 stations.sort_unstable();
1661 stations.dedup();
1662 if stations.len() > ceiling {
1663 return full_refit(SeededFallback::SeedsOverCeiling);
1664 }
1665 if stations.iter().any(|index| pool.binary_search(index).is_err()) {
1683 return full_refit(SeededFallback::SeedsOutsideStationPool);
1684 }
1685 for round in 0..=8 {
1686 let Ok((curve, current, parameters, deviation, rms, between)) =
1687 fit_station_set(points, stations, &arc, local_interpolation, degree)
1688 else {
1689 return full_refit(SeededFallback::StationSetFailed);
1690 };
1691 stations = current;
1692 if !deviation.is_finite() || !rms.is_finite() || !between.is_finite() {
1693 return full_refit(SeededFallback::NonFiniteFitReading);
1694 }
1695 if deviation <= floor {
1696 let report = PolylineFitReport {
1697 deviation,
1698 rms,
1699 between_stations: between,
1700 tolerance,
1701 floor,
1702 stations: stations.len(),
1703 controls: curve.control_points.len(),
1704 input_points: points.len(),
1705 pool_points: pool.len(),
1706 dropped_band: band,
1707 rungs: round + 1,
1708 local_lane: local_interpolation,
1709 lane_switched: false,
1710 exit: PolylineFitExit::Converged,
1711 };
1712 record_polyline_fit(&report);
1713 let kept = stations.iter().map(|&index| points[index]).collect();
1714 return Ok((PolylineFit { curve, parameters, kept, report }, SeededPath::Accepted { rounds: round + 1 }));
1715 }
1716 if stations.len() >= ceiling {
1717 return full_refit(SeededFallback::CeilingReached);
1718 }
1719 if round == 8 {
1720 return full_refit(SeededFallback::RoundsSpent);
1721 }
1722 let mut candidates = Vec::new();
1723 for span in 0..stations.len() - 1 {
1724 let (from, to) = (stations[span], stations[span + 1]);
1725 let (t0, t1) = (parameters[span], parameters[span + 1]);
1726 let lower = parameters[span.saturating_sub(1)];
1727 let upper = parameters[(span + 2).min(stations.len() - 1)];
1728 let mut candidate: Option<(usize, f64)> = None;
1729 for &index in pool.iter().filter(|&&i| i > from && i < to) {
1730 let length = arc[to] - arc[from];
1731 let seed = if length > 0.0 { t0 + (t1 - t0) * (arc[index] - arc[from]) / length } else { t0 };
1732 let Ok(error) = distance_to_curve_near(&curve, points[index], seed, lower, upper) else {
1735 return full_refit(SeededFallback::SpanUnreadable);
1736 };
1737 if !error.is_finite() {
1738 return full_refit(SeededFallback::SpanUnreadable);
1739 }
1740 if candidate.is_none_or(|(_, previous)| error > previous) {
1741 candidate = Some((index, error));
1742 }
1743 }
1744 if let Some((index, error)) = candidate {
1745 if error > floor {
1746 candidates.push((index, error));
1747 }
1748 }
1749 }
1750 candidates.sort_by(|a, b| b.1.total_cmp(&a.1).then_with(|| a.0.cmp(&b.0)));
1751 let added = candidates.len().min(16).min(ceiling - stations.len());
1752 if added == 0 {
1753 return full_refit(SeededFallback::NothingToAdd);
1754 }
1755 stations.extend(candidates.iter().take(added).map(|&(index, _)| index));
1756 stations.sort_unstable();
1757 }
1758 full_refit(SeededFallback::RoundsSpent)
1760}
1761
1762pub(crate) fn fit_polyline_representatives_with_path(
1782 points: &[Vec3],
1783 seeds: &[usize],
1784 tolerance: f64,
1785 full_budget: usize,
1786 local_interpolation: bool,
1787 degree: usize,
1788) -> Result<(PolylineFit, SeededPath, RepresentativeCost), String> {
1789 fit_polyline_representatives_with_witnesses(points, seeds, &[], tolerance, tolerance, full_budget, local_interpolation, degree)
1790}
1791
1792#[allow(clippy::too_many_arguments)]
1806pub(crate) fn fit_polyline_representatives_with_witnesses(
1807 points: &[Vec3],
1808 seeds: &[usize],
1809 tight: &[usize],
1810 tight_tolerance: f64,
1811 tolerance: f64,
1812 full_budget: usize,
1813 local_interpolation: bool,
1814 degree: usize,
1815) -> Result<(PolylineFit, SeededPath, RepresentativeCost), String> {
1816 let mut cost = RepresentativeCost { witnesses: points.len(), start_offset: f64::NAN, end_offset: f64::NAN, ..RepresentativeCost::default() };
1817 let full_refit = |reason: SeededFallback, mut cost: RepresentativeCost| {
1818 cost.full_refit = true;
1819 fit_polyline_of_degree(points, tolerance, full_budget, local_interpolation, degree)
1820 .map(|fit| (fit, SeededPath::Fallback(reason), cost))
1821 };
1822 if !tolerance.is_finite()
1823 || !tight_tolerance.is_finite()
1824 || points.iter().any(|p| !p.x.is_finite() || !p.y.is_finite() || !p.z.is_finite())
1825 {
1826 return full_refit(SeededFallback::NonFiniteInput, cost);
1827 }
1828 if points.len() < 3 {
1829 return full_refit(SeededFallback::Degenerate, cost);
1830 }
1831 if std::env::var("BREP_POLYLINE_FIT_REFINE").as_deref() == Ok("0") {
1832 return full_refit(SeededFallback::RefineDisabled, cost);
1833 }
1834 let mut tight: Vec<usize> = tight.iter().copied().filter(|&index| index < points.len()).collect();
1835 tight.sort_unstable();
1836 tight.dedup();
1837 let pool = station_pool(points, tolerance);
1838 if pool.len() < 2 || pool[0] != 0 || pool[pool.len() - 1] != points.len() - 1 {
1839 return full_refit(SeededFallback::PoolTooSmall, cost);
1840 }
1841 let arc = cumulative_chord(points);
1842 let band = dropped_band(points, &pool);
1843 if !band.is_finite() || arc.iter().any(|value| !value.is_finite()) {
1844 return full_refit(SeededFallback::NonFiniteBandOrArc, cost);
1845 }
1846 let budget = full_budget.max(2);
1847 let ceiling = pool
1848 .len()
1849 .min(MAX_FIT_STATIONS)
1850 .min(budget.saturating_mul(LADDER_BUDGET_FACTOR))
1851 .max(budget);
1852 let mut stations = seeds
1853 .iter()
1854 .copied()
1855 .filter(|index| pool.binary_search(index).is_ok())
1856 .chain([0, points.len() - 1])
1857 .collect::<Vec<_>>();
1858 stations.sort_unstable();
1859 stations.dedup();
1860 cost.representatives = stations.len();
1861 if stations.len() > ceiling {
1862 return full_refit(SeededFallback::SeedsOverCeiling, cost);
1863 }
1864 for round in 0..=8 {
1865 cost.candidate_fits += 1;
1866 let Ok((curve, current, parameters, deviation, rms, between)) =
1867 fit_station_set(points, stations, &arc, local_interpolation, degree)
1868 else {
1869 return full_refit(SeededFallback::StationSetFailed, cost);
1870 };
1871 stations = current;
1872 if !deviation.is_finite() || !rms.is_finite() || !between.is_finite() {
1873 return full_refit(SeededFallback::NonFiniteFitReading, cost);
1874 }
1875 let mut tight_over: Vec<usize> = Vec::new();
1878 for &index in &tight {
1879 let span = match stations.binary_search(&index) {
1880 Ok(position) => {
1881 let distance = curve.evaluate(parameters[position]).map(|p| p.sub(points[index]).length());
1884 match distance {
1885 Ok(distance) if distance.is_finite() => {
1886 if distance > tight_tolerance {
1887 return full_refit(SeededFallback::TightStationOverTolerance, cost);
1888 }
1889 }
1890 _ => return full_refit(SeededFallback::SpanUnreadable, cost),
1891 }
1892 continue;
1893 }
1894 Err(at) if at == 0 || at >= stations.len() => return full_refit(SeededFallback::SpanUnreadable, cost),
1897 Err(at) => at - 1,
1898 };
1899 let (from, to) = (stations[span], stations[span + 1]);
1900 let (t0, t1) = (parameters[span], parameters[span + 1]);
1901 let length = arc[to] - arc[from];
1902 let seed = if length > 0.0 { t0 + (t1 - t0) * (arc[index] - arc[from]) / length } else { t0 };
1903 let lower = parameters[span.saturating_sub(1)];
1904 let upper = parameters[(span + 2).min(stations.len() - 1)];
1905 let Ok(error) = distance_to_curve_near(&curve, points[index], seed, lower, upper) else {
1906 return full_refit(SeededFallback::SpanUnreadable, cost);
1907 };
1908 if !error.is_finite() {
1909 return full_refit(SeededFallback::SpanUnreadable, cost);
1910 }
1911 if error > tight_tolerance {
1912 tight_over.push(index);
1913 }
1914 }
1915 if deviation <= tolerance && tight_over.is_empty() {
1916 let ends = curve
1921 .domain()
1922 .and_then(|[t0, t1]| Ok((curve.evaluate(t0)?, curve.evaluate(t1)?)));
1923 let Ok((start, end)) = ends else {
1924 return full_refit(SeededFallback::EndpointUnreadable, cost);
1925 };
1926 cost.start_offset = start.sub(points[0]).length();
1927 cost.end_offset = end.sub(points[points.len() - 1]).length();
1928 if !(cost.start_offset.is_finite() && cost.end_offset.is_finite()) {
1929 cost.start_offset = f64::NAN;
1930 cost.end_offset = f64::NAN;
1931 return full_refit(SeededFallback::EndpointUnreadable, cost);
1932 }
1933 let report = PolylineFitReport {
1934 deviation,
1935 rms,
1936 between_stations: between,
1937 tolerance,
1938 floor: tolerance,
1939 stations: stations.len(),
1940 controls: curve.control_points.len(),
1941 input_points: points.len(),
1942 pool_points: pool.len(),
1943 dropped_band: band,
1944 rungs: round + 1,
1945 local_lane: local_interpolation,
1946 lane_switched: false,
1947 exit: PolylineFitExit::Converged,
1948 };
1949 record_polyline_fit(&report);
1950 let kept = stations.iter().map(|&index| points[index]).collect();
1951 return Ok((PolylineFit { curve, parameters, kept, report }, SeededPath::Accepted { rounds: round + 1 }, cost));
1952 }
1953 if stations.len() >= ceiling {
1954 return full_refit(SeededFallback::CeilingReached, cost);
1955 }
1956 if round == 8 {
1957 return full_refit(SeededFallback::RoundsSpent, cost);
1958 }
1959 let mut candidates = Vec::new();
1960 for span in 0..stations.len() - 1 {
1961 let (from, to) = (stations[span], stations[span + 1]);
1962 let (t0, t1) = (parameters[span], parameters[span + 1]);
1963 let lower = parameters[span.saturating_sub(1)];
1964 let upper = parameters[(span + 2).min(stations.len() - 1)];
1965 let mut candidate: Option<(usize, f64)> = None;
1966 for &index in pool.iter().filter(|&&i| i > from && i < to) {
1967 let length = arc[to] - arc[from];
1968 let seed = if length > 0.0 { t0 + (t1 - t0) * (arc[index] - arc[from]) / length } else { t0 };
1969 let Ok(error) = distance_to_curve_near(&curve, points[index], seed, lower, upper) else {
1970 return full_refit(SeededFallback::SpanUnreadable, cost);
1971 };
1972 if !error.is_finite() {
1973 return full_refit(SeededFallback::SpanUnreadable, cost);
1974 }
1975 if candidate.is_none_or(|(_, previous)| error > previous) {
1976 candidate = Some((index, error));
1977 }
1978 }
1979 if let Some((index, error)) = candidate {
1980 if error > tolerance {
1981 candidates.push((index, error));
1982 }
1983 }
1984 }
1985 for &index in &tight_over {
1988 let at = stations.binary_search(&index).unwrap_err();
1989 let (from, to) = (stations[at - 1], stations[at]);
1990 let nearest = pool
1991 .iter()
1992 .copied()
1993 .filter(|&member| member > from && member < to)
1994 .min_by(|&a, &b| (arc[a] - arc[index]).abs().total_cmp(&(arc[b] - arc[index]).abs()).then_with(|| a.cmp(&b)));
1995 if let Some(member) = nearest {
1996 match candidates.iter_mut().find(|(chosen, _)| *chosen == member) {
1999 Some(entry) => entry.1 = f64::INFINITY,
2000 None => candidates.push((member, f64::INFINITY)),
2001 }
2002 }
2003 }
2004 candidates.sort_by(|a, b| b.1.total_cmp(&a.1).then_with(|| a.0.cmp(&b.0)));
2005 let added = candidates.len().min(16).min(ceiling - stations.len());
2006 if added == 0 {
2007 return full_refit(SeededFallback::NothingToAdd, cost);
2010 }
2011 stations.extend(candidates.iter().take(added).map(|&(index, _)| index));
2012 stations.sort_unstable();
2013 }
2014 full_refit(SeededFallback::RoundsSpent, cost)
2015}
2016
2017
2018fn fit_polyline_inner(
2019 points: &[Vec3],
2020 tolerance: f64,
2021 maximum_points: usize,
2022 local_interpolation: bool,
2023 refine: bool,
2024 degree: usize,
2025) -> Result<PolylineFit, String> {
2026 let pool = station_pool(points, tolerance);
2027 if pool.len() < 2 {
2028 return Err("fit_polyline: degenerate polyline".into());
2029 }
2030 let arc = cumulative_chord(points);
2031 let band = dropped_band(points, &pool);
2032 let floor = tolerance.max(band);
2033 let mut budget = maximum_points.max(2);
2034 let ceiling = pool
2035 .len()
2036 .min(MAX_FIT_STATIONS)
2037 .min(budget.saturating_mul(LADDER_BUDGET_FACTOR))
2038 .max(budget);
2039 let (mut curve, mut stations, mut parameters, mut deviation, mut rms, mut between) =
2040 fit_rung(points, &pool, &arc, budget, local_interpolation, degree)?;
2041 let mut rungs = 1usize;
2042 let mut exit = if deviation <= floor {
2043 PolylineFitExit::Converged
2044 } else {
2045 PolylineFitExit::Unrefined
2046 };
2047 while refine && deviation > floor {
2048 if budget >= ceiling {
2049 exit = if ceiling >= pool.len() {
2050 PolylineFitExit::InputExhausted
2051 } else {
2052 PolylineFitExit::StationCeiling
2053 };
2054 break;
2055 }
2056 budget = (budget * 2).min(ceiling);
2057 let rung = fit_rung(points, &pool, &arc, budget, local_interpolation, degree)?;
2058 rungs += 1;
2059 if rung.3 >= deviation * STALL_FACTOR {
2060 if rung.3 < deviation {
2061 (curve, stations, parameters, deviation, rms, between) = rung;
2062 }
2063 exit = if deviation <= floor {
2067 PolylineFitExit::Converged
2068 } else {
2069 PolylineFitExit::Stalled
2070 };
2071 break;
2072 }
2073 (curve, stations, parameters, deviation, rms, between) = rung;
2074 if deviation <= floor {
2075 exit = PolylineFitExit::Converged;
2076 }
2077 }
2078 let report = PolylineFitReport {
2079 deviation,
2080 rms,
2081 between_stations: between,
2082 tolerance,
2083 floor,
2084 stations: stations.len(),
2085 controls: curve.control_points.len(),
2086 input_points: points.len(),
2087 pool_points: pool.len(),
2088 dropped_band: band,
2089 rungs,
2090 local_lane: local_interpolation,
2091 lane_switched: false,
2092 exit,
2093 };
2094 Ok(PolylineFit {
2095 curve,
2096 parameters,
2097 kept: stations.iter().map(|index| points[*index]).collect(),
2098 report,
2099 })
2100}
2101
2102#[derive(Clone, Copy, Debug, Default, PartialEq)]
2115pub struct PolylineFitLedger {
2116 pub fits: u64,
2118 pub unmet: u64,
2120 pub station_ceiling: u64,
2122 pub input_exhausted: u64,
2124 pub stalled: u64,
2126 pub unrefined: u64,
2128 pub refined: u64,
2130 pub met_at_pool: u64,
2133 pub lane_switched: u64,
2136 pub extra_rungs: u64,
2138 pub worst_unmet_deviation: f64,
2140 pub worst_dropped_band: f64,
2142}
2143
2144impl PolylineFitLedger {
2145 fn record(&mut self, report: &PolylineFitReport) {
2146 self.fits += 1;
2147 self.extra_rungs += (report.rungs - 1) as u64;
2148 if report.rungs > 1 {
2149 self.refined += 1;
2150 }
2151 self.worst_dropped_band = self.worst_dropped_band.max(report.dropped_band);
2152 if report.lane_switched {
2153 self.lane_switched += 1;
2154 }
2155 if report.met_tolerance() {
2156 if report.saturated() {
2157 self.met_at_pool += 1;
2158 }
2159 return;
2160 }
2161 self.unmet += 1;
2162 match report.exit {
2163 PolylineFitExit::Converged => {}
2164 PolylineFitExit::StationCeiling => self.station_ceiling += 1,
2165 PolylineFitExit::InputExhausted => self.input_exhausted += 1,
2166 PolylineFitExit::Stalled => self.stalled += 1,
2167 PolylineFitExit::Unrefined => self.unrefined += 1,
2168 }
2169 self.worst_unmet_deviation = self.worst_unmet_deviation.max(report.deviation);
2170 }
2171
2172 fn fold(&mut self, child: &PolylineFitLedger) {
2173 self.fits += child.fits;
2174 self.unmet += child.unmet;
2175 self.station_ceiling += child.station_ceiling;
2176 self.input_exhausted += child.input_exhausted;
2177 self.stalled += child.stalled;
2178 self.unrefined += child.unrefined;
2179 self.refined += child.refined;
2180 self.met_at_pool += child.met_at_pool;
2181 self.lane_switched += child.lane_switched;
2182 self.extra_rungs += child.extra_rungs;
2183 self.worst_unmet_deviation = self.worst_unmet_deviation.max(child.worst_unmet_deviation);
2184 self.worst_dropped_band = self.worst_dropped_band.max(child.worst_dropped_band);
2185 }
2186
2187 pub fn report_into(&self, diagnostics: &mut KernelDiagnostics) {
2194 diagnostics.count_n("fit.polyline.fits", self.fits);
2195 diagnostics.count_n("fit.polyline.unmet_tolerance", self.unmet);
2196 diagnostics.count_n("fit.polyline.exit.station_ceiling", self.station_ceiling);
2197 diagnostics.count_n("fit.polyline.exit.input_exhausted", self.input_exhausted);
2198 diagnostics.count_n("fit.polyline.exit.stalled", self.stalled);
2199 diagnostics.count_n("fit.polyline.exit.unrefined", self.unrefined);
2200 diagnostics.count_n("fit.polyline.refined", self.refined);
2201 diagnostics.count_n("fit.polyline.met_at_pool", self.met_at_pool);
2202 diagnostics.count_n("fit.polyline.lane_switched", self.lane_switched);
2203 diagnostics.count_n("fit.polyline.extra_rungs", self.extra_rungs);
2204 diagnostics.measure_max("fit.polyline.worst_unmet_deviation", self.worst_unmet_deviation);
2205 diagnostics.measure_max("fit.polyline.worst_dropped_band", self.worst_dropped_band);
2206 if self.unmet > 0 {
2207 diagnostics.event(
2208 DiagnosticSeverity::Degraded,
2209 KernelStage::Intersect,
2210 "fit.polyline.unmet_tolerance",
2211 format!(
2212 "{} of {} polyline fits did not reach the tolerance they were given \
2213 (station ceiling {}, input exhausted {}, stalled {}, unrefined {}); \
2214 worst deviation {:.3e}; the simplification's own band reaches {:.3e}",
2215 self.unmet,
2216 self.fits,
2217 self.station_ceiling,
2218 self.input_exhausted,
2219 self.stalled,
2220 self.unrefined,
2221 self.worst_unmet_deviation,
2222 self.worst_dropped_band,
2223 ),
2224 );
2225 }
2226 }
2227}
2228
2229thread_local! {
2230 static POLYLINE_FIT_SCOPES: std::cell::RefCell<Vec<PolylineFitLedger>> =
2232 const { std::cell::RefCell::new(Vec::new()) };
2233}
2234
2235pub struct PolylineFitScope {
2242 _thread_bound: std::marker::PhantomData<*const ()>,
2244}
2245
2246impl PolylineFitScope {
2247 pub fn open() -> Self {
2248 POLYLINE_FIT_SCOPES.with(|scopes| scopes.borrow_mut().push(PolylineFitLedger::default()));
2249 Self {
2250 _thread_bound: std::marker::PhantomData,
2251 }
2252 }
2253
2254 pub fn close(self) -> PolylineFitLedger {
2256 let ledger = POLYLINE_FIT_SCOPES.with(|scopes| {
2257 let mut scopes = scopes.borrow_mut();
2258 let ledger = scopes.pop().unwrap_or_default();
2259 if let Some(parent) = scopes.last_mut() {
2260 parent.fold(&ledger);
2261 }
2262 ledger
2263 });
2264 std::mem::forget(self);
2265 ledger
2266 }
2267}
2268
2269impl Drop for PolylineFitScope {
2270 fn drop(&mut self) {
2271 POLYLINE_FIT_SCOPES.with(|scopes| {
2272 scopes.borrow_mut().pop();
2273 });
2274 }
2275}
2276
2277fn record_polyline_fit(report: &PolylineFitReport) {
2278 if std::env::var_os("BREP_DEBUG_POLYLINE_FIT").is_some() {
2281 eprintln!("POLYLINE-FIT {report:?}");
2282 }
2283 POLYLINE_FIT_SCOPES.with(|scopes| {
2284 if let Some(open) = scopes.borrow_mut().last_mut() {
2285 open.record(report);
2286 }
2287 });
2288}
2289
2290
2291
2292fn raw_basis(knots: &[f64], degree: usize, span: usize, parameter: f64) -> Vec<f64> {
2296 let mut basis = vec![0.0; degree + 1];
2297 let mut left = vec![0.0; degree + 1];
2298 let mut right = vec![0.0; degree + 1];
2299 basis[0] = 1.0;
2300 for j in 1..=degree {
2301 left[j] = parameter - knots[span + 1 - j];
2302 right[j] = knots[span + j] - parameter;
2303 let mut saved = 0.0;
2304 for r in 0..j {
2305 let denominator = right[r + 1] + left[j - r];
2306 let temp = if denominator.abs() > 0.0 {
2307 basis[r] / denominator
2308 } else {
2309 0.0
2310 };
2311 basis[r] = saved + right[r + 1] * temp;
2312 saved = left[j - r] * temp;
2313 }
2314 basis[j] = saved;
2315 }
2316 basis
2317}
2318
2319fn raw_insert_knot(knots: &mut Vec<f64>, controls: &mut Vec<Vec3>, degree: usize, parameter: f64) {
2321 let span = knots
2325 .iter()
2326 .rposition(|&knot| knot <= parameter + 1e-14)
2327 .unwrap()
2328 .min(controls.len() - 1);
2329 let mut fresh = Vec::with_capacity(controls.len() + 1);
2330 fresh.extend_from_slice(&controls[..=span - degree]);
2331 for i in span - degree + 1..=span {
2332 let denominator = knots[i + degree] - knots[i];
2333 let alpha = if denominator.abs() > 0.0 {
2334 (parameter - knots[i]) / denominator
2335 } else {
2336 0.0
2337 };
2338 fresh.push(
2339 controls[i - 1]
2340 .scale(1.0 - alpha)
2341 .add(controls[i].scale(alpha)),
2342 );
2343 }
2344 fresh.extend_from_slice(&controls[span..]);
2345 *controls = fresh;
2346 knots.insert(span + 1, parameter);
2347}
2348
2349pub fn interpolate_curve_closed(points: &[Vec3], parameters: &[f64]) -> Result<NurbsCurve, String> {
2357 interpolate_curve_closed_of_degree(points, parameters, 3)
2358}
2359
2360pub fn interpolate_curve_closed_of_degree(
2366 points: &[Vec3],
2367 parameters: &[f64],
2368 degree: usize,
2369) -> Result<NurbsCurve, String> {
2370 if degree % 2 == 0 || degree < 3 {
2371 return Err("interpolate_curve_closed: the degree must be odd and at least 3".into());
2372 }
2373 let station_count = points.len();
2374 if station_count < degree + 1 {
2375 return Err("interpolate_curve_closed: need more stations than the degree".into());
2376 }
2377 if parameters.len() != station_count + 1 {
2378 return Err(
2379 "interpolate_curve_closed: parameters must have one more entry than points".into(),
2380 );
2381 }
2382 if parameters.windows(2).any(|pair| pair[1] <= pair[0]) {
2383 return Err("interpolate_curve_closed: parameters must increase".into());
2384 }
2385 let period = parameters[station_count] - parameters[0];
2386 let cyclic = |j: i64| -> f64 {
2389 let s = station_count as i64;
2390 let wrap = j.div_euclid(s);
2391 parameters[j.rem_euclid(s) as usize] + wrap as f64 * period
2392 };
2393 let raw_knots: Vec<f64> = (-(degree as i64)..=(station_count as i64 + degree as i64)).map(cyclic).collect();
2394 let mut matrix = vec![vec![0.0; station_count]; station_count];
2397 for i in 0..station_count {
2398 let span = i + degree;
2399 let basis = raw_basis(&raw_knots, degree, span, parameters[i]);
2400 for (offset, value) in basis.iter().enumerate() {
2401 let index = (i as i64 + offset as i64 - degree as i64).rem_euclid(station_count as i64) as usize;
2404 matrix[i][index] += value;
2405 }
2406 }
2407 let solve_axis = |axis: fn(Vec3) -> f64| {
2408 solve_dense(
2409 matrix.clone(),
2410 points.iter().copied().map(axis).collect::<Vec<_>>(),
2411 )
2412 };
2413 let xs = solve_axis(|point| point.x)?;
2414 let ys = solve_axis(|point| point.y)?;
2415 let zs = solve_axis(|point| point.z)?;
2416 let cyclic_controls: Vec<Vec3> = (0..station_count)
2417 .map(|index| Vec3::new(xs[index], ys[index], zs[index]))
2418 .collect();
2419 let mut window_controls: Vec<Vec3> = (-(degree as i64)..(station_count as i64))
2421 .map(|j| cyclic_controls[j.rem_euclid(station_count as i64) as usize])
2422 .collect();
2423 let mut window_knots = raw_knots.clone();
2424 for _ in 0..degree {
2427 raw_insert_knot(
2428 &mut window_knots,
2429 &mut window_controls,
2430 degree,
2431 parameters[0],
2432 );
2433 }
2434 for _ in 0..degree {
2435 raw_insert_knot(
2436 &mut window_knots,
2437 &mut window_controls,
2438 degree,
2439 parameters[station_count],
2440 );
2441 }
2442 let first = window_knots
2446 .iter()
2447 .position(|&knot| (knot - parameters[0]).abs() < 1e-12)
2448 .ok_or("interpolate_curve_closed: clamp lost the start knot")?;
2449 let last = window_knots
2450 .iter()
2451 .rposition(|&knot| (knot - parameters[station_count]).abs() < 1e-12)
2452 .ok_or("interpolate_curve_closed: clamp lost the end knot")?;
2453 let clamped_knots: Vec<f64> = window_knots[first..=last].to_vec();
2454 let control_count = clamped_knots.len() - degree - 1;
2455 let clamped_controls: Vec<Vec4> = window_controls[first..first + control_count]
2456 .iter()
2457 .map(|point| Vec4::from_point(*point, 1.0))
2458 .collect();
2459 NurbsCurve::new(degree, clamped_knots, clamped_controls)
2460}
2461