1use crate::Vec3;
2use serde::{Deserialize, Serialize};
3
4const EPS: f64 = 1e-12;
5
6pub const KNOT_IDENTITY_TOL: f64 = 1e-9;
13
14pub const KNOT_DEDUP_EPS: f64 = 1e-12;
28
29pub(crate) fn interior_knot_count(knots: &[f64], degree: usize) -> usize {
33 distinct_interior_knots(knots, degree).count()
34}
35
36fn distinct_interior_knots(knots: &[f64], degree: usize) -> impl Iterator<Item = f64> + '_ {
37 let start = knots[degree];
38 let end = knots[knots.len() - 1 - degree];
39 let mut previous = None;
40 knots.iter().copied().filter(move |&knot| {
41 if knot <= start + KNOT_DEDUP_EPS || knot >= end - KNOT_DEDUP_EPS {
42 return false;
43 }
44 if previous.is_none_or(|value: f64| (value - knot).abs() > KNOT_DEDUP_EPS) {
45 previous = Some(knot);
46 true
47 } else {
48 false
49 }
50 })
51}
52
53#[derive(Clone, Copy, Debug, Deserialize, Serialize)]
54pub struct Vec4 {
55 pub x: f64,
56 pub y: f64,
57 pub z: f64,
58 pub w: f64,
59}
60
61pub(crate) fn parameter_line(u0: f64, v0: f64, u1: f64, v1: f64) -> Result<NurbsCurve, String> {
63 make_line(Vec3::new(u0, v0, 0.0), Vec3::new(u1, v1, 0.0))
64}
65
66pub(crate) fn curve_to_plane_parameters(
68 curve: &NurbsCurve,
69 origin: Vec3,
70 x_axis: Vec3,
71 y_axis: Vec3,
72) -> Result<NurbsCurve, String> {
73 NurbsCurve::new(
74 curve.degree,
75 curve.knots.clone(),
76 curve
77 .control_points
78 .iter()
79 .map(|point| {
80 let euclidean = Vec3::new(point.x / point.w, point.y / point.w, point.z / point.w);
81 let delta = euclidean.sub(origin);
82 Vec4 {
83 x: delta.dot(x_axis) * point.w,
84 y: delta.dot(y_axis) * point.w,
85 z: 0.0,
86 w: point.w,
87 }
88 })
89 .collect(),
90 )
91}
92
93impl Vec4 {
94 pub fn from_point(point: Vec3, weight: f64) -> Self {
95 Self {
96 x: point.x * weight,
97 y: point.y * weight,
98 z: point.z * weight,
99 w: weight,
100 }
101 }
102
103 pub(crate) fn add(self, rhs: Self) -> Self {
104 Self {
105 x: self.x + rhs.x,
106 y: self.y + rhs.y,
107 z: self.z + rhs.z,
108 w: self.w + rhs.w,
109 }
110 }
111
112 pub(crate) fn scale(self, factor: f64) -> Self {
113 Self {
114 x: self.x * factor,
115 y: self.y * factor,
116 z: self.z * factor,
117 w: self.w * factor,
118 }
119 }
120
121 pub(crate) fn point(self) -> Result<Vec3, String> {
122 if self.w.abs() <= EPS {
123 return Err("cannot project a homogeneous point with zero weight".into());
124 }
125 Ok(Vec3::new(self.x / self.w, self.y / self.w, self.z / self.w))
126 }
127}
128
129pub(crate) const MAX_STACK_DEGREE: usize = 7;
133pub(crate) const MAX_STACK_ORDER: usize = MAX_STACK_DEGREE + 1;
134
135fn knot_reject(reason: &str, knots: &[f64], degree: usize) -> String {
141 if std::env::var("BREP_DEBUG_KNOTS").is_err() {
142 return reason.to_string();
143 }
144 let mut worst_descent = f64::NEG_INFINITY;
145 let mut worst_index = 0usize;
146 for (index, pair) in knots.windows(2).enumerate() {
147 let descent = pair[0] - pair[1];
148 if descent > worst_descent {
149 worst_descent = descent;
150 worst_index = index;
151 }
152 }
153 let detail = format!(
154 "{reason} [degree={degree} count={} worst_descent={worst_descent:.6e} at index \
155 {worst_index} knots={knots:?}]",
156 knots.len()
157 );
158 eprintln!(
159 "KNOT-REJECT {detail}\n{}",
160 std::backtrace::Backtrace::force_capture()
161 );
162 detail
163}
164
165pub(crate) fn validate_knots(knots: &[f64], degree: usize) -> Result<(), String> {
169 if degree < 1 {
170 return Err("KnotVector: degree must be >= 1".into());
171 }
172 if knots.len() < 2 * (degree + 1) {
173 return Err(format!(
174 "KnotVector: need at least {} knots for degree {}, got {}",
175 2 * (degree + 1),
176 degree,
177 knots.len()
178 ));
179 }
180 if knots.iter().any(|value| !value.is_finite()) {
181 return Err("KnotVector: knots must be finite".into());
182 }
183 if knots
184 .windows(2)
185 .any(|pair| pair[1] < pair[0] - KNOT_IDENTITY_TOL)
186 {
187 return Err(knot_reject(
188 "KnotVector: knots must be non-decreasing",
189 knots,
190 degree,
191 ));
192 }
193 let first = knots[0];
194 let last = knots[knots.len() - 1];
195 for index in 0..=degree {
196 if (knots[index] - first).abs() > KNOT_IDENTITY_TOL {
197 return Err(knot_reject(
198 "KnotVector: expected clamped start",
199 knots,
200 degree,
201 ));
202 }
203 if (knots[knots.len() - 1 - index] - last).abs() > KNOT_IDENTITY_TOL {
204 return Err(knot_reject("KnotVector: expected clamped end", knots, degree));
205 }
206 }
207 if last - first <= KNOT_IDENTITY_TOL {
208 return Err(knot_reject(
209 "KnotVector: degenerate parameter range",
210 knots,
211 degree,
212 ));
213 }
214 Ok(())
215}
216
217pub(crate) fn knot_domain(knots: &[f64], degree: usize) -> [f64; 2] {
218 [knots[degree], knots[knots.len() - 1 - degree]]
219}
220
221pub(crate) fn knot_clamp(knots: &[f64], degree: usize, parameter: f64) -> f64 {
222 let [start, end] = knot_domain(knots, degree);
223 parameter.clamp(start, end)
224}
225
226pub(crate) fn knot_find_span(knots: &[f64], degree: usize, parameter: f64) -> usize {
227 let parameter = knot_clamp(knots, degree, parameter);
228 let n = knots.len() - degree - 2;
229 if parameter >= knots[n + 1] {
230 let mut span = n;
237 while span > degree && knots[span] >= knots[span + 1] {
238 span -= 1;
239 }
240 return span;
241 }
242 if parameter <= knots[degree] {
243 let mut span = degree;
250 while span < n && knots[span + 1] <= parameter {
251 span += 1;
252 }
253 return span;
254 }
255 let mut low = degree;
256 let mut high = n + 1;
257 let mut middle = (low + high) / 2;
258 while parameter < knots[middle] || parameter >= knots[middle + 1] {
259 if parameter < knots[middle] {
260 high = middle;
261 } else {
262 low = middle;
263 }
264 middle = (low + high) / 2;
265 }
266 middle
267}
268
269pub(crate) fn basis_functions_into(
272 knots: &[f64],
273 degree: usize,
274 span: usize,
275 parameter: f64,
276 basis: &mut [f64; MAX_STACK_ORDER],
277) {
278 debug_assert!(degree <= MAX_STACK_DEGREE);
279 let mut left = [0.0f64; MAX_STACK_ORDER];
280 let mut right = [0.0f64; MAX_STACK_ORDER];
281 basis[0] = 1.0;
282 for j in 1..=degree {
283 left[j] = parameter - knots[span + 1 - j];
284 right[j] = knots[span + j] - parameter;
285 let mut saved = 0.0;
286 for r in 0..j {
287 let denominator = right[r + 1] + left[j - r];
288 let temporary = if denominator.abs() <= EPS {
289 0.0
290 } else {
291 basis[r] / denominator
292 };
293 basis[r] = saved + right[r + 1] * temporary;
294 saved = left[j - r] * temporary;
295 }
296 basis[j] = saved;
297 }
298}
299
300pub(crate) fn basis_derivatives_into(
306 knots: &[f64],
307 degree: usize,
308 span: usize,
309 parameter: f64,
310 derivative_count: usize,
311 out: &mut [[f64; MAX_STACK_ORDER]],
312) {
313 debug_assert!(degree <= MAX_STACK_DEGREE);
314 debug_assert!(out.len() > derivative_count);
315 let p = degree;
316 let n = derivative_count.min(p);
317 let mut ndu = [[0.0f64; MAX_STACK_ORDER]; MAX_STACK_ORDER];
318 let mut left = [0.0f64; MAX_STACK_ORDER];
319 let mut right = [0.0f64; MAX_STACK_ORDER];
320 ndu[0][0] = 1.0;
321
322 for j in 1..=p {
323 left[j] = parameter - knots[span + 1 - j];
324 right[j] = knots[span + j] - parameter;
325 let mut saved = 0.0;
326 for r in 0..j {
327 ndu[j][r] = right[r + 1] + left[j - r];
328 let temporary = if ndu[j][r].abs() <= EPS {
329 0.0
330 } else {
331 ndu[r][j - 1] / ndu[j][r]
332 };
333 ndu[r][j] = saved + right[r + 1] * temporary;
334 saved = left[j - r] * temporary;
335 }
336 ndu[j][j] = saved;
337 }
338
339 for j in 0..=p {
340 out[0][j] = ndu[j][p];
341 }
342 let mut a = [[0.0f64; MAX_STACK_ORDER]; 2];
343 for r in 0..=p {
344 let mut s1 = 0;
345 let mut s2 = 1;
346 a[0][0] = 1.0;
347 for k in 1..=n {
348 a[s2] = [0.0; MAX_STACK_ORDER];
349 let mut value = 0.0;
350 let rk = r as isize - k as isize;
351 let pk = p - k;
352 if r >= k {
353 let denominator = ndu[pk + 1][rk as usize];
354 a[s2][0] = a[s1][0] / denominator;
355 value = a[s2][0] * ndu[rk as usize][pk];
356 }
357 let j1 = if rk >= -1 { 1 } else { (-rk) as usize };
358 let j2 = if r <= pk + 1 { k - 1 } else { p - r };
359 if j1 <= j2 {
360 for j in j1..=j2 {
361 let index = (rk + j as isize) as usize;
362 a[s2][j] = (a[s1][j] - a[s1][j - 1]) / ndu[pk + 1][index];
363 value += a[s2][j] * ndu[index][pk];
364 }
365 }
366 if r <= pk {
367 a[s2][k] = -a[s1][k - 1] / ndu[pk + 1][r];
368 value += a[s2][k] * ndu[r][pk];
369 }
370 out[k][r] = value;
371 std::mem::swap(&mut s1, &mut s2);
372 }
373 }
374 let mut factor = p as f64;
375 for k in 1..=n {
376 for value in out[k][..=p].iter_mut() {
377 *value *= factor;
378 }
379 factor *= (p - k) as f64;
380 }
381}
382
383#[derive(Clone, Debug, Deserialize, Serialize)]
384pub struct KnotVector {
385 pub knots: Vec<f64>,
386 pub degree: usize,
387}
388
389pub(crate) fn interior_knots(knots: &[f64], degree: usize) -> Vec<f64> {
391 distinct_interior_knots(knots, degree).collect()
392}
393
394impl KnotVector {
395 pub fn new(knots: Vec<f64>, degree: usize) -> Result<Self, String> {
396 validate_knots(&knots, degree)?;
397 Ok(Self { knots, degree })
398 }
399
400 pub fn control_point_count(&self) -> usize {
401 self.knots.len() - self.degree - 1
402 }
403
404 pub fn domain(&self) -> [f64; 2] {
405 [
406 self.knots[self.degree],
407 self.knots[self.knots.len() - 1 - self.degree],
408 ]
409 }
410
411 pub fn clamp_param(&self, parameter: f64) -> f64 {
412 knot_clamp(&self.knots, self.degree, parameter)
413 }
414
415 pub fn find_span(&self, parameter: f64) -> usize {
416 knot_find_span(&self.knots, self.degree, parameter)
417 }
418
419 pub fn basis_functions(&self, span: usize, parameter: f64) -> Vec<f64> {
420 if self.degree <= MAX_STACK_DEGREE {
421 let mut basis = [0.0f64; MAX_STACK_ORDER];
422 basis_functions_into(&self.knots, self.degree, span, parameter, &mut basis);
423 return basis[..=self.degree].to_vec();
424 }
425 let mut basis = vec![0.0; self.degree + 1];
426 let mut left = vec![0.0; self.degree + 1];
427 let mut right = vec![0.0; self.degree + 1];
428 basis[0] = 1.0;
429 for j in 1..=self.degree {
430 left[j] = parameter - self.knots[span + 1 - j];
431 right[j] = self.knots[span + j] - parameter;
432 let mut saved = 0.0;
433 for r in 0..j {
434 let denominator = right[r + 1] + left[j - r];
435 let temporary = if denominator.abs() <= EPS {
436 0.0
437 } else {
438 basis[r] / denominator
439 };
440 basis[r] = saved + right[r + 1] * temporary;
441 saved = left[j - r] * temporary;
442 }
443 basis[j] = saved;
444 }
445 basis
446 }
447
448 pub fn basis_derivatives(
449 &self,
450 span: usize,
451 parameter: f64,
452 derivative_count: usize,
453 ) -> Vec<Vec<f64>> {
454 if self.degree <= MAX_STACK_DEGREE && derivative_count <= MAX_STACK_DEGREE {
455 let mut rows = [[0.0f64; MAX_STACK_ORDER]; MAX_STACK_ORDER];
456 basis_derivatives_into(
457 &self.knots,
458 self.degree,
459 span,
460 parameter,
461 derivative_count,
462 &mut rows[..=derivative_count],
463 );
464 return rows[..=derivative_count]
465 .iter()
466 .map(|row| row[..=self.degree].to_vec())
467 .collect();
468 }
469 let p = self.degree;
470 let n = derivative_count.min(p);
471 let mut ndu = vec![vec![0.0; p + 1]; p + 1];
472 let mut left = vec![0.0; p + 1];
473 let mut right = vec![0.0; p + 1];
474 ndu[0][0] = 1.0;
475
476 for j in 1..=p {
477 left[j] = parameter - self.knots[span + 1 - j];
478 right[j] = self.knots[span + j] - parameter;
479 let mut saved = 0.0;
480 for r in 0..j {
481 ndu[j][r] = right[r + 1] + left[j - r];
482 let temporary = if ndu[j][r].abs() <= EPS {
483 0.0
484 } else {
485 ndu[r][j - 1] / ndu[j][r]
486 };
487 ndu[r][j] = saved + right[r + 1] * temporary;
488 saved = left[j - r] * temporary;
489 }
490 ndu[j][j] = saved;
491 }
492
493 let mut derivatives = vec![vec![0.0; p + 1]; derivative_count + 1];
494 for j in 0..=p {
495 derivatives[0][j] = ndu[j][p];
496 }
497 let mut a = vec![vec![0.0; p + 1]; 2];
498 for r in 0..=p {
499 let mut s1 = 0;
500 let mut s2 = 1;
501 a[0][0] = 1.0;
502 for k in 1..=n {
503 a[s2].fill(0.0);
504 let mut value = 0.0;
505 let rk = r as isize - k as isize;
506 let pk = p - k;
507 if r >= k {
508 let denominator = ndu[pk + 1][rk as usize];
509 a[s2][0] = a[s1][0] / denominator;
510 value = a[s2][0] * ndu[rk as usize][pk];
511 }
512 let j1 = if rk >= -1 { 1 } else { (-rk) as usize };
513 let j2 = if r <= pk + 1 { k - 1 } else { p - r };
514 if j1 <= j2 {
515 for j in j1..=j2 {
516 let index = (rk + j as isize) as usize;
517 a[s2][j] = (a[s1][j] - a[s1][j - 1]) / ndu[pk + 1][index];
518 value += a[s2][j] * ndu[index][pk];
519 }
520 }
521 if r <= pk {
522 a[s2][k] = -a[s1][k - 1] / ndu[pk + 1][r];
523 value += a[s2][k] * ndu[r][pk];
524 }
525 derivatives[k][r] = value;
526 std::mem::swap(&mut s1, &mut s2);
527 }
528 }
529 let mut factor = p as f64;
530 for (k, row) in derivatives.iter_mut().enumerate().take(n + 1).skip(1) {
531 for value in row {
532 *value *= factor;
533 }
534 factor *= (p - k) as f64;
535 }
536 derivatives
537 }
538}
539
540#[derive(Clone, Debug, Deserialize, Serialize)]
548pub struct NurbsCurve {
549 pub degree: usize,
550 pub knots: Vec<f64>,
551 pub control_points: Vec<Vec4>,
552 #[serde(skip, default)]
556 validated: std::cell::Cell<bool>,
557}
558
559impl NurbsCurve {
560 pub(crate) fn is_validated(&self) -> bool {
563 self.validated.get()
564 }
565
566 pub(crate) fn set_validated(&mut self, validated: bool) {
568 self.validated = std::cell::Cell::new(validated);
569 }
570
571 pub fn new(degree: usize, knots: Vec<f64>, control_points: Vec<Vec4>) -> Result<Self, String> {
572 let curve = Self {
573 degree,
574 knots,
575 control_points,
576 validated: std::cell::Cell::new(false),
577 };
578 curve.ensure_valid()?;
579 Ok(curve)
580 }
581
582 fn ensure_valid(&self) -> Result<(), String> {
584 if self.validated.get() {
585 return Ok(());
586 }
587 validate_knots(&self.knots, self.degree)?;
588 let expected = self.knots.len() - self.degree - 1;
589 if self.control_points.len() != expected {
590 return Err(format!(
591 "NurbsCurve: knot vector implies {} control points, got {}",
592 expected,
593 self.control_points.len()
594 ));
595 }
596 if self.control_points.iter().any(|point| {
597 point.w <= EPS
598 || ![point.x, point.y, point.z, point.w]
599 .iter()
600 .all(|value| value.is_finite())
601 }) {
602 return Err("NurbsCurve: control points must be finite with positive weights".into());
603 }
604 self.validated.set(true);
605 Ok(())
606 }
607
608 fn knot_vector(&self) -> Result<KnotVector, String> {
609 KnotVector::new(self.knots.clone(), self.degree)
610 }
611
612 pub fn domain(&self) -> Result<[f64; 2], String> {
613 self.ensure_valid()?;
614 Ok(knot_domain(&self.knots, self.degree))
615 }
616
617 pub fn evaluate_homogeneous(&self, parameter: f64) -> Result<Vec4, String> {
618 self.ensure_valid()?;
619 let span = knot_find_span(&self.knots, self.degree, parameter);
620 let parameter = knot_clamp(&self.knots, self.degree, parameter);
621 let mut point = Vec4 {
622 x: 0.0,
623 y: 0.0,
624 z: 0.0,
625 w: 0.0,
626 };
627 if self.degree <= MAX_STACK_DEGREE {
628 let mut basis = [0.0f64; MAX_STACK_ORDER];
629 basis_functions_into(&self.knots, self.degree, span, parameter, &mut basis);
630 for (index, value) in basis[..=self.degree].iter().enumerate() {
631 point = point.add(self.control_points[span - self.degree + index].scale(*value));
632 }
633 } else {
634 let knot_vector = self.knot_vector()?;
635 let basis = knot_vector.basis_functions(span, parameter);
636 for (index, value) in basis.iter().enumerate() {
637 point = point.add(self.control_points[span - self.degree + index].scale(*value));
638 }
639 }
640 Ok(point)
641 }
642
643 pub fn evaluate(&self, parameter: f64) -> Result<Vec3, String> {
644 self.evaluate_homogeneous(parameter)?.point()
645 }
646
647 pub(crate) fn straight_segment(&self, tolerance: f64) -> Option<(Vec3, Vec3)> {
667 self.ensure_valid().ok()?;
668 let points = self
669 .control_points
670 .iter()
671 .map(|point| point.point().ok())
672 .collect::<Option<Vec<Vec3>>>()?;
673 let start = *points.first()?;
674 let end = *points.last()?;
675 let chord = end.sub(start);
676 let length = chord.length();
677 if length <= tolerance {
678 return None;
679 }
680 let direction = chord.scale(1.0 / length);
681 let mut reached = 0.0f64;
682 for point in &points {
683 let offset = point.sub(start);
684 let axial = offset.dot(direction);
685 if offset.sub(direction.scale(axial)).length() > tolerance {
686 return None; }
688 if axial < reached - tolerance {
689 return None; }
691 reached = reached.max(axial);
692 }
693 Some((start, end))
694 }
695
696 pub fn evaluate_extended(&self, parameter: f64) -> Result<Vec3, String> {
701 Ok(self.derivatives_extended(parameter, 0)?[0])
702 }
703
704 pub fn derivatives_extended(
708 &self,
709 parameter: f64,
710 derivative_count: usize,
711 ) -> Result<Vec<Vec3>, String> {
712 let [start, end] = self.domain()?;
713 if parameter >= start && parameter <= end {
714 return self.derivatives(parameter, derivative_count);
715 }
716 let period = end - start;
717 if period > 0.0 {
718 let closed =
719 self.evaluate(start)?.sub(self.evaluate(end)?).length() <= 1e-9 * (1.0 + period);
720 if closed {
721 let wrapped = start + (parameter - start).rem_euclid(period);
722 return self.derivatives(wrapped, derivative_count);
723 }
724 }
725 let boundary = if parameter < start { start } else { end };
726 let base = self.derivatives(boundary, derivative_count.max(1))?;
727 let mut result = Vec::with_capacity(derivative_count + 1);
728 result.push(base[0].add(base[1].scale(parameter - boundary)));
729 if derivative_count >= 1 {
730 result.push(base[1]);
731 }
732 for _ in 2..=derivative_count {
733 result.push(Vec3::default());
734 }
735 Ok(result)
736 }
737
738 pub fn derivatives(
739 &self,
740 parameter: f64,
741 derivative_count: usize,
742 ) -> Result<Vec<Vec3>, String> {
743 self.ensure_valid()?;
744 let parameter = knot_clamp(&self.knots, self.degree, parameter);
745 let span = knot_find_span(&self.knots, self.degree, parameter);
746 let calculated_count = derivative_count.min(self.degree);
747 let mut homogeneous: Vec<Vec4> = Vec::with_capacity(calculated_count + 1);
748 if self.degree <= MAX_STACK_DEGREE {
749 let mut rows = [[0.0f64; MAX_STACK_ORDER]; MAX_STACK_ORDER];
750 basis_derivatives_into(
751 &self.knots,
752 self.degree,
753 span,
754 parameter,
755 calculated_count,
756 &mut rows[..=calculated_count],
757 );
758 for row in rows.iter().take(calculated_count + 1) {
759 let mut point = Vec4 {
760 x: 0.0,
761 y: 0.0,
762 z: 0.0,
763 w: 0.0,
764 };
765 for (index, value) in row.iter().enumerate().take(self.degree + 1) {
766 point =
767 point.add(self.control_points[span - self.degree + index].scale(*value));
768 }
769 homogeneous.push(point);
770 }
771 } else {
772 let knot_vector = self.knot_vector()?;
773 let basis = knot_vector.basis_derivatives(span, parameter, calculated_count);
774 for row in basis.iter().take(calculated_count + 1) {
775 let mut point = Vec4 {
776 x: 0.0,
777 y: 0.0,
778 z: 0.0,
779 w: 0.0,
780 };
781 for (index, value) in row.iter().enumerate().take(self.degree + 1) {
782 point =
783 point.add(self.control_points[span - self.degree + index].scale(*value));
784 }
785 homogeneous.push(point);
786 }
787 }
788
789 let mut result: Vec<Vec3> = Vec::with_capacity(derivative_count + 1);
790 for k in 0..=calculated_count {
791 let mut value = Vec3::new(homogeneous[k].x, homogeneous[k].y, homogeneous[k].z);
792 for i in 1..=k {
793 value = value.sub(result[k - i].scale(binomial(k, i) * homogeneous[i].w));
794 }
795 result.push(value.scale(1.0 / homogeneous[0].w));
796 }
797 result.resize(derivative_count + 1, Vec3::default());
798 Ok(result)
799 }
800
801 pub fn unit_tangent(
826 &self,
827 parameter: f64,
828 range_start: f64,
829 range_end: f64,
830 ) -> Result<Vec3, String> {
831 let derivative = self.derivatives(parameter, 1)?[1];
832 self.tangent_or_one_sided_limit(derivative, parameter, range_start, range_end)
833 }
834
835 pub fn point_and_unit_tangent_extended(
857 &self,
858 parameter: f64,
859 range_start: f64,
860 range_end: f64,
861 ) -> Result<(Vec3, Vec3), String> {
862 let derivatives = self.derivatives_extended(parameter, 1)?;
863 if let Ok(tangent) = derivatives[1].normalized() {
864 return Ok((derivatives[0], tangent));
865 }
866 let read_at = self.extended_read_parameter(parameter)?;
867 let low = range_start.min(range_end).min(read_at);
868 let high = range_start.max(range_end).max(read_at);
869 let tangent = self.tangent_or_one_sided_limit(derivatives[1], read_at, low, high)?;
870 Ok((derivatives[0], tangent))
871 }
872
873 fn extended_read_parameter(&self, parameter: f64) -> Result<f64, String> {
879 let [start, end] = self.domain()?;
880 if parameter >= start && parameter <= end {
881 return Ok(parameter);
882 }
883 if self.extension_wraps(start, end)? {
884 return Ok(start + (parameter - start).rem_euclid(end - start));
885 }
886 Ok(if parameter < start { start } else { end })
887 }
888
889 fn extension_wraps(&self, start: f64, end: f64) -> Result<bool, String> {
892 let period = end - start;
893 Ok(period > 0.0
894 && self.evaluate(start)?.sub(self.evaluate(end)?).length() <= 1e-9 * (1.0 + period))
895 }
896
897 pub(crate) fn stationary_open_end(&self, parameter: f64) -> Result<Option<f64>, String> {
909 let [start, end] = self.domain()?;
910 if parameter >= start && parameter <= end {
911 return Ok(None);
912 }
913 let boundary = if parameter < start { start } else { end };
914 if self.derivatives(boundary, 1)?[1].normalized().is_ok()
915 || self.extension_wraps(start, end)?
916 {
917 return Ok(None);
918 }
919 Ok(Some(boundary))
920 }
921
922 fn tangent_or_one_sided_limit(
927 &self,
928 derivative: Vec3,
929 parameter: f64,
930 range_start: f64,
931 range_end: f64,
932 ) -> Result<Vec3, String> {
933 let error = match derivative.normalized() {
934 Ok(tangent) => return Ok(tangent),
935 Err(error) => error,
936 };
937 if std::env::var("BREP_CUSP_TANGENT_RESCUE").as_deref() == Ok("0") {
938 return Err(error);
939 }
940 self.stationary_tangent_rescue(parameter, range_start, range_end)
941 .ok_or_else(|| {
942 format!(
943 "curve tangent: the first derivative vanishes at t = {parameter} and \
944 everywhere in [{range_start}, {range_end}] the probe can reach, so this \
945 curve has no direction of travel to report"
946 )
947 })
948 }
949
950 pub fn stationary_tangent_rescue(
978 &self,
979 parameter: f64,
980 range_start: f64,
981 range_end: f64,
982 ) -> Option<Vec3> {
983 let low = range_start.min(range_end);
984 let high = range_start.max(range_end);
985 let span = high - low;
986 if !(span > 0.0) {
987 return None;
988 }
989 for scale in [1e-6, 1e-4, 1e-2, 1e-1] {
990 let step = span * scale;
991 for probe in [parameter + step, parameter - step] {
992 if probe < low || probe > high {
993 continue;
994 }
995 if let Some(unit) = self
996 .derivatives(probe, 1)
997 .ok()
998 .and_then(|derivatives| derivatives[1].normalized().ok())
999 {
1000 return Some(unit);
1001 }
1002 }
1003 }
1004 None
1005 }
1006
1007 pub(crate) fn derivatives_small(
1018 &self,
1019 parameter: f64,
1020 derivative_count: usize,
1021 ) -> Result<[Vec3; 3], String> {
1022 debug_assert!(derivative_count <= 2);
1023 self.ensure_valid()?;
1024 let parameter = knot_clamp(&self.knots, self.degree, parameter);
1025 let span = knot_find_span(&self.knots, self.degree, parameter);
1026 let calculated_count = derivative_count.min(self.degree);
1027 let zero = Vec4 {
1028 x: 0.0,
1029 y: 0.0,
1030 z: 0.0,
1031 w: 0.0,
1032 };
1033 let mut homogeneous = [zero; 3];
1034 if self.degree <= MAX_STACK_DEGREE {
1035 let mut rows = [[0.0f64; MAX_STACK_ORDER]; MAX_STACK_ORDER];
1036 basis_derivatives_into(
1037 &self.knots,
1038 self.degree,
1039 span,
1040 parameter,
1041 calculated_count,
1042 &mut rows[..=calculated_count],
1043 );
1044 for (k, row) in rows.iter().take(calculated_count + 1).enumerate() {
1045 let mut point = zero;
1046 for (index, value) in row.iter().enumerate().take(self.degree + 1) {
1047 point =
1048 point.add(self.control_points[span - self.degree + index].scale(*value));
1049 }
1050 homogeneous[k] = point;
1051 }
1052 } else {
1053 let knot_vector = self.knot_vector()?;
1054 let basis = knot_vector.basis_derivatives(span, parameter, calculated_count);
1055 for (k, row) in basis.iter().take(calculated_count + 1).enumerate() {
1056 let mut point = zero;
1057 for (index, value) in row.iter().enumerate().take(self.degree + 1) {
1058 point =
1059 point.add(self.control_points[span - self.degree + index].scale(*value));
1060 }
1061 homogeneous[k] = point;
1062 }
1063 }
1064
1065 let mut result = [Vec3::default(); 3];
1066 for k in 0..=calculated_count {
1067 let mut value = Vec3::new(homogeneous[k].x, homogeneous[k].y, homogeneous[k].z);
1068 for i in 1..=k {
1069 value = value.sub(result[k - i].scale(binomial(k, i) * homogeneous[i].w));
1070 }
1071 result[k] = value.scale(1.0 / homogeneous[0].w);
1072 }
1073 Ok(result)
1074 }
1075
1076 #[inline]
1079 pub(crate) fn deriv1(&self, parameter: f64) -> Result<(Vec3, Vec3), String> {
1080 let d = self.derivatives_small(parameter, 1)?;
1081 Ok((d[0], d[1]))
1082 }
1083
1084 pub fn reversed(&self) -> Result<Self, String> {
1085 let start = self.knots[0];
1086 let end = self.knots[self.knots.len() - 1];
1087 let knots = self
1088 .knots
1089 .iter()
1090 .rev()
1091 .map(|knot| start + end - knot)
1092 .collect();
1093 let control_points = self.control_points.iter().rev().copied().collect();
1094 Self::new(self.degree, knots, control_points)
1095 }
1096
1097 pub fn insert_knot(&self, parameter: f64, requested: usize) -> Result<Self, String> {
1098 let knot_vector = self.knot_vector()?;
1099 let degree = self.degree;
1100 let parameter = knot_vector.clamp_param(parameter);
1101 let parameter = self
1106 .knots
1107 .iter()
1108 .copied()
1109 .find(|knot| (knot - parameter).abs() <= KNOT_IDENTITY_TOL)
1110 .unwrap_or(parameter);
1111 let multiplicity = self
1112 .knots
1113 .iter()
1114 .filter(|knot| (**knot - parameter).abs() <= KNOT_IDENTITY_TOL)
1115 .count();
1116 let insertion_count = requested.min(degree.saturating_sub(multiplicity));
1117 if insertion_count == 0 {
1118 return Ok(self.clone());
1119 }
1120 let span = knot_vector.find_span(parameter);
1121 let last_control = self.control_points.len() - 1;
1122 let mut knots = Vec::with_capacity(self.knots.len() + insertion_count);
1123 knots.extend_from_slice(&self.knots[..=span]);
1124 knots.extend(std::iter::repeat_n(parameter, insertion_count));
1125 knots.extend_from_slice(&self.knots[span + 1..]);
1126
1127 let mut output = vec![
1128 Vec4 {
1129 x: 0.0,
1130 y: 0.0,
1131 z: 0.0,
1132 w: 1.0,
1133 };
1134 last_control + 1 + insertion_count
1135 ];
1136 output[..=span - degree].copy_from_slice(&self.control_points[..=span - degree]);
1137 for index in span - multiplicity..=last_control {
1138 output[index + insertion_count] = self.control_points[index];
1139 }
1140 let mut affected = vec![
1141 Vec4 {
1142 x: 0.0,
1143 y: 0.0,
1144 z: 0.0,
1145 w: 1.0,
1146 };
1147 degree + 1
1148 ];
1149 affected[..=degree - multiplicity]
1150 .copy_from_slice(&self.control_points[span - degree..=span - multiplicity]);
1151 let mut left = 0;
1152 for insertion in 1..=insertion_count {
1153 left = span - degree + insertion;
1154 for index in 0..=degree - insertion - multiplicity {
1155 let denominator = self.knots[index + span + 1] - self.knots[left + index];
1156 let alpha = (parameter - self.knots[left + index]) / denominator;
1157 affected[index] = affected[index + 1]
1158 .scale(alpha)
1159 .add(affected[index].scale(1.0 - alpha));
1160 }
1161 output[left] = affected[0];
1162 output[span + insertion_count - insertion - multiplicity] =
1163 affected[degree - insertion - multiplicity];
1164 }
1165 for index in left + 1..span - multiplicity {
1166 output[index] = affected[index - left];
1167 }
1168 Self::new(degree, knots, output)
1169 }
1170
1171 pub fn split(&self, parameter: f64) -> Result<(Self, Self), String> {
1172 let [start, end] = self.domain()?;
1173 if parameter <= start + KNOT_IDENTITY_TOL || parameter >= end - KNOT_IDENTITY_TOL {
1174 return Err(format!(
1175 "NurbsCurve.split: parameter {parameter} must be strictly inside domain [{start}, {end}]"
1176 ));
1177 }
1178 let parameter = self
1181 .knots
1182 .iter()
1183 .copied()
1184 .find(|knot| (knot - parameter).abs() <= KNOT_IDENTITY_TOL)
1185 .unwrap_or(parameter);
1186 let multiplicity = self
1187 .knots
1188 .iter()
1189 .filter(|knot| (**knot - parameter).abs() <= KNOT_IDENTITY_TOL)
1190 .count();
1191 let refined = self.insert_knot(parameter, self.degree.saturating_sub(multiplicity))?;
1192 let first = refined
1193 .knots
1194 .iter()
1195 .position(|knot| (*knot - parameter).abs() <= KNOT_IDENTITY_TOL)
1196 .ok_or_else(|| "NurbsCurve.split: inserted knot not found".to_string())?;
1197 let mut left_knots = refined.knots[..first + self.degree].to_vec();
1198 left_knots.push(parameter);
1199 let left_points = refined.control_points[..first].to_vec();
1200 let mut right_knots = vec![parameter; self.degree + 1];
1201 right_knots.extend_from_slice(&refined.knots[first + self.degree..]);
1202 let right_points = refined.control_points[first - 1..].to_vec();
1203 Ok((
1204 Self::new(self.degree, left_knots, left_points)?,
1205 Self::new(self.degree, right_knots, right_points)?,
1206 ))
1207 }
1208}
1209
1210pub fn make_line(start: Vec3, end: Vec3) -> Result<NurbsCurve, String> {
1211 NurbsCurve::new(
1212 1,
1213 vec![0.0, 0.0, 1.0, 1.0],
1214 vec![Vec4::from_point(start, 1.0), Vec4::from_point(end, 1.0)],
1215 )
1216}
1217
1218pub fn make_arc(
1219 center: Vec3,
1220 x_axis: Vec3,
1221 y_axis: Vec3,
1222 radius: f64,
1223 start_angle: f64,
1224 end_angle: f64,
1225) -> Result<NurbsCurve, String> {
1226 if radius <= EPS {
1227 return Err("makeArc: radius must be positive".into());
1228 }
1229 let x_axis = x_axis.normalized()?;
1230 let y_axis = y_axis.normalized()?;
1231 if x_axis.dot(y_axis).abs() > 1e-9 {
1232 return Err("makeArc: xAxis and yAxis must be orthogonal".into());
1233 }
1234 let mut theta = end_angle - start_angle;
1235 if theta <= EPS {
1236 return Err("makeArc: endAngle must exceed startAngle".into());
1237 }
1238 if theta > std::f64::consts::TAU + EPS {
1239 return Err("makeArc: sweep exceeds full circle".into());
1240 }
1241 theta = theta.min(std::f64::consts::TAU);
1242 let segment_count = ((theta / std::f64::consts::FRAC_PI_2 - EPS).ceil() as usize).clamp(1, 4);
1243 let segment_angle = theta / segment_count as f64;
1244 let middle_weight = (segment_angle / 2.0).cos();
1245 let point_at = |angle: f64| {
1246 center
1247 .add(x_axis.scale(radius * angle.cos()))
1248 .add(y_axis.scale(radius * angle.sin()))
1249 };
1250 let tangent_at = |angle: f64| x_axis.scale(-angle.sin()).add(y_axis.scale(angle.cos()));
1251
1252 let mut points = Vec::with_capacity(2 * segment_count + 1);
1253 let mut angle = start_angle;
1254 let mut first_point = point_at(angle);
1255 let mut first_tangent = tangent_at(angle);
1256 points.push(Vec4::from_point(first_point, 1.0));
1257 for _ in 0..segment_count {
1258 angle += segment_angle;
1259 let end_point = point_at(angle);
1260 let end_tangent = tangent_at(angle);
1261 let cross = first_tangent.cross(end_tangent);
1262 let denominator = cross.length_squared();
1263 if denominator <= EPS {
1264 return Err("makeArc: arc tangents are parallel".into());
1265 }
1266 let distance = end_point.sub(first_point).cross(end_tangent).dot(cross) / denominator;
1267 let middle = first_point.add(first_tangent.scale(distance));
1268 points.push(Vec4::from_point(middle, middle_weight));
1269 points.push(Vec4::from_point(end_point, 1.0));
1270 first_point = end_point;
1271 first_tangent = end_tangent;
1272 }
1273 let mut knots = vec![0.0, 0.0, 0.0];
1274 for index in 1..segment_count {
1275 let knot = index as f64 / segment_count as f64;
1276 knots.extend([knot, knot]);
1277 }
1278 knots.extend([1.0, 1.0, 1.0]);
1279 NurbsCurve::new(2, knots, points)
1280}
1281
1282pub fn make_circle(center: Vec3, normal: Vec3, radius: f64) -> Result<NurbsCurve, String> {
1283 let normal = normal.normalized()?;
1284 let x_axis = normal.perpendicular()?;
1285 let y_axis = normal.cross(x_axis).normalized()?;
1286 make_arc(center, x_axis, y_axis, radius, 0.0, std::f64::consts::TAU)
1287}
1288
1289fn conic_frame(fn_name: &str, primary: Vec3, hint: Vec3) -> Result<(Vec3, Vec3), String> {
1295 let x_axis = primary
1296 .normalized()
1297 .map_err(|_| format!("{fn_name}: primary axis must be non-zero"))?;
1298 if hint.length() <= EPS {
1299 return Err(format!("{fn_name}: in-plane direction must be non-zero"));
1300 }
1301 hint.sub(x_axis.scale(hint.dot(x_axis)))
1302 .normalized()
1303 .map(|y_axis| (x_axis, y_axis))
1304 .map_err(|_| format!("{fn_name}: frame directions must not be parallel"))
1305}
1306
1307fn conic_range(fn_name: &str, t0: f64, t1: f64) -> Result<(), String> {
1311 if !t0.is_finite() || !t1.is_finite() {
1312 return Err(format!("{fn_name}: parameter range must be finite"));
1313 }
1314 if t1 - t0 <= KNOT_IDENTITY_TOL {
1315 return Err(format!("{fn_name}: t1 ({t1}) must exceed t0 ({t0})"));
1316 }
1317 Ok(())
1318}
1319
1320fn conic_points_finite(fn_name: &str, points: &[Vec4]) -> Result<(), String> {
1325 if points.iter().any(|point| {
1326 ![point.x, point.y, point.z, point.w]
1327 .iter()
1328 .all(|value| value.is_finite())
1329 }) {
1330 return Err(format!(
1331 "{fn_name}: control points overflow f64 — parameter range too extreme"
1332 ));
1333 }
1334 Ok(())
1335}
1336
1337pub fn make_parabola(
1345 vertex: Vec3,
1346 axis: Vec3,
1347 latus_direction: Vec3,
1348 focal: f64,
1349 t0: f64,
1350 t1: f64,
1351) -> Result<NurbsCurve, String> {
1352 if !focal.is_finite() || focal <= EPS {
1353 return Err("make_parabola: focal distance must be positive".into());
1354 }
1355 conic_range("make_parabola", t0, t1)?;
1356 let (x_axis, y_axis) = conic_frame("make_parabola", axis, latus_direction)?;
1357 let point_at = |t: f64| {
1358 vertex
1359 .add(x_axis.scale(focal * t * t))
1360 .add(y_axis.scale(2.0 * focal * t))
1361 };
1362 let middle = vertex
1367 .add(x_axis.scale(focal * t0 * t1))
1368 .add(y_axis.scale(focal * (t0 + t1)));
1369 let control_points = vec![
1370 Vec4::from_point(point_at(t0), 1.0),
1371 Vec4::from_point(middle, 1.0),
1372 Vec4::from_point(point_at(t1), 1.0),
1373 ];
1374 conic_points_finite("make_parabola", &control_points)?;
1375 NurbsCurve::new(2, vec![t0, t0, t0, t1, t1, t1], control_points)
1376}
1377
1378pub fn make_hyperbola(
1388 center: Vec3,
1389 major_axis: Vec3,
1390 minor_axis: Vec3,
1391 a: f64,
1392 b: f64,
1393 t0: f64,
1394 t1: f64,
1395) -> Result<NurbsCurve, String> {
1396 if !a.is_finite() || a <= EPS {
1397 return Err("make_hyperbola: semi-axis a must be positive".into());
1398 }
1399 if !b.is_finite() || b <= EPS {
1400 return Err("make_hyperbola: semi-axis b must be positive".into());
1401 }
1402 conic_range("make_hyperbola", t0, t1)?;
1403 let (x_axis, y_axis) = conic_frame("make_hyperbola", major_axis, minor_axis)?;
1404 let point_at = |t: f64| {
1405 center
1406 .add(x_axis.scale(a * t.cosh()))
1407 .add(y_axis.scale(b * t.sinh()))
1408 };
1409 let mid = 0.5 * (t0 + t1);
1410 let middle_weight = (0.5 * (t1 - t0)).cosh();
1411 let apex = center
1418 .add(x_axis.scale(a * mid.cosh() / middle_weight))
1419 .add(y_axis.scale(b * mid.sinh() / middle_weight));
1420 let control_points = vec![
1421 Vec4::from_point(point_at(t0), 1.0),
1422 Vec4::from_point(apex, middle_weight),
1423 Vec4::from_point(point_at(t1), 1.0),
1424 ];
1425 conic_points_finite("make_hyperbola", &control_points)?;
1426 NurbsCurve::new(2, vec![t0, t0, t0, t1, t1, t1], control_points)
1427}
1428
1429pub fn uniform_clamped_knots(
1430 control_point_count: usize,
1431 degree: usize,
1432) -> Result<Vec<f64>, String> {
1433 if control_point_count == 0 || control_point_count - 1 < degree {
1434 return Err("uniformClampedKnots: need at least degree+1 control points".into());
1435 }
1436 let n = control_point_count - 1;
1437 let mut knots = vec![0.0; degree + 1];
1438 let interior = n - degree;
1439 for index in 1..=interior {
1440 knots.push(index as f64 / (interior + 1) as f64);
1441 }
1442 knots.extend(std::iter::repeat_n(1.0, degree + 1));
1443 Ok(knots)
1444}
1445
1446fn binomial(n: usize, k: usize) -> f64 {
1447 if k > n {
1448 return 0.0;
1449 }
1450 let k = k.min(n - k);
1451 (1..=k).fold(1.0, |value, index| {
1452 value * (n - k + index) as f64 / index as f64
1453 })
1454}
1455
1456
1457