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(crate) fn solve_dense_shared<const K: usize>(
63 mut matrix: Vec<Vec<f64>>,
64 mut rhs: [Vec<f64>; K],
65) -> Result<[Vec<f64>; K], String> {
66 let count = rhs.first().map_or(0, Vec::len);
67 if count == 0
68 || matrix.len() != count
69 || matrix.iter().any(|row| row.len() != count)
70 || rhs.iter().any(|side| side.len() != count)
71 {
72 return Err("solve_dense: matrix must be square and match RHS".into());
73 }
74 let scale = matrix
75 .iter()
76 .flatten()
77 .fold(0.0_f64, |maximum, value| maximum.max(value.abs()));
78 if scale == 0.0 {
79 return Err("solve_dense: singular matrix".into());
80 }
81 let mut pivot_row: Vec<(usize, f64)> = Vec::with_capacity(count);
82 for column in 0..count {
83 let pivot = (column..count)
84 .max_by(|&a, &b| matrix[a][column].abs().total_cmp(&matrix[b][column].abs()))
85 .unwrap();
86 if matrix[pivot][column].abs() <= 1e-13 * scale {
87 return Err("solve_dense: singular matrix".into());
88 }
89 matrix.swap(column, pivot);
90 for side in rhs.iter_mut() {
91 side.swap(column, pivot);
92 }
93 let diagonal = matrix[column][column];
94 pivot_row.clear();
95 pivot_row.extend(
96 (column + 1..count)
97 .map(|entry| (entry, matrix[column][entry]))
98 .filter(|&(_, value)| value != 0.0),
99 );
100 for row in column + 1..count {
101 let factor = matrix[row][column] / diagonal;
102 matrix[row][column] = 0.0;
103 if factor != 0.0 {
104 let target = &mut matrix[row];
105 for &(entry, value) in &pivot_row {
106 target[entry] -= factor * value;
107 }
108 }
109 for side in rhs.iter_mut() {
110 let subtrahend = factor * side[column];
111 side[row] -= subtrahend;
112 }
113 }
114 }
115 let mut results: [Vec<f64>; K] = std::array::from_fn(|_| vec![0.0; count]);
116 for (side, result) in rhs.iter().zip(results.iter_mut()) {
117 for row in (0..count).rev() {
118 let remainder: f64 = (row + 1..count)
119 .map(|column| matrix[row][column] * result[column])
120 .sum();
121 result[row] = (side[row] - remainder) / matrix[row][row];
122 }
123 }
124 Ok(results)
125}
126
127pub fn solve_small<const N: usize>(
131 mut matrix: [[f64; N]; N],
132 mut rhs: [f64; N],
133 count: usize,
134) -> Result<[f64; N], String> {
135 if count == 0 || count > N {
136 return Err("solve_small: invalid system size".into());
137 }
138 let scale = matrix
139 .iter()
140 .take(count)
141 .flat_map(|row| row.iter().take(count))
142 .fold(0.0_f64, |maximum, value| maximum.max(value.abs()));
143 if scale == 0.0 {
144 return Err("solve_small: singular matrix".into());
145 }
146 for column in 0..count {
147 let pivot = (column..count)
148 .max_by(|&a, &b| matrix[a][column].abs().total_cmp(&matrix[b][column].abs()))
149 .unwrap();
150 if matrix[pivot][column].abs() <= 1e-13 * scale {
151 return Err("solve_small: singular matrix".into());
152 }
153 matrix.swap(column, pivot);
154 rhs.swap(column, pivot);
155 let diagonal = matrix[column][column];
156 for row in column + 1..count {
157 let factor = matrix[row][column] / diagonal;
158 matrix[row][column] = 0.0;
159 for entry in column + 1..count {
160 matrix[row][entry] -= factor * matrix[column][entry];
161 }
162 rhs[row] -= factor * rhs[column];
163 }
164 }
165 let mut result = [0.0; N];
166 for row in (0..count).rev() {
167 let remainder: f64 = (row + 1..count)
168 .map(|column| matrix[row][column] * result[column])
169 .sum();
170 result[row] = (rhs[row] - remainder) / matrix[row][row];
171 }
172 Ok(result)
173}
174
175pub fn solve_collocation(
186 matrix: &[Vec<f64>],
187 rhs: &[f64],
188 degree: usize,
189) -> Result<Vec<f64>, String> {
190 let count = rhs.len();
191 let bandwidth = degree.max(1).min(count.saturating_sub(1).max(1));
192 if let Ok(solution) = solve_banded(matrix, rhs, bandwidth) {
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 += matrix[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(matrix.to_vec(), rhs.to_vec())
212}
213
214pub(crate) struct CollocationBand {
223 count: usize,
224 bandwidth: usize,
225 data: Vec<f64>,
226}
227
228impl CollocationBand {
229 pub(crate) fn new(count: usize, degree: usize) -> Self {
232 let bandwidth = degree.max(1).min(count.saturating_sub(1).max(1));
233 CollocationBand { count, bandwidth, data: vec![0.0; count * (2 * bandwidth + 1)] }
234 }
235
236 fn slot(&self, row: usize, column: usize) -> Option<usize> {
237 let offset = column as isize - row as isize + self.bandwidth as isize;
238 (offset >= 0 && offset <= 2 * self.bandwidth as isize)
239 .then(|| row * (2 * self.bandwidth + 1) + offset as usize)
240 }
241
242 pub(crate) fn set(&mut self, row: usize, column: usize, value: f64) -> bool {
245 match self.slot(row, column) {
246 Some(index) => {
247 self.data[index] = value;
248 true
249 }
250 None => value == 0.0,
251 }
252 }
253
254 fn get(&self, row: usize, column: usize) -> f64 {
255 self.slot(row, column).map_or(0.0, |index| self.data[index])
256 }
257
258 fn to_dense(&self) -> Vec<Vec<f64>> {
259 (0..self.count)
260 .map(|row| (0..self.count).map(|column| self.get(row, column)).collect())
261 .collect()
262 }
263}
264
265pub(crate) fn solve_collocation_banded(band: &CollocationBand, rhs: &[f64]) -> Result<Vec<f64>, String> {
270 let count = rhs.len();
271 if count == 0 || band.count != count {
272 return Err("solve_banded: matrix must be square and match RHS".into());
273 }
274 let bandwidth = band.bandwidth;
275 if let Ok(solution) = solve_band_storage(band, rhs) {
276 let scale = rhs
277 .iter()
278 .fold(0.0_f64, |maximum, value| maximum.max(value.abs()));
279 let tolerance = 1e-9 * scale.max(1.0);
280 let mut residual = 0.0_f64;
281 for row in 0..count {
282 let lo = row.saturating_sub(bandwidth);
283 let hi = (row + bandwidth).min(count - 1);
284 let mut accumulated = 0.0;
285 for col in lo..=hi {
286 accumulated += band.get(row, col) * solution[col];
287 }
288 residual = residual.max((accumulated - rhs[row]).abs());
289 }
290 if residual <= tolerance {
291 return Ok(solution);
292 }
293 }
294 solve_dense(band.to_dense(), rhs.to_vec())
295}
296
297fn solve_band_storage(band: &CollocationBand, rhs: &[f64]) -> Result<Vec<f64>, String> {
298 let count = rhs.len();
299 let bandwidth = band.bandwidth;
300 let width = 2 * bandwidth + 1;
301 let mut matrix = band.data.clone();
302 let mut rhs = rhs.to_vec();
303 let at = |row: usize, column: usize| row * width + column + bandwidth - row;
305 for column in 0..count {
306 let pivot = matrix[at(column, column)];
307 if pivot.abs() <= 1e-300 {
308 return Err("solve_banded: singular matrix".into());
309 }
310 let maximum_row = (column + bandwidth).min(count - 1);
311 for row in column + 1..=maximum_row {
312 let factor = matrix[at(row, column)] / pivot;
313 if factor == 0.0 {
314 continue;
315 }
316 let maximum_column = (column + bandwidth).min(count - 1);
317 for entry in column..=maximum_column {
318 matrix[at(row, entry)] -= factor * matrix[at(column, entry)];
319 }
320 rhs[row] -= factor * rhs[column];
321 }
322 }
323 let mut result = vec![0.0; count];
324 for row in (0..count).rev() {
325 let mut value = rhs[row];
326 let maximum_column = (row + bandwidth).min(count - 1);
327 for column in row + 1..=maximum_column {
328 value -= matrix[at(row, column)] * result[column];
329 }
330 if matrix[at(row, row)].abs() <= 1e-300 {
331 return Err("solve_banded: singular matrix".into());
332 }
333 result[row] = value / matrix[at(row, row)];
334 }
335 Ok(result)
336}
337
338fn solve_collocation_banded_axes(
342 band: &CollocationBand,
343 mut rhs: [Vec<f64>; 3],
344) -> Result<[Vec<f64>; 3], String> {
345 let original = rhs.clone();
346 let count = band.count;
347 let bandwidth = band.bandwidth;
348 let width = 2 * bandwidth + 1;
349 let at = |row: usize, column: usize| row * width + column + bandwidth - row;
350 let fallback = || Ok([
351 solve_collocation_banded(band, &original[0])?,
352 solve_collocation_banded(band, &original[1])?,
353 solve_collocation_banded(band, &original[2])?,
354 ]);
355 if count == 0 || rhs.iter().any(|axis| axis.len() != count) {
356 return fallback();
357 }
358 let mut matrix = band.data.clone();
359 for column in 0..count {
360 let pivot = matrix[at(column, column)];
361 if pivot.abs() <= 1e-300 { return fallback(); }
362 let maximum_row = (column + bandwidth).min(count - 1);
363 for row in column + 1..=maximum_row {
364 let factor = matrix[at(row, column)] / pivot;
365 if factor == 0.0 { continue; }
366 let maximum_column = (column + bandwidth).min(count - 1);
367 for entry in column..=maximum_column {
368 matrix[at(row, entry)] -= factor * matrix[at(column, entry)];
369 }
370 for axis in &mut rhs {
371 axis[row] -= factor * axis[column];
372 }
373 }
374 }
375 let mut solutions = std::array::from_fn(|_| vec![0.0; count]);
376 for axis in 0..3 {
377 for row in (0..count).rev() {
378 let mut value = rhs[axis][row];
379 let maximum_column = (row + bandwidth).min(count - 1);
380 for column in row + 1..=maximum_column {
381 value -= matrix[at(row, column)] * solutions[axis][column];
382 }
383 if matrix[at(row, row)].abs() <= 1e-300 { return fallback(); }
384 solutions[axis][row] = value / matrix[at(row, row)];
385 }
386 let scale = original[axis].iter().fold(0.0_f64, |m, v| m.max(v.abs()));
387 let tolerance = 1e-9 * scale.max(1.0);
388 let mut residual = 0.0_f64;
389 for row in 0..count {
390 let lo = row.saturating_sub(bandwidth);
391 let hi = (row + bandwidth).min(count - 1);
392 let mut accumulated = 0.0;
393 for col in lo..=hi {
394 accumulated += band.get(row, col) * solutions[axis][col];
395 }
396 residual = residual.max((accumulated - original[axis][row]).abs());
397 }
398 if residual > tolerance {
399 solutions[axis] = solve_dense(band.to_dense(), original[axis].clone())?;
400 }
401 }
402 Ok(solutions)
403}
404
405fn assert_interpolant_bits(before: &NurbsCurve, after: &NurbsCurve) {
406 let bits = |c: &NurbsCurve| (c.degree,
407 c.knots.iter().map(|v| v.to_bits()).collect::<Vec<_>>(),
408 c.control_points.iter().map(|p| (p.x.to_bits(), p.y.to_bits(), p.z.to_bits(), p.w.to_bits())).collect::<Vec<_>>());
409 assert_eq!(bits(before), bits(after), "shared interpolant elimination moved a coefficient");
410}
411
412pub fn solve_banded(
413 matrix: &[Vec<f64>],
414 rhs: &[f64],
415 bandwidth: usize,
416) -> Result<Vec<f64>, String> {
417 let count = rhs.len();
418 if count == 0 || matrix.len() != count || matrix.iter().any(|row| row.len() != count) {
419 return Err("solve_banded: matrix must be square and match RHS".into());
420 }
421 let mut matrix = matrix.to_vec();
422 let mut rhs = rhs.to_vec();
423 for column in 0..count {
424 let pivot = matrix[column][column];
425 if pivot.abs() <= 1e-300 {
426 return Err("solve_banded: singular matrix".into());
427 }
428 let maximum_row = (column + bandwidth).min(count - 1);
429 for row in column + 1..=maximum_row {
430 let factor = matrix[row][column] / pivot;
431 if factor == 0.0 {
432 continue;
433 }
434 let maximum_column = (column + bandwidth).min(count - 1);
435 for entry in column..=maximum_column {
436 matrix[row][entry] -= factor * matrix[column][entry];
437 }
438 rhs[row] -= factor * rhs[column];
439 }
440 }
441 let mut result = vec![0.0; count];
442 for row in (0..count).rev() {
443 let mut value = rhs[row];
444 let maximum_column = (row + bandwidth).min(count - 1);
445 for column in row + 1..=maximum_column {
446 value -= matrix[row][column] * result[column];
447 }
448 if matrix[row][row].abs() <= 1e-300 {
449 return Err("solve_banded: singular matrix".into());
450 }
451 result[row] = value / matrix[row][row];
452 }
453 Ok(result)
454}
455
456pub fn interpolate_homogeneous(
463 points: &[Vec4],
464 degree: usize,
465 parameters: &[f64],
466) -> Result<NurbsCurve, String> {
467 if points.len() < 2 || parameters.len() != points.len() {
468 return Err(
469 "interpolate_homogeneous: points and parameters must have matching length >= 2".into(),
470 );
471 }
472 if parameters.windows(2).any(|pair| pair[1] <= pair[0]) {
473 return Err("interpolate_homogeneous: parameters must be strictly increasing".into());
474 }
475 let n = points.len() - 1;
476 let degree = degree.min(n);
477 let m = n + degree + 1;
478 let mut knots = vec![0.0; m + 1];
479 for knot in &mut knots[m - degree..=m] {
480 *knot = 1.0;
481 }
482 for j in 1..=n.saturating_sub(degree) {
483 knots[j + degree] = parameters[j..j + degree].iter().sum::<f64>() / degree as f64;
484 }
485 let knot_vector = KnotVector::new(knots.clone(), degree)?;
486 let mut matrix = vec![vec![0.0; n + 1]; n + 1];
487 for (row, ¶meter) in parameters.iter().enumerate() {
488 let span = knot_vector.find_span(parameter);
489 let basis = knot_vector.basis_functions(span, parameter);
490 for (offset, value) in basis.into_iter().enumerate() {
491 matrix[row][span - degree + offset] = value;
492 }
493 }
494 let solve_axis = |axis: fn(&Vec4) -> f64| {
495 solve_collocation(
496 &matrix,
497 &points.iter().map(axis).collect::<Vec<_>>(),
498 degree,
499 )
500 };
501 let xs = solve_axis(|point| point.x)?;
502 let ys = solve_axis(|point| point.y)?;
503 let zs = solve_axis(|point| point.z)?;
504 let ws = solve_axis(|point| point.w)?;
505 NurbsCurve::new(
506 degree,
507 knots,
508 (0..=n)
509 .map(|index| Vec4 {
510 x: xs[index],
511 y: ys[index],
512 z: zs[index],
513 w: ws[index],
514 })
515 .collect(),
516 )
517}
518
519pub fn interpolate_curve(
535 points: &[Vec3],
536 degree: usize,
537 parameters: &[f64],
538) -> Result<NurbsCurve, String> {
539 let option = std::env::var("BREP_INTERPOLANT_AXIS_REUSE").unwrap_or_default();
540 let enabled = matches!(option.as_str(), "on" | "1" | "check");
541 if option == "check" {
542 let before = interpolate_curve_with_axis_reuse(points, degree, parameters, false);
543 let after = interpolate_curve_with_axis_reuse(points, degree, parameters, true);
544 match (&before, &after) {
545 (Ok(a), Ok(b)) => assert_interpolant_bits(a, b),
546 (Err(a), Err(b)) => assert_eq!(a, b),
547 _ => panic!("shared interpolant elimination changed the result"),
548 }
549 eprintln!("BREP_INTERPOLANT_AXIS_CENSUS stations={} matched=true", points.len());
550 return before;
551 }
552 interpolate_curve_with_axis_reuse(points, degree, parameters, enabled)
553}
554
555fn interpolate_curve_with_axis_reuse(
556 points: &[Vec3],
557 degree: usize,
558 parameters: &[f64],
559 reuse_axes: bool,
560) -> Result<NurbsCurve, String> {
561 if points.len() < 2 || parameters.len() != points.len() {
562 return Err(
563 "interpolate_curve: points and parameters must have matching length >= 2".into(),
564 );
565 }
566 if parameters.windows(2).any(|pair| pair[1] <= pair[0]) {
567 return Err("interpolate_curve: parameters must be strictly increasing".into());
568 }
569 let n = points.len() - 1;
570 let degree = degree.min(n);
571 let m = n + degree + 1;
572 let mut knots = vec![parameters[0]; m + 1];
573 for knot in &mut knots[m - degree..=m] {
574 *knot = parameters[n];
575 }
576 for j in 1..=n.saturating_sub(degree) {
577 knots[j + degree] = parameters[j..j + degree].iter().sum::<f64>() / degree as f64;
578 }
579 let knot_vector = KnotVector::new(knots.clone(), degree)?;
580 let mut band = CollocationBand::new(n + 1, degree);
583 let mut dense: Option<Vec<Vec<f64>>> = None;
584 for (row, ¶meter) in parameters.iter().enumerate() {
585 let span = knot_vector.find_span(parameter);
586 let basis = knot_vector.basis_functions(span, parameter);
587 for (offset, value) in basis.into_iter().enumerate() {
588 let column = span - degree + offset;
589 if !band.set(row, column, value) {
590 dense.get_or_insert_with(|| band.to_dense())[row][column] = value;
591 } else if let Some(dense) = dense.as_mut() {
592 dense[row][column] = value;
593 }
594 }
595 }
596 let solve_axis = |axis: fn(Vec3) -> f64| {
597 let rhs = points.iter().copied().map(axis).collect::<Vec<_>>();
598 match &dense {
599 Some(matrix) => solve_collocation(matrix, &rhs, degree),
600 None => solve_collocation_banded(&band, &rhs),
601 }
602 };
603 let [xs, ys, zs] = if reuse_axes && dense.is_none() {
604 solve_collocation_banded_axes(&band, [
605 points.iter().map(|p| p.x).collect(),
606 points.iter().map(|p| p.y).collect(),
607 points.iter().map(|p| p.z).collect(),
608 ])?
609 } else {
610 [solve_axis(|point| point.x)?, solve_axis(|point| point.y)?, solve_axis(|point| point.z)?]
611 };
612 NurbsCurve::new(
613 degree,
614 knots,
615 (0..=n)
616 .map(|index| Vec4::from_point(Vec3::new(xs[index], ys[index], zs[index]), 1.0))
617 .collect(),
618 )
619}
620
621pub fn interpolate_curve_with_end_tangents(
628 points: &[Vec3],
629 parameters: &[f64],
630 start_tangent: Vec3,
631 end_tangent: Vec3,
632) -> Result<NurbsCurve, String> {
633 if points.len() < 2 || parameters.len() != points.len() {
634 return Err(
635 "interpolate_curve_with_end_tangents: points and parameters must match, >= 2".into(),
636 );
637 }
638 if parameters.windows(2).any(|pair| pair[1] <= pair[0]) {
639 return Err("interpolate_curve_with_end_tangents: parameters must increase".into());
640 }
641 let degree = 3usize;
642 let n = points.len() - 1;
643 let control_count = n + 3;
644 let t0 = parameters[0];
645 let t1 = parameters[n];
646 const PT_KNOTS_DEFAULT: bool = true;
647 static PT_KNOTS: std::sync::OnceLock<bool> = std::sync::OnceLock::new();
648 let pt_knots = *PT_KNOTS.get_or_init(|| {
649 match std::env::var("BREP_FIT_PT_KNOTS").as_deref() {
650 Ok("0") => false,
651 Ok("1") => true,
652 _ => PT_KNOTS_DEFAULT,
653 }
654 });
655 let mut knots = vec![t0; degree + 1];
661 for j in 0..n.saturating_sub(1) {
662 let window = if pt_knots {
663 ¶meters[j..j + degree]
664 } else {
665 ¶meters[j + 1..(j + degree).min(n) + 1]
666 };
667 knots.push(window.iter().sum::<f64>() / window.len() as f64);
668 }
669 knots.extend(std::iter::repeat(t1).take(degree + 1));
670 if knots.len() != control_count + degree + 1 {
671 return Err(format!(
672 "interpolate_curve_with_end_tangents: internal knot count {} for {} controls",
673 knots.len(),
674 control_count
675 ));
676 }
677 let knot_vector = KnotVector::new(knots.clone(), degree)?;
678 let mut matrix = vec![vec![0.0; control_count]; control_count];
679 let mut rhs_points = vec![Vec3::default(); control_count];
680 matrix[0][0] = 1.0;
684 rhs_points[0] = points[0];
685 let start_span = knots[degree + 1] - t0;
686 matrix[1][0] = -(degree as f64) / start_span;
687 matrix[1][1] = (degree as f64) / start_span;
688 rhs_points[1] = start_tangent;
689 for (index, ¶meter) in parameters.iter().enumerate().take(n).skip(1) {
690 let row = index + 1;
691 let span = knot_vector.find_span(parameter);
692 let basis = knot_vector.basis_functions(span, parameter);
693 for (offset, value) in basis.into_iter().enumerate() {
694 matrix[row][span - degree + offset] = value;
695 }
696 rhs_points[row] = points[index];
697 }
698 let end_span = t1 - knots[control_count - 1];
699 matrix[control_count - 2][control_count - 2] = -(degree as f64) / end_span;
700 matrix[control_count - 2][control_count - 1] = (degree as f64) / end_span;
701 rhs_points[control_count - 2] = end_tangent;
702 matrix[control_count - 1][control_count - 1] = 1.0;
703 rhs_points[control_count - 1] = points[n];
704 let solve_axis = |axis: fn(Vec3) -> f64| {
705 solve_dense(
706 matrix.clone(),
707 rhs_points.iter().copied().map(axis).collect::<Vec<_>>(),
708 )
709 };
710 let xs = solve_axis(|point| point.x)?;
711 let ys = solve_axis(|point| point.y)?;
712 let zs = solve_axis(|point| point.z)?;
713 NurbsCurve::new(
714 degree,
715 knots,
716 (0..control_count)
717 .map(|index| Vec4::from_point(Vec3::new(xs[index], ys[index], zs[index]), 1.0))
718 .collect(),
719 )
720}
721
722pub fn interpolate_curve_thinned(
723 points: &[Vec3],
724 degree: usize,
725 maximum_points: usize,
726) -> Result<NurbsCurve, String> {
727 if maximum_points < 2 {
728 return Err("interpolate_curve_thinned: maximum point count must be at least 2".into());
729 }
730 if points.len() <= maximum_points {
731 let parameters = chord_parameters(points);
732 return interpolate_curve(points, degree, ¶meters);
733 }
734 let step = (points.len() - 1) as f64 / (maximum_points - 1) as f64;
735 let thinned = (0..maximum_points)
736 .map(|index| points[(index as f64 * step).round() as usize])
737 .collect::<Vec<_>>();
738 let parameters = chord_parameters(&thinned);
739 interpolate_curve(&thinned, degree, ¶meters)
740}
741
742fn chord_parameters(points: &[Vec3]) -> Vec<f64> {
743 if points.len() < 2 {
744 return vec![0.0; points.len()];
745 }
746 let mut parameters = vec![0.0; points.len()];
747 for index in 1..points.len() {
748 parameters[index] = parameters[index - 1] + points[index].sub(points[index - 1]).length();
749 }
750 let length = parameters[points.len() - 1];
751 if length <= 1e-15 {
752 for (index, parameter) in parameters.iter_mut().enumerate() {
753 *parameter = index as f64 / (points.len() - 1) as f64;
754 }
755 } else {
756 for parameter in &mut parameters {
757 *parameter /= length;
758 }
759 }
760 parameters
761}
762
763pub fn simplify_polyline_indices(points: &[Vec3], tolerance: f64) -> Vec<usize> {
772 if points.len() <= 2 {
773 return (0..points.len()).collect();
774 }
775 let mut keep = vec![false; points.len()];
776 keep[0] = true;
777 keep[points.len() - 1] = true;
778 let mut stack = vec![(0usize, points.len() - 1)];
779 while let Some((start, end)) = stack.pop() {
780 if end - start < 2 {
781 continue;
782 }
783 let a = points[start];
784 let direction = points[end].sub(a);
785 let length_squared = direction.length_squared().max(1e-300);
786 let mut worst = None;
787 let mut worst_distance = tolerance;
788 for (index, point) in points.iter().enumerate().take(end).skip(start + 1) {
789 let fraction = point.sub(a).dot(direction) / length_squared;
790 let fraction = fraction.clamp(0.0, 1.0);
791 let distance = point.sub(a.add(direction.scale(fraction))).length();
792 if distance > worst_distance {
793 worst_distance = distance;
794 worst = Some(index);
795 }
796 }
797 if let Some(index) = worst {
798 keep[index] = true;
799 stack.push((start, index));
800 stack.push((index, end));
801 }
802 }
803 keep.into_iter()
804 .enumerate()
805 .filter_map(|(index, keep)| keep.then_some(index))
806 .collect()
807}
808
809pub fn simplify_polyline(points: &[Vec3], tolerance: f64) -> Vec<Vec3> {
810 simplify_polyline_indices(points, tolerance)
811 .into_iter()
812 .map(|index| points[index])
813 .collect()
814}
815
816pub fn interpolate_curve_local(
817 points: &[Vec3],
818 parameters: &[f64],
819 tension: f64,
820) -> Result<NurbsCurve, String> {
821 if points.len() != parameters.len() || points.len() < 2 {
822 return Err(
823 "interpolate_curve_local: points and parameters must have matching length >= 2".into(),
824 );
825 }
826 if points.len() == 2 {
827 return interpolate_curve(points, 1, parameters);
828 }
829 let count = points.len();
830 let mut tangents = vec![Vec3::default(); count];
831 tangents[0] = points[1]
832 .sub(points[0])
833 .scale(1.0 / (parameters[1] - parameters[0]));
834 tangents[count - 1] = points[count - 1]
835 .sub(points[count - 2])
836 .scale(1.0 / (parameters[count - 1] - parameters[count - 2]));
837 for index in 1..count - 1 {
838 let previous_interval = parameters[index] - parameters[index - 1];
839 let next_interval = parameters[index + 1] - parameters[index];
840 let total = previous_interval + next_interval;
841 let previous_secant = points[index]
842 .sub(points[index - 1])
843 .scale(1.0 / previous_interval);
844 let next_secant = points[index + 1]
845 .sub(points[index])
846 .scale(1.0 / next_interval);
847 let mut tangent =
848 points[index - 1]
849 .scale(-next_interval / (previous_interval * total))
850 .add(points[index].scale(
851 (next_interval - previous_interval) / (previous_interval * next_interval),
852 ))
853 .add(points[index + 1].scale(previous_interval / (next_interval * total)));
854 if previous_secant.dot(next_secant) <= 0.0
855 || tangent.dot(previous_secant) <= 0.0
856 || tangent.dot(next_secant) <= 0.0
857 {
858 tangent = Vec3::default();
859 } else {
860 let maximum = 3.0 * previous_secant.length().min(next_secant.length());
861 if tangent.length() > maximum {
862 tangent = tangent.normalized()?.scale(maximum);
863 }
864 }
865 tangents[index] = tangent;
866 }
867 if tension != 1.0 {
868 for tangent in &mut tangents {
869 *tangent = tangent.scale(tension);
870 }
871 }
872 let mut control_points = vec![Vec4::from_point(points[0], 1.0)];
873 for index in 0..count - 1 {
874 let interval = parameters[index + 1] - parameters[index];
875 control_points.extend([
876 Vec4::from_point(
877 points[index].add(tangents[index].scale(interval / 3.0)),
878 1.0,
879 ),
880 Vec4::from_point(
881 points[index + 1].sub(tangents[index + 1].scale(interval / 3.0)),
882 1.0,
883 ),
884 Vec4::from_point(points[index + 1], 1.0),
885 ]);
886 }
887 let mut knots = vec![parameters[0]; 4];
888 for parameter in ¶meters[1..count - 1] {
889 knots.extend([*parameter; 3]);
890 }
891 knots.extend([parameters[count - 1]; 4]);
892 NurbsCurve::new(3, knots, control_points)
893}
894
895const LADDER_BUDGET_FACTOR: usize = 2;
911
912pub const MAX_FIT_STATIONS: usize = 1024;
934
935const MEASURE_NEWTON_STEPS: usize = 3;
941
942const MEASURE_SPAN_FRACTIONS: [f64; 3] = [0.25, 0.5, 0.75];
948
949const STALL_FACTOR: f64 = 0.9;
954
955#[derive(Clone, Copy, Debug, PartialEq)]
973pub struct PolylineFitReport {
974 pub deviation: f64,
978 pub rms: f64,
980 pub between_stations: f64,
984 pub tolerance: f64,
986 pub floor: f64,
998 pub stations: usize,
1000 pub controls: usize,
1002 pub input_points: usize,
1004 pub pool_points: usize,
1007 pub dropped_band: f64,
1014 pub rungs: usize,
1017 pub local_lane: bool,
1022 pub lane_switched: bool,
1025 pub exit: PolylineFitExit,
1027}
1028
1029impl PolylineFitReport {
1030 pub fn met_tolerance(&self) -> bool {
1034 self.deviation <= self.floor
1035 }
1036
1037 pub fn met_requested_tolerance(&self) -> bool {
1039 self.deviation <= self.tolerance
1040 }
1041
1042 pub fn saturated(&self) -> bool {
1053 self.stations >= self.pool_points
1054 }
1055}
1056
1057#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
1062pub enum PolylineFitExit {
1063 Converged,
1065 StationCeiling,
1068 InputExhausted,
1072 Stalled,
1076 Unrefined,
1079}
1080
1081#[derive(Clone, Debug)]
1082pub struct PolylineFit {
1083 pub curve: NurbsCurve,
1084 pub parameters: Vec<f64>,
1085 pub kept: Vec<Vec3>,
1086 pub report: PolylineFitReport,
1088}
1089
1090pub(crate) fn polyline_station_pool(points: &[Vec3], tolerance: f64) -> Vec<usize> {
1102 station_pool(points, tolerance)
1103}
1104
1105fn station_pool(points: &[Vec3], tolerance: f64) -> Vec<usize> {
1106 let simplified = simplify_polyline_indices(points, tolerance);
1107 if simplified.len() > 2 {
1108 let total: f64 = simplified
1109 .windows(2)
1110 .map(|pair| points[pair[1]].sub(points[pair[0]]).length())
1111 .sum();
1112 let floor = (tolerance * 0.01).max(total * 1e-4);
1113 let first = points[simplified[0]];
1114 let last = points[simplified[simplified.len() - 1]];
1115 let mut conditioned = vec![simplified[0]];
1116 for index in &simplified[1..simplified.len() - 1] {
1117 let point = points[*index];
1118 if point.sub(first).length() > floor
1119 && point.sub(last).length() > floor
1120 && point.sub(points[*conditioned.last().unwrap()]).length() > floor
1121 {
1122 conditioned.push(*index);
1123 }
1124 }
1125 conditioned.push(simplified[simplified.len() - 1]);
1126 conditioned
1127 } else {
1128 let mut distinct: Vec<usize> = Vec::new();
1129 for index in simplified {
1130 if distinct.last().is_none_or(|previous: &usize| {
1131 points[index].sub(points[*previous]).length() > tolerance * 0.01
1132 }) {
1133 distinct.push(index);
1134 }
1135 }
1136 distinct
1137 }
1138}
1139
1140fn decimate_stations(pool: &[usize], maximum_points: usize) -> Vec<usize> {
1142 if pool.len() <= maximum_points {
1143 return pool.to_vec();
1144 }
1145 let step = (pool.len() - 1) as f64 / (maximum_points - 1) as f64;
1146 (0..maximum_points)
1147 .map(|index| pool[(index as f64 * step).round() as usize])
1148 .collect()
1149}
1150
1151fn station_parameters(points: &[Vec3], stations: &[usize]) -> Option<Vec<f64>> {
1154 let total: f64 = stations
1155 .windows(2)
1156 .map(|pair| points[pair[1]].sub(points[pair[0]]).length())
1157 .sum();
1158 if total <= 0.0 {
1159 return None;
1160 }
1161 let mut parameters = vec![0.0; stations.len()];
1162 let mut accumulated = 0.0;
1163 for index in 1..stations.len() {
1164 accumulated += points[stations[index]]
1165 .sub(points[stations[index - 1]])
1166 .length();
1167 parameters[index] = accumulated / total;
1168 }
1169 *parameters.last_mut().unwrap() = 1.0;
1170 Some(parameters)
1171}
1172
1173fn distance_to_segment(point: Vec3, from: Vec3, to: Vec3) -> f64 {
1174 point.distance_to_segment(from, to, 0.0)
1175}
1176
1177fn distance_to_curve_near(
1186 curve: &NurbsCurve,
1187 point: Vec3,
1188 seed: f64,
1189 lower: f64,
1190 upper: f64,
1191) -> Result<f64, String> {
1192 let mut parameter = seed.clamp(lower, upper);
1193 let mut best = f64::INFINITY;
1194 for _ in 0..MEASURE_NEWTON_STEPS {
1195 let derivatives = curve.derivatives_small(parameter, 2)?;
1196 let residual = derivatives[0].sub(point);
1197 best = best.min(residual.length_squared());
1198 let slope = derivatives[1].dot(residual);
1199 let curvature = derivatives[2].dot(residual) + derivatives[1].length_squared();
1200 if curvature.abs() <= f64::MIN_POSITIVE {
1201 break;
1202 }
1203 let next = (parameter - slope / curvature).clamp(lower, upper);
1204 if next == parameter {
1205 break;
1206 }
1207 parameter = next;
1208 }
1209 let residual = curve.evaluate(parameter)?.sub(point).length_squared();
1210 Ok(best.min(residual).sqrt())
1211}
1212
1213fn measure_polyline_fit(
1235 curve: &NurbsCurve,
1236 points: &[Vec3],
1237 stations: &[usize],
1238 parameters: &[f64],
1239 arc: &[f64],
1240) -> Result<(f64, f64, f64), String> {
1241 let mut worst = 0.0_f64;
1242 let mut sum_squares = 0.0_f64;
1243 let mut samples = 0usize;
1244 let mut between = 0.0_f64;
1245 let last = stations.len() - 1;
1246 for span in 0..last {
1247 let (from, to) = (stations[span], stations[span + 1]);
1248 let (t0, t1) = (parameters[span], parameters[span + 1]);
1249 let lower = parameters[span.saturating_sub(1)];
1252 let upper = parameters[(span + 2).min(last)];
1253 let length = arc[to] - arc[from];
1257 for index in from..to {
1258 let seed = if length > 0.0 {
1259 t0 + (t1 - t0) * (arc[index] - arc[from]) / length
1260 } else {
1261 t0
1262 };
1263 let distance = distance_to_curve_near(curve, points[index], seed, lower, upper)?;
1264 worst = worst.max(distance);
1265 sum_squares += distance * distance;
1266 samples += 1;
1267 }
1268 let window_start = from.saturating_sub(1);
1271 let window_end = (to + 1).min(points.len() - 1);
1272 for fraction in MEASURE_SPAN_FRACTIONS {
1273 let sample = curve.evaluate(t0 + (t1 - t0) * fraction)?;
1274 let mut nearest = f64::INFINITY;
1275 for index in window_start..window_end {
1276 nearest = nearest.min(distance_to_segment(
1277 sample,
1278 points[index],
1279 points[index + 1],
1280 ));
1281 }
1282 if nearest.is_finite() {
1283 between = between.max(nearest);
1284 }
1285 }
1286 }
1287 let distance = distance_to_curve_near(
1288 curve,
1289 points[stations[last]],
1290 parameters[last],
1291 parameters[last.saturating_sub(1)],
1292 parameters[last],
1293 )?;
1294 worst = worst.max(distance);
1295 sum_squares += distance * distance;
1296 samples += 1;
1297 let rms = (sum_squares / samples as f64).sqrt();
1298 Ok((worst, rms, between))
1299}
1300
1301fn dropped_band(points: &[Vec3], pool: &[usize]) -> f64 {
1305 let mut worst = 0.0_f64;
1306 for span in 0..pool.len() - 1 {
1307 let (from, to) = (pool[span], pool[span + 1]);
1308 for index in from + 1..to {
1309 worst = worst.max(distance_to_segment(
1310 points[index],
1311 points[from],
1312 points[to],
1313 ));
1314 }
1315 }
1316 worst
1317}
1318
1319fn cumulative_chord(points: &[Vec3]) -> Vec<f64> {
1321 let mut arc = vec![0.0; points.len()];
1322 for index in 1..points.len() {
1323 arc[index] = arc[index - 1] + points[index].sub(points[index - 1]).length();
1324 }
1325 arc
1326}
1327
1328fn fit_rung(
1330 points: &[Vec3],
1331 pool: &[usize],
1332 arc: &[f64],
1333 maximum_points: usize,
1334 local_interpolation: bool,
1335 degree: usize,
1336) -> Result<(NurbsCurve, Vec<usize>, Vec<f64>, f64, f64, f64), String> {
1337 let stations = decimate_stations(pool, maximum_points);
1338 fit_station_set(points, stations, arc, local_interpolation, degree)
1339}
1340
1341fn fit_station_set(
1342 points: &[Vec3],
1343 stations: Vec<usize>,
1344 arc: &[f64],
1345 local_interpolation: bool,
1346 degree: usize,
1347) -> Result<(NurbsCurve, Vec<usize>, Vec<f64>, f64, f64, f64), String> {
1348 let split_started = crate::profile::profile_enabled().then(web_time::Instant::now);
1349 let parameters = station_parameters(points, &stations)
1350 .ok_or_else(|| "fit_polyline: degenerate polyline".to_string())?;
1351 let kept = stations.iter().map(|index| points[*index]).collect::<Vec<_>>();
1352 let closed = kept.len() > degree + 1
1356 && kept[0].sub(kept[kept.len() - 1]).length() <= 1e-12 * (1.0 + kept[0].length());
1357 let params_done = split_started.map(|_| web_time::Instant::now());
1358 let curve = if local_interpolation {
1359 interpolate_curve_local(&kept, ¶meters, 1.0)?
1360 } else if closed
1361 && degree > 3
1362 && degree % 2 == 1
1363 && std::env::var("BREP_SECTION_FIT_PERIODIC").as_deref() != Ok("0")
1364 {
1365 interpolate_curve_closed_of_degree(&kept[..kept.len() - 1], ¶meters, degree)?
1366 } else {
1367 interpolate_curve(&kept, degree.min(kept.len() - 1), ¶meters)?
1368 };
1369 let interp_done = split_started.map(|_| web_time::Instant::now());
1370 let (deviation, rms, between) =
1371 measure_polyline_fit(&curve, points, &stations, ¶meters, arc)?;
1372 if let (Some(a), Some(b), Some(c)) = (split_started, params_done, interp_done) {
1373 STATION_SET_SPLIT.with(|cell| {
1374 let mut cell = cell.borrow_mut();
1375 cell.0 += 1;
1376 cell.1 += stations.len() as u64;
1377 cell.2 += points.len() as u64;
1378 cell.3 += (b - a).as_secs_f64() * 1e3;
1379 cell.4 += (c - b).as_secs_f64() * 1e3;
1380 cell.5 += c.elapsed().as_secs_f64() * 1e3;
1381 });
1382 }
1383 Ok((curve, stations, parameters, deviation, rms, between))
1384}
1385
1386thread_local! {
1390 static STATION_SET_SPLIT: std::cell::RefCell<(u64, u64, u64, f64, f64, f64)> = const { std::cell::RefCell::new((0, 0, 0, 0.0, 0.0, 0.0)) };
1391}
1392pub(crate) fn station_set_split_take() -> (u64, u64, u64, f64, f64, f64) {
1394 STATION_SET_SPLIT.with(|cell| std::mem::take(&mut *cell.borrow_mut()))
1395}
1396
1397pub fn fit_polyline_at(
1405 points: &[Vec3],
1406 tolerance: f64,
1407 maximum_points: usize,
1408 local_interpolation: bool,
1409) -> Result<PolylineFit, String> {
1410 let fit = fit_polyline_inner(points, tolerance, maximum_points, local_interpolation, false, 3)?;
1411 record_polyline_fit(&fit.report);
1412 Ok(fit)
1413}
1414
1415pub fn fit_polyline(
1464 points: &[Vec3],
1465 tolerance: f64,
1466 maximum_points: usize,
1467 local_interpolation: bool,
1468) -> Result<PolylineFit, String> {
1469 fit_polyline_of_degree(points, tolerance, maximum_points, local_interpolation, 3)
1470}
1471
1472pub fn fit_polyline_of_degree(
1481 points: &[Vec3],
1482 tolerance: f64,
1483 maximum_points: usize,
1484 local_interpolation: bool,
1485 degree: usize,
1486) -> Result<PolylineFit, String> {
1487 fit_polyline_of_degree_with_recovery(points, tolerance, maximum_points, local_interpolation, degree)
1488 .map(|(fit, _)| fit)
1489}
1490
1491pub(crate) fn fit_polyline_of_degree_with_recovery(
1495 points: &[Vec3],
1496 tolerance: f64,
1497 maximum_points: usize,
1498 local_interpolation: bool,
1499 degree: usize,
1500) -> Result<(PolylineFit, bool), String> {
1501 let refine = std::env::var("BREP_POLYLINE_FIT_REFINE").as_deref() != Ok("0");
1502 let asked = fit_polyline_inner(points, tolerance, maximum_points, local_interpolation, refine, degree)?;
1503 if asked.report.met_tolerance() || !refine {
1504 record_polyline_fit(&asked.report);
1505 return Ok((asked, false));
1506 }
1507 let mut other =
1508 fit_polyline_inner(points, tolerance, maximum_points, !local_interpolation, refine, degree)?;
1509 let spent = asked.report.rungs + other.report.rungs;
1510 let mut best = if other.report.deviation < asked.report.deviation {
1511 other.report.lane_switched = true;
1512 other
1513 } else {
1514 asked
1515 };
1516 let mut spent = spent;
1521 let mut recovered = false;
1522 if !best.report.met_tolerance() {
1523 let (adaptive, rounds) = fit_polyline_adaptive(points, tolerance, maximum_points, degree)?;
1524 spent += rounds;
1525 if let Some(mut adaptive) = adaptive {
1526 adaptive.report.lane_switched = local_interpolation;
1527 best = adaptive;
1528 recovered = true;
1529 }
1530 }
1531 best.report.rungs = spent;
1533 record_polyline_fit(&best.report);
1534 Ok((best, recovered))
1535}
1536
1537fn fit_polyline_adaptive(
1541 points: &[Vec3],
1542 tolerance: f64,
1543 maximum_points: usize,
1544 degree: usize,
1545) -> Result<(Option<PolylineFit>, usize), String> {
1546 let pool = station_pool(points, tolerance);
1547 let arc = cumulative_chord(points);
1548 let band = dropped_band(points, &pool);
1549 let floor = tolerance.max(band);
1550 let budget = maximum_points.max(2);
1551 let ceiling = pool.len().min(MAX_FIT_STATIONS)
1552 .min(budget.saturating_mul(LADDER_BUDGET_FACTOR)).max(budget);
1553 let mut stations = decimate_stations(&pool, budget);
1554 for round in 0..=8 {
1557 let Ok((curve, current, parameters, deviation, rms, between)) =
1558 fit_station_set(points, stations, &arc, false, degree)
1559 else { return Ok((None, round + 1)); };
1560 stations = current;
1561 if deviation <= floor {
1562 let report = PolylineFitReport {
1563 deviation, rms, between_stations: between, tolerance, floor,
1564 stations: stations.len(), controls: curve.control_points.len(),
1565 input_points: points.len(), pool_points: pool.len(), dropped_band: band,
1566 rungs: round + 1, local_lane: false, lane_switched: false,
1567 exit: PolylineFitExit::Converged,
1568 };
1569 return Ok((Some(PolylineFit {
1570 curve, parameters, kept: stations.iter().map(|&i| points[i]).collect(), report,
1571 }), round + 1));
1572 }
1573 if stations.len() >= ceiling || round == 8 { return Ok((None, round + 1)); }
1574 let mut candidates = Vec::new();
1575 for span in 0..stations.len() - 1 {
1576 let (from, to) = (stations[span], stations[span + 1]);
1577 let (t0, t1) = (parameters[span], parameters[span + 1]);
1578 let lower = parameters[span.saturating_sub(1)];
1579 let upper = parameters[(span + 2).min(stations.len() - 1)];
1580 let mut candidate: Option<(usize, f64)> = None;
1581 for &index in pool.iter().filter(|&&i| i > from && i < to) {
1582 let length = arc[to] - arc[from];
1583 let seed = if length > 0.0 {
1584 t0 + (t1 - t0) * (arc[index] - arc[from]) / length
1585 } else { t0 };
1586 let error = distance_to_curve_near(&curve, points[index], seed, lower, upper)?;
1587 if candidate.is_none_or(|(_, previous)| error > previous) {
1588 candidate = Some((index, error));
1589 }
1590 }
1591 if let Some((index, error)) = candidate {
1592 if error > floor { candidates.push((index, error)); }
1593 }
1594 }
1595 candidates.sort_by(|a, b| b.1.total_cmp(&a.1).then_with(|| a.0.cmp(&b.0)));
1596 let added = candidates.len().min(16).min(ceiling - stations.len());
1597 if added == 0 { return Ok((None, round + 1)); }
1598 stations.extend(candidates.iter().take(added).map(|&(index, _)| index));
1599 stations.sort_unstable();
1600 }
1601 unreachable!("the bounded refinement returns on its last round")
1602}
1603
1604#[derive(Clone, Copy, Debug, PartialEq, Eq)]
1635pub(crate) enum SeededPath {
1636 Accepted { rounds: usize },
1638 Fallback(SeededFallback),
1640}
1641
1642#[derive(Clone, Copy, Debug, PartialEq, Eq)]
1644pub(crate) enum SeededFallback {
1645 NonFiniteInput,
1646 Degenerate,
1647 RefineDisabled,
1648 PoolTooSmall,
1649 NonFiniteBandOrArc,
1650 NonFiniteFloor,
1651 SeedsOverCeiling,
1652 SeedsOutsideStationPool,
1656 StationSetFailed,
1657 NonFiniteFitReading,
1658 SpanUnreadable,
1659 CeilingReached,
1660 RoundsSpent,
1661 NothingToAdd,
1662 EndpointUnreadable,
1665 TightStationOverTolerance,
1669}
1670
1671#[derive(Clone, Copy, Debug, Default, PartialEq)]
1674pub(crate) struct RepresentativeCost {
1675 pub(crate) representatives: usize,
1679 pub(crate) witnesses: usize,
1681 pub(crate) candidate_fits: usize,
1683 pub(crate) full_refit: bool,
1685 pub(crate) start_offset: f64,
1694 pub(crate) end_offset: f64,
1695}
1696
1697pub(crate) fn polyline_station_pool_floor(points: &[Vec3], tolerance: f64) -> f64 {
1704 let simplified = simplify_polyline_indices(points, tolerance);
1705 if simplified.len() > 2 {
1706 let total: f64 = simplified.windows(2).map(|pair| points[pair[1]].sub(points[pair[0]]).length()).sum();
1707 (tolerance * 0.01).max(total * 1e-4)
1708 } else {
1709 tolerance * 0.01
1710 }
1711}
1712
1713pub(crate) fn fit_polyline_seeded_with_path(
1714 points: &[Vec3],
1715 seeds: &[usize],
1716 tolerance: f64,
1717 full_budget: usize,
1718 local_interpolation: bool,
1719 degree: usize,
1720) -> Result<(PolylineFit, SeededPath), String> {
1721 let full_refit = |reason: SeededFallback| {
1722 fit_polyline_of_degree(points, tolerance, full_budget, local_interpolation, degree)
1723 .map(|fit| (fit, SeededPath::Fallback(reason)))
1724 };
1725 if !tolerance.is_finite() || points.iter().any(|p| !p.x.is_finite() || !p.y.is_finite() || !p.z.is_finite()) {
1727 return full_refit(SeededFallback::NonFiniteInput);
1728 }
1729 if points.len() < 3 {
1730 return full_refit(SeededFallback::Degenerate);
1731 }
1732 if std::env::var("BREP_POLYLINE_FIT_REFINE").as_deref() == Ok("0") {
1733 return full_refit(SeededFallback::RefineDisabled);
1734 }
1735 let pool = station_pool(points, tolerance);
1736 if pool.len() < 2 {
1737 return full_refit(SeededFallback::PoolTooSmall);
1738 }
1739 let arc = cumulative_chord(points);
1740 let band = dropped_band(points, &pool);
1741 if !band.is_finite() || arc.iter().any(|value| !value.is_finite()) {
1742 return full_refit(SeededFallback::NonFiniteBandOrArc);
1743 }
1744 let floor = tolerance.max(band);
1745 if !floor.is_finite() {
1746 return full_refit(SeededFallback::NonFiniteFloor);
1747 }
1748 let budget = full_budget.max(2);
1751 let ceiling = pool
1752 .len()
1753 .min(MAX_FIT_STATIONS)
1754 .min(budget.saturating_mul(LADDER_BUDGET_FACTOR))
1755 .max(budget);
1756 let mut stations = seeds
1757 .iter()
1758 .copied()
1759 .filter(|&index| index < points.len())
1760 .chain([0, points.len() - 1])
1761 .collect::<Vec<_>>();
1762 stations.sort_unstable();
1763 stations.dedup();
1764 if stations.len() > ceiling {
1765 return full_refit(SeededFallback::SeedsOverCeiling);
1766 }
1767 if stations.iter().any(|index| pool.binary_search(index).is_err()) {
1785 return full_refit(SeededFallback::SeedsOutsideStationPool);
1786 }
1787 for round in 0..=8 {
1788 let Ok((curve, current, parameters, deviation, rms, between)) =
1789 fit_station_set(points, stations, &arc, local_interpolation, degree)
1790 else {
1791 return full_refit(SeededFallback::StationSetFailed);
1792 };
1793 stations = current;
1794 if !deviation.is_finite() || !rms.is_finite() || !between.is_finite() {
1795 return full_refit(SeededFallback::NonFiniteFitReading);
1796 }
1797 if deviation <= floor {
1798 let report = PolylineFitReport {
1799 deviation,
1800 rms,
1801 between_stations: between,
1802 tolerance,
1803 floor,
1804 stations: stations.len(),
1805 controls: curve.control_points.len(),
1806 input_points: points.len(),
1807 pool_points: pool.len(),
1808 dropped_band: band,
1809 rungs: round + 1,
1810 local_lane: local_interpolation,
1811 lane_switched: false,
1812 exit: PolylineFitExit::Converged,
1813 };
1814 record_polyline_fit(&report);
1815 let kept = stations.iter().map(|&index| points[index]).collect();
1816 return Ok((PolylineFit { curve, parameters, kept, report }, SeededPath::Accepted { rounds: round + 1 }));
1817 }
1818 if stations.len() >= ceiling {
1819 return full_refit(SeededFallback::CeilingReached);
1820 }
1821 if round == 8 {
1822 return full_refit(SeededFallback::RoundsSpent);
1823 }
1824 let mut candidates = Vec::new();
1825 for span in 0..stations.len() - 1 {
1826 let (from, to) = (stations[span], stations[span + 1]);
1827 let (t0, t1) = (parameters[span], parameters[span + 1]);
1828 let lower = parameters[span.saturating_sub(1)];
1829 let upper = parameters[(span + 2).min(stations.len() - 1)];
1830 let mut candidate: Option<(usize, f64)> = None;
1831 for &index in pool.iter().filter(|&&i| i > from && i < to) {
1832 let length = arc[to] - arc[from];
1833 let seed = if length > 0.0 { t0 + (t1 - t0) * (arc[index] - arc[from]) / length } else { t0 };
1834 let Ok(error) = distance_to_curve_near(&curve, points[index], seed, lower, upper) else {
1837 return full_refit(SeededFallback::SpanUnreadable);
1838 };
1839 if !error.is_finite() {
1840 return full_refit(SeededFallback::SpanUnreadable);
1841 }
1842 if candidate.is_none_or(|(_, previous)| error > previous) {
1843 candidate = Some((index, error));
1844 }
1845 }
1846 if let Some((index, error)) = candidate {
1847 if error > floor {
1848 candidates.push((index, error));
1849 }
1850 }
1851 }
1852 candidates.sort_by(|a, b| b.1.total_cmp(&a.1).then_with(|| a.0.cmp(&b.0)));
1853 let added = candidates.len().min(16).min(ceiling - stations.len());
1854 if added == 0 {
1855 return full_refit(SeededFallback::NothingToAdd);
1856 }
1857 stations.extend(candidates.iter().take(added).map(|&(index, _)| index));
1858 stations.sort_unstable();
1859 }
1860 full_refit(SeededFallback::RoundsSpent)
1862}
1863
1864pub(crate) fn fit_polyline_representatives_with_path(
1884 points: &[Vec3],
1885 seeds: &[usize],
1886 tolerance: f64,
1887 full_budget: usize,
1888 local_interpolation: bool,
1889 degree: usize,
1890) -> Result<(PolylineFit, SeededPath, RepresentativeCost), String> {
1891 fit_polyline_representatives_with_witnesses(points, seeds, &[], tolerance, tolerance, full_budget, local_interpolation, degree)
1892}
1893
1894#[allow(clippy::too_many_arguments)]
1908pub(crate) fn fit_polyline_representatives_with_witnesses(
1909 points: &[Vec3],
1910 seeds: &[usize],
1911 tight: &[usize],
1912 tight_tolerance: f64,
1913 tolerance: f64,
1914 full_budget: usize,
1915 local_interpolation: bool,
1916 degree: usize,
1917) -> Result<(PolylineFit, SeededPath, RepresentativeCost), String> {
1918 let mut cost = RepresentativeCost { witnesses: points.len(), start_offset: f64::NAN, end_offset: f64::NAN, ..RepresentativeCost::default() };
1919 let full_refit = |reason: SeededFallback, mut cost: RepresentativeCost| {
1920 cost.full_refit = true;
1921 fit_polyline_of_degree(points, tolerance, full_budget, local_interpolation, degree)
1922 .map(|fit| (fit, SeededPath::Fallback(reason), cost))
1923 };
1924 if !tolerance.is_finite()
1925 || !tight_tolerance.is_finite()
1926 || points.iter().any(|p| !p.x.is_finite() || !p.y.is_finite() || !p.z.is_finite())
1927 {
1928 return full_refit(SeededFallback::NonFiniteInput, cost);
1929 }
1930 if points.len() < 3 {
1931 return full_refit(SeededFallback::Degenerate, cost);
1932 }
1933 if std::env::var("BREP_POLYLINE_FIT_REFINE").as_deref() == Ok("0") {
1934 return full_refit(SeededFallback::RefineDisabled, cost);
1935 }
1936 let mut tight: Vec<usize> = tight.iter().copied().filter(|&index| index < points.len()).collect();
1937 tight.sort_unstable();
1938 tight.dedup();
1939 let pool = station_pool(points, tolerance);
1940 if pool.len() < 2 || pool[0] != 0 || pool[pool.len() - 1] != points.len() - 1 {
1941 return full_refit(SeededFallback::PoolTooSmall, cost);
1942 }
1943 let arc = cumulative_chord(points);
1944 let band = dropped_band(points, &pool);
1945 if !band.is_finite() || arc.iter().any(|value| !value.is_finite()) {
1946 return full_refit(SeededFallback::NonFiniteBandOrArc, cost);
1947 }
1948 let budget = full_budget.max(2);
1949 let ceiling = pool
1950 .len()
1951 .min(MAX_FIT_STATIONS)
1952 .min(budget.saturating_mul(LADDER_BUDGET_FACTOR))
1953 .max(budget);
1954 let mut stations = seeds
1955 .iter()
1956 .copied()
1957 .filter(|index| pool.binary_search(index).is_ok())
1958 .chain([0, points.len() - 1])
1959 .collect::<Vec<_>>();
1960 stations.sort_unstable();
1961 stations.dedup();
1962 cost.representatives = stations.len();
1963 if stations.len() > ceiling {
1964 return full_refit(SeededFallback::SeedsOverCeiling, cost);
1965 }
1966 for round in 0..=8 {
1967 cost.candidate_fits += 1;
1968 let Ok((curve, current, parameters, deviation, rms, between)) =
1969 fit_station_set(points, stations, &arc, local_interpolation, degree)
1970 else {
1971 return full_refit(SeededFallback::StationSetFailed, cost);
1972 };
1973 stations = current;
1974 if !deviation.is_finite() || !rms.is_finite() || !between.is_finite() {
1975 return full_refit(SeededFallback::NonFiniteFitReading, cost);
1976 }
1977 let mut tight_over: Vec<usize> = Vec::new();
1980 for &index in &tight {
1981 let span = match stations.binary_search(&index) {
1982 Ok(position) => {
1983 let distance = curve.evaluate(parameters[position]).map(|p| p.sub(points[index]).length());
1986 match distance {
1987 Ok(distance) if distance.is_finite() => {
1988 if distance > tight_tolerance {
1989 return full_refit(SeededFallback::TightStationOverTolerance, cost);
1990 }
1991 }
1992 _ => return full_refit(SeededFallback::SpanUnreadable, cost),
1993 }
1994 continue;
1995 }
1996 Err(at) if at == 0 || at >= stations.len() => return full_refit(SeededFallback::SpanUnreadable, cost),
1999 Err(at) => at - 1,
2000 };
2001 let (from, to) = (stations[span], stations[span + 1]);
2002 let (t0, t1) = (parameters[span], parameters[span + 1]);
2003 let length = arc[to] - arc[from];
2004 let seed = if length > 0.0 { t0 + (t1 - t0) * (arc[index] - arc[from]) / length } else { t0 };
2005 let lower = parameters[span.saturating_sub(1)];
2006 let upper = parameters[(span + 2).min(stations.len() - 1)];
2007 let Ok(error) = distance_to_curve_near(&curve, points[index], seed, lower, upper) else {
2008 return full_refit(SeededFallback::SpanUnreadable, cost);
2009 };
2010 if !error.is_finite() {
2011 return full_refit(SeededFallback::SpanUnreadable, cost);
2012 }
2013 if error > tight_tolerance {
2014 tight_over.push(index);
2015 }
2016 }
2017 if deviation <= tolerance && tight_over.is_empty() {
2018 let ends = curve
2023 .domain()
2024 .and_then(|[t0, t1]| Ok((curve.evaluate(t0)?, curve.evaluate(t1)?)));
2025 let Ok((start, end)) = ends else {
2026 return full_refit(SeededFallback::EndpointUnreadable, cost);
2027 };
2028 cost.start_offset = start.sub(points[0]).length();
2029 cost.end_offset = end.sub(points[points.len() - 1]).length();
2030 if !(cost.start_offset.is_finite() && cost.end_offset.is_finite()) {
2031 cost.start_offset = f64::NAN;
2032 cost.end_offset = f64::NAN;
2033 return full_refit(SeededFallback::EndpointUnreadable, cost);
2034 }
2035 let report = PolylineFitReport {
2036 deviation,
2037 rms,
2038 between_stations: between,
2039 tolerance,
2040 floor: tolerance,
2041 stations: stations.len(),
2042 controls: curve.control_points.len(),
2043 input_points: points.len(),
2044 pool_points: pool.len(),
2045 dropped_band: band,
2046 rungs: round + 1,
2047 local_lane: local_interpolation,
2048 lane_switched: false,
2049 exit: PolylineFitExit::Converged,
2050 };
2051 record_polyline_fit(&report);
2052 let kept = stations.iter().map(|&index| points[index]).collect();
2053 return Ok((PolylineFit { curve, parameters, kept, report }, SeededPath::Accepted { rounds: round + 1 }, cost));
2054 }
2055 if stations.len() >= ceiling {
2056 return full_refit(SeededFallback::CeilingReached, cost);
2057 }
2058 if round == 8 {
2059 return full_refit(SeededFallback::RoundsSpent, cost);
2060 }
2061 let mut candidates = Vec::new();
2062 for span in 0..stations.len() - 1 {
2063 let (from, to) = (stations[span], stations[span + 1]);
2064 let (t0, t1) = (parameters[span], parameters[span + 1]);
2065 let lower = parameters[span.saturating_sub(1)];
2066 let upper = parameters[(span + 2).min(stations.len() - 1)];
2067 let mut candidate: Option<(usize, f64)> = None;
2068 for &index in pool.iter().filter(|&&i| i > from && i < to) {
2069 let length = arc[to] - arc[from];
2070 let seed = if length > 0.0 { t0 + (t1 - t0) * (arc[index] - arc[from]) / length } else { t0 };
2071 let Ok(error) = distance_to_curve_near(&curve, points[index], seed, lower, upper) else {
2072 return full_refit(SeededFallback::SpanUnreadable, cost);
2073 };
2074 if !error.is_finite() {
2075 return full_refit(SeededFallback::SpanUnreadable, cost);
2076 }
2077 if candidate.is_none_or(|(_, previous)| error > previous) {
2078 candidate = Some((index, error));
2079 }
2080 }
2081 if let Some((index, error)) = candidate {
2082 if error > tolerance {
2083 candidates.push((index, error));
2084 }
2085 }
2086 }
2087 for &index in &tight_over {
2090 let at = stations.binary_search(&index).unwrap_err();
2091 let (from, to) = (stations[at - 1], stations[at]);
2092 let nearest = pool
2093 .iter()
2094 .copied()
2095 .filter(|&member| member > from && member < to)
2096 .min_by(|&a, &b| (arc[a] - arc[index]).abs().total_cmp(&(arc[b] - arc[index]).abs()).then_with(|| a.cmp(&b)));
2097 if let Some(member) = nearest {
2098 match candidates.iter_mut().find(|(chosen, _)| *chosen == member) {
2101 Some(entry) => entry.1 = f64::INFINITY,
2102 None => candidates.push((member, f64::INFINITY)),
2103 }
2104 }
2105 }
2106 candidates.sort_by(|a, b| b.1.total_cmp(&a.1).then_with(|| a.0.cmp(&b.0)));
2107 let added = candidates.len().min(16).min(ceiling - stations.len());
2108 if added == 0 {
2109 return full_refit(SeededFallback::NothingToAdd, cost);
2112 }
2113 stations.extend(candidates.iter().take(added).map(|&(index, _)| index));
2114 stations.sort_unstable();
2115 }
2116 full_refit(SeededFallback::RoundsSpent, cost)
2117}
2118
2119
2120fn fit_polyline_inner(
2121 points: &[Vec3],
2122 tolerance: f64,
2123 maximum_points: usize,
2124 local_interpolation: bool,
2125 refine: bool,
2126 degree: usize,
2127) -> Result<PolylineFit, String> {
2128 let pool = station_pool(points, tolerance);
2129 if pool.len() < 2 {
2130 return Err("fit_polyline: degenerate polyline".into());
2131 }
2132 let arc = cumulative_chord(points);
2133 let band = dropped_band(points, &pool);
2134 let floor = tolerance.max(band);
2135 let mut budget = maximum_points.max(2);
2136 let ceiling = pool
2137 .len()
2138 .min(MAX_FIT_STATIONS)
2139 .min(budget.saturating_mul(LADDER_BUDGET_FACTOR))
2140 .max(budget);
2141 let (mut curve, mut stations, mut parameters, mut deviation, mut rms, mut between) =
2142 fit_rung(points, &pool, &arc, budget, local_interpolation, degree)?;
2143 let mut rungs = 1usize;
2144 let mut exit = if deviation <= floor {
2145 PolylineFitExit::Converged
2146 } else {
2147 PolylineFitExit::Unrefined
2148 };
2149 while refine && deviation > floor {
2150 if budget >= ceiling {
2151 exit = if ceiling >= pool.len() {
2152 PolylineFitExit::InputExhausted
2153 } else {
2154 PolylineFitExit::StationCeiling
2155 };
2156 break;
2157 }
2158 budget = (budget * 2).min(ceiling);
2159 let rung = fit_rung(points, &pool, &arc, budget, local_interpolation, degree)?;
2160 rungs += 1;
2161 if rung.3 >= deviation * STALL_FACTOR {
2162 if rung.3 < deviation {
2163 (curve, stations, parameters, deviation, rms, between) = rung;
2164 }
2165 exit = if deviation <= floor {
2169 PolylineFitExit::Converged
2170 } else {
2171 PolylineFitExit::Stalled
2172 };
2173 break;
2174 }
2175 (curve, stations, parameters, deviation, rms, between) = rung;
2176 if deviation <= floor {
2177 exit = PolylineFitExit::Converged;
2178 }
2179 }
2180 let report = PolylineFitReport {
2181 deviation,
2182 rms,
2183 between_stations: between,
2184 tolerance,
2185 floor,
2186 stations: stations.len(),
2187 controls: curve.control_points.len(),
2188 input_points: points.len(),
2189 pool_points: pool.len(),
2190 dropped_band: band,
2191 rungs,
2192 local_lane: local_interpolation,
2193 lane_switched: false,
2194 exit,
2195 };
2196 Ok(PolylineFit {
2197 curve,
2198 parameters,
2199 kept: stations.iter().map(|index| points[*index]).collect(),
2200 report,
2201 })
2202}
2203
2204#[derive(Clone, Copy, Debug, Default, PartialEq)]
2217pub struct PolylineFitLedger {
2218 pub fits: u64,
2220 pub unmet: u64,
2222 pub station_ceiling: u64,
2224 pub input_exhausted: u64,
2226 pub stalled: u64,
2228 pub unrefined: u64,
2230 pub refined: u64,
2232 pub met_at_pool: u64,
2235 pub lane_switched: u64,
2238 pub extra_rungs: u64,
2240 pub worst_unmet_deviation: f64,
2242 pub worst_dropped_band: f64,
2244}
2245
2246impl PolylineFitLedger {
2247 fn record(&mut self, report: &PolylineFitReport) {
2248 self.fits += 1;
2249 self.extra_rungs += (report.rungs - 1) as u64;
2250 if report.rungs > 1 {
2251 self.refined += 1;
2252 }
2253 self.worst_dropped_band = self.worst_dropped_band.max(report.dropped_band);
2254 if report.lane_switched {
2255 self.lane_switched += 1;
2256 }
2257 if report.met_tolerance() {
2258 if report.saturated() {
2259 self.met_at_pool += 1;
2260 }
2261 return;
2262 }
2263 self.unmet += 1;
2264 match report.exit {
2265 PolylineFitExit::Converged => {}
2266 PolylineFitExit::StationCeiling => self.station_ceiling += 1,
2267 PolylineFitExit::InputExhausted => self.input_exhausted += 1,
2268 PolylineFitExit::Stalled => self.stalled += 1,
2269 PolylineFitExit::Unrefined => self.unrefined += 1,
2270 }
2271 self.worst_unmet_deviation = self.worst_unmet_deviation.max(report.deviation);
2272 }
2273
2274 fn fold(&mut self, child: &PolylineFitLedger) {
2275 self.fits += child.fits;
2276 self.unmet += child.unmet;
2277 self.station_ceiling += child.station_ceiling;
2278 self.input_exhausted += child.input_exhausted;
2279 self.stalled += child.stalled;
2280 self.unrefined += child.unrefined;
2281 self.refined += child.refined;
2282 self.met_at_pool += child.met_at_pool;
2283 self.lane_switched += child.lane_switched;
2284 self.extra_rungs += child.extra_rungs;
2285 self.worst_unmet_deviation = self.worst_unmet_deviation.max(child.worst_unmet_deviation);
2286 self.worst_dropped_band = self.worst_dropped_band.max(child.worst_dropped_band);
2287 }
2288
2289 pub fn report_into(&self, diagnostics: &mut KernelDiagnostics) {
2296 diagnostics.count_n("fit.polyline.fits", self.fits);
2297 diagnostics.count_n("fit.polyline.unmet_tolerance", self.unmet);
2298 diagnostics.count_n("fit.polyline.exit.station_ceiling", self.station_ceiling);
2299 diagnostics.count_n("fit.polyline.exit.input_exhausted", self.input_exhausted);
2300 diagnostics.count_n("fit.polyline.exit.stalled", self.stalled);
2301 diagnostics.count_n("fit.polyline.exit.unrefined", self.unrefined);
2302 diagnostics.count_n("fit.polyline.refined", self.refined);
2303 diagnostics.count_n("fit.polyline.met_at_pool", self.met_at_pool);
2304 diagnostics.count_n("fit.polyline.lane_switched", self.lane_switched);
2305 diagnostics.count_n("fit.polyline.extra_rungs", self.extra_rungs);
2306 diagnostics.measure_max("fit.polyline.worst_unmet_deviation", self.worst_unmet_deviation);
2307 diagnostics.measure_max("fit.polyline.worst_dropped_band", self.worst_dropped_band);
2308 if self.unmet > 0 {
2309 diagnostics.event(
2310 DiagnosticSeverity::Degraded,
2311 KernelStage::Intersect,
2312 "fit.polyline.unmet_tolerance",
2313 format!(
2314 "{} of {} polyline fits did not reach the tolerance they were given \
2315 (station ceiling {}, input exhausted {}, stalled {}, unrefined {}); \
2316 worst deviation {:.3e}; the simplification's own band reaches {:.3e}",
2317 self.unmet,
2318 self.fits,
2319 self.station_ceiling,
2320 self.input_exhausted,
2321 self.stalled,
2322 self.unrefined,
2323 self.worst_unmet_deviation,
2324 self.worst_dropped_band,
2325 ),
2326 );
2327 }
2328 }
2329}
2330
2331thread_local! {
2332 static POLYLINE_FIT_SCOPES: std::cell::RefCell<Vec<PolylineFitLedger>> =
2334 const { std::cell::RefCell::new(Vec::new()) };
2335}
2336
2337pub struct PolylineFitScope {
2344 _thread_bound: std::marker::PhantomData<*const ()>,
2346}
2347
2348impl PolylineFitScope {
2349 pub fn open() -> Self {
2350 POLYLINE_FIT_SCOPES.with(|scopes| scopes.borrow_mut().push(PolylineFitLedger::default()));
2351 Self {
2352 _thread_bound: std::marker::PhantomData,
2353 }
2354 }
2355
2356 pub fn close(self) -> PolylineFitLedger {
2358 let ledger = POLYLINE_FIT_SCOPES.with(|scopes| {
2359 let mut scopes = scopes.borrow_mut();
2360 let ledger = scopes.pop().unwrap_or_default();
2361 if let Some(parent) = scopes.last_mut() {
2362 parent.fold(&ledger);
2363 }
2364 ledger
2365 });
2366 std::mem::forget(self);
2367 ledger
2368 }
2369}
2370
2371impl Drop for PolylineFitScope {
2372 fn drop(&mut self) {
2373 POLYLINE_FIT_SCOPES.with(|scopes| {
2374 scopes.borrow_mut().pop();
2375 });
2376 }
2377}
2378
2379fn record_polyline_fit(report: &PolylineFitReport) {
2380 if std::env::var_os("BREP_DEBUG_POLYLINE_FIT").is_some() {
2383 eprintln!("POLYLINE-FIT {report:?}");
2384 }
2385 POLYLINE_FIT_SCOPES.with(|scopes| {
2386 if let Some(open) = scopes.borrow_mut().last_mut() {
2387 open.record(report);
2388 }
2389 });
2390}
2391
2392
2393
2394fn raw_basis(knots: &[f64], degree: usize, span: usize, parameter: f64) -> Vec<f64> {
2398 crate::curve::basis_functions_raw(knots, degree, span, parameter, 0.0)
2399}
2400
2401fn raw_insert_knot(knots: &mut Vec<f64>, controls: &mut Vec<Vec3>, degree: usize, parameter: f64) {
2403 let span = knots
2407 .iter()
2408 .rposition(|&knot| knot <= parameter + 1e-14)
2409 .unwrap()
2410 .min(controls.len() - 1);
2411 let (fresh, fresh_knots) = crate::curve::boehm_insert_raw(controls, knots, degree, parameter, span,
2412 |previous, current, numerator, denominator| {
2413 let alpha = if denominator.abs() > 0.0 { numerator / denominator } else { 0.0 };
2414 previous.scale(1.0 - alpha).add(current.scale(alpha))
2415 });
2416 *controls = fresh;
2417 *knots = fresh_knots;
2418}
2419
2420fn closed_collocation_matrix(raw_knots: &[f64], parameters: &[f64], degree: usize, station_count: usize) -> Vec<Vec<f64>> {
2432 let mut matrix = vec![vec![0.0; station_count]; station_count];
2433 for i in 0..station_count {
2434 let span = i + degree;
2435 let basis = raw_basis(raw_knots, degree, span, parameters[i]);
2436 for (offset, value) in basis.iter().enumerate() {
2437 let index = (i as i64 + offset as i64 - degree as i64).rem_euclid(station_count as i64) as usize;
2438 matrix[i][index] += value;
2439 }
2440 }
2441 matrix
2442}
2443
2444pub fn interpolate_curve_closed(points: &[Vec3], parameters: &[f64]) -> Result<NurbsCurve, String> {
2445 interpolate_curve_closed_of_degree(points, parameters, 3)
2446}
2447
2448pub fn interpolate_curve_closed_of_degree(
2454 points: &[Vec3],
2455 parameters: &[f64],
2456 degree: usize,
2457) -> Result<NurbsCurve, String> {
2458 if degree % 2 == 0 || degree < 3 {
2459 return Err("interpolate_curve_closed: the degree must be odd and at least 3".into());
2460 }
2461 let station_count = points.len();
2462 if station_count < degree + 1 {
2463 return Err("interpolate_curve_closed: need more stations than the degree".into());
2464 }
2465 if parameters.len() != station_count + 1 {
2466 return Err(
2467 "interpolate_curve_closed: parameters must have one more entry than points".into(),
2468 );
2469 }
2470 if parameters.windows(2).any(|pair| pair[1] <= pair[0]) {
2471 return Err("interpolate_curve_closed: parameters must increase".into());
2472 }
2473 let period = parameters[station_count] - parameters[0];
2474 let cyclic = |j: i64| -> f64 {
2477 let s = station_count as i64;
2478 let wrap = j.div_euclid(s);
2479 parameters[j.rem_euclid(s) as usize] + wrap as f64 * period
2480 };
2481 let raw_knots: Vec<f64> = (-(degree as i64)..=(station_count as i64 + degree as i64)).map(cyclic).collect();
2482 let matrix = closed_collocation_matrix(&raw_knots, parameters, degree, station_count);
2483 let [xs, ys, zs] = solve_dense_shared(
2486 matrix,
2487 [
2488 points.iter().map(|point| point.x).collect::<Vec<_>>(),
2489 points.iter().map(|point| point.y).collect::<Vec<_>>(),
2490 points.iter().map(|point| point.z).collect::<Vec<_>>(),
2491 ],
2492 )?;
2493 let cyclic_controls: Vec<Vec3> = (0..station_count)
2494 .map(|index| Vec3::new(xs[index], ys[index], zs[index]))
2495 .collect();
2496 let mut window_controls: Vec<Vec3> = (-(degree as i64)..(station_count as i64))
2498 .map(|j| cyclic_controls[j.rem_euclid(station_count as i64) as usize])
2499 .collect();
2500 let mut window_knots = raw_knots.clone();
2501 for _ in 0..degree {
2504 raw_insert_knot(
2505 &mut window_knots,
2506 &mut window_controls,
2507 degree,
2508 parameters[0],
2509 );
2510 }
2511 for _ in 0..degree {
2512 raw_insert_knot(
2513 &mut window_knots,
2514 &mut window_controls,
2515 degree,
2516 parameters[station_count],
2517 );
2518 }
2519 let first = window_knots
2523 .iter()
2524 .position(|&knot| (knot - parameters[0]).abs() < 1e-12)
2525 .ok_or("interpolate_curve_closed: clamp lost the start knot")?;
2526 let last = window_knots
2527 .iter()
2528 .rposition(|&knot| (knot - parameters[station_count]).abs() < 1e-12)
2529 .ok_or("interpolate_curve_closed: clamp lost the end knot")?;
2530 let clamped_knots: Vec<f64> = window_knots[first..=last].to_vec();
2531 let control_count = clamped_knots.len() - degree - 1;
2532 let clamped_controls: Vec<Vec4> = window_controls[first..first + control_count]
2533 .iter()
2534 .map(|point| Vec4::from_point(*point, 1.0))
2535 .collect();
2536 NurbsCurve::new(degree, clamped_knots, clamped_controls)
2537}
2538