1use crate::curve::interior_knot_count;
2use crate::fit::solve_small;
3use crate::projection::project_point_to_surface_from_seed;
4use crate::{project_point_to_surface, KnotVector, NurbsSurface, Vec3};
5use serde::{Deserialize, Serialize};
6
7const EPSILON: f64 = 1e-12;
8const LINEAR_TOLERANCE: f64 = 1e-7;
9
10const TRANSVERSE_START_CROSS: f64 = 1e-2;
25
26pub(crate) const TRANSVERSE_SEED_CROSS: f64 = TRANSVERSE_START_CROSS;
31
32const MARCH_REACH_EXTENTS: f64 = 4.0;
60
61fn step_budget(maximum_steps: usize, maximum_step: f64, diagonal: f64) -> usize {
64 if !(maximum_step > 0.0) || !diagonal.is_finite() {
65 return maximum_steps;
66 }
67 let needed = (MARCH_REACH_EXTENTS * diagonal / maximum_step).ceil();
68 if !needed.is_finite() || needed <= maximum_steps as f64 {
69 return maximum_steps;
70 }
71 needed as usize
72}
73
74#[derive(Clone, Copy)]
75struct Bounds {
76 minimum: Vec3,
77 maximum: Vec3,
78}
79
80impl Bounds {
81 fn from_surface(surface: &NurbsSurface) -> Result<Self, String> {
82 let mut minimum = Vec3::new(f64::INFINITY, f64::INFINITY, f64::INFINITY);
83 let mut maximum = Vec3::new(f64::NEG_INFINITY, f64::NEG_INFINITY, f64::NEG_INFINITY);
84 for point in surface.control_points.iter().flatten() {
85 let point = point.point()?;
86 minimum.x = minimum.x.min(point.x);
87 minimum.y = minimum.y.min(point.y);
88 minimum.z = minimum.z.min(point.z);
89 maximum.x = maximum.x.max(point.x);
90 maximum.y = maximum.y.max(point.y);
91 maximum.z = maximum.z.max(point.z);
92 }
93 Ok(Self { minimum, maximum })
94 }
95
96 fn diagonal(self) -> f64 {
97 self.maximum.sub(self.minimum).length()
98 }
99
100 fn expanded(self, amount: f64) -> Self {
101 let delta = Vec3::new(amount, amount, amount);
102 Self {
103 minimum: self.minimum.sub(delta),
104 maximum: self.maximum.add(delta),
105 }
106 }
107
108 fn contains(self, point: Vec3) -> bool {
109 point.x >= self.minimum.x
110 && point.x <= self.maximum.x
111 && point.y >= self.minimum.y
112 && point.y <= self.maximum.y
113 && point.z >= self.minimum.z
114 && point.z <= self.maximum.z
115 }
116
117 fn intersects(self, other: Self, tolerance: f64) -> bool {
118 self.expanded(tolerance).minimum.x <= other.maximum.x
119 && self.maximum.x + tolerance >= other.minimum.x
120 && self.minimum.y - tolerance <= other.maximum.y
121 && self.maximum.y + tolerance >= other.minimum.y
122 && self.minimum.z - tolerance <= other.maximum.z
123 && self.maximum.z + tolerance >= other.minimum.z
124 }
125}
126
127#[derive(Clone, Copy)]
128struct SurfaceInfo<'a> {
129 surface: &'a NurbsSurface,
130 u0: f64,
131 u1: f64,
132 v0: f64,
133 v1: f64,
134 closed_u: bool,
135 closed_v: bool,
136}
137
138fn surface_info(surface: &NurbsSurface) -> Result<SurfaceInfo<'_>, String> {
139 let ku = KnotVector::new(surface.knots_u.clone(), surface.degree_u)?;
140 let kv = KnotVector::new(surface.knots_v.clone(), surface.degree_v)?;
141 let [u0, u1] = ku.domain();
142 let [v0, v1] = kv.domain();
143 let mut closed_u = true;
144 let mut closed_v = true;
145 for fraction in [0.17, 0.5, 0.83] {
146 let v = v0 + (v1 - v0) * fraction;
147 if surface
148 .evaluate(u0, v)?
149 .sub(surface.evaluate(u1, v)?)
150 .length()
151 > LINEAR_TOLERANCE * 10.0
152 {
153 closed_u = false;
154 }
155 let u = u0 + (u1 - u0) * fraction;
156 if surface
157 .evaluate(u, v0)?
158 .sub(surface.evaluate(u, v1)?)
159 .length()
160 > LINEAR_TOLERANCE * 10.0
161 {
162 closed_v = false;
163 }
164 }
165 Ok(SurfaceInfo {
166 surface,
167 u0,
168 u1,
169 v0,
170 v1,
171 closed_u,
172 closed_v,
173 })
174}
175
176fn fit(value: f64, minimum: f64, maximum: f64, closed: bool) -> f64 {
177 if closed {
178 (value - minimum).rem_euclid(maximum - minimum) + minimum
179 } else {
180 value.clamp(minimum, maximum)
181 }
182}
183
184
185#[derive(Clone, Copy)]
186struct IntersectionPoint {
187 ua: f64,
188 va: f64,
189 ub: f64,
190 vb: f64,
191 point: Vec3,
192}
193
194fn refine_point(
195 first: SurfaceInfo<'_>,
196 second: SurfaceInfo<'_>,
197 mut ua: f64,
198 mut va: f64,
199 mut ub: f64,
200 mut vb: f64,
201 tolerance: f64,
202) -> Result<Option<IntersectionPoint>, String> {
203 for _ in 0..40 {
204 let da = first.surface.derivatives(ua, va, 1)?;
205 let db = second.surface.derivatives(ub, vb, 1)?;
206 let residual = da[0][0].sub(db[0][0]);
207 if residual.length() <= tolerance * 0.01 + EPSILON {
208 return Ok(Some(IntersectionPoint {
209 ua,
210 va,
211 ub,
212 vb,
213 point: da[0][0].add(db[0][0]).scale(0.5),
214 }));
215 }
216 let columns = [
217 da[1][0],
218 da[0][1],
219 db[1][0].scale(-1.0),
220 db[0][1].scale(-1.0),
221 ];
222 let mut matrix = [[0.0f64; 3]; 3];
223 for column in columns {
224 let values = [column.x, column.y, column.z];
225 for row in 0..3 {
226 for col in 0..3 {
227 matrix[row][col] += values[row] * values[col];
228 }
229 }
230 }
231 let lambda = match solve_small(matrix, [-residual.x, -residual.y, -residual.z], 3) {
232 Ok(value) => value,
233 Err(_) => return Ok(None),
234 };
235 let multiplier = Vec3::new(lambda[0], lambda[1], lambda[2]);
236 let mut changes = columns.map(|column| column.dot(multiplier));
237 let domains = [
238 first.u1 - first.u0,
239 first.v1 - first.v0,
240 second.u1 - second.u0,
241 second.v1 - second.v0,
242 ];
243 for index in 0..4 {
244 changes[index] = changes[index].clamp(-domains[index] / 4.0, domains[index] / 4.0);
245 }
246 ua = fit(ua + changes[0], first.u0, first.u1, first.closed_u);
247 va = fit(va + changes[1], first.v0, first.v1, first.closed_v);
248 ub = fit(ub + changes[2], second.u0, second.u1, second.closed_u);
249 vb = fit(vb + changes[3], second.v0, second.v1, second.closed_v);
250 }
251 let a = first.surface.evaluate(ua, va)?;
252 let b = second.surface.evaluate(ub, vb)?;
253 if a.sub(b).length() <= tolerance {
254 Ok(Some(IntersectionPoint {
255 ua,
256 va,
257 ub,
258 vb,
259 point: a.add(b).scale(0.5),
260 }))
261 } else {
262 Ok(None)
263 }
264}
265
266fn find_start_points(
267 first: SurfaceInfo<'_>,
268 second: SurfaceInfo<'_>,
269 tolerance: f64,
270 density: f64,
271) -> Result<Vec<IntersectionPoint>, String> {
272 let grid_count = |info: SurfaceInfo<'_>, direction_u: bool| {
273 let (knots, degree) = if direction_u {
274 (&info.surface.knots_u, info.surface.degree_u)
275 } else {
276 (&info.surface.knots_v, info.surface.degree_v)
277 };
278 (8.0_f64)
279 .max(((interior_knot_count(knots, degree) + 1) * (degree + 1)) as f64 * density)
280 .ceil()
281 .min(24.0) as usize
282 };
283 let nu = grid_count(first, true);
284 let nv = grid_count(first, false);
285 let first_box = Bounds::from_surface(first.surface)?;
286 let second_box = Bounds::from_surface(second.surface)?;
287 let gate = first_box.diagonal().min(second_box.diagonal()) * 0.15 + tolerance;
288 let expanded_second = second_box.expanded((tolerance * 100.0).max(gate));
289 let mut starts = Vec::new();
290 for i in 0..=nu {
291 for j in 0..=nv {
292 let ua = first.u0 + (first.u1 - first.u0) * i as f64 / nu as f64;
293 let va = first.v0 + (first.v1 - first.v0) * j as f64 / nv as f64;
294 let point = first.surface.evaluate(ua, va)?;
295 if !expanded_second.contains(point) {
296 continue;
297 }
298 let projection = project_point_to_surface(second.surface, point)?;
299 if projection.distance > gate {
300 continue;
301 }
302 if let Some(refined) =
303 refine_point(first, second, ua, va, projection.u, projection.v, tolerance)?
304 {
305 starts.push(refined);
306 }
307 }
308 }
309 Ok(starts)
310}
311
312struct SpanLeaf {
315 bounds: Bounds,
316 centre: [f64; 2],
317}
318
319fn span_leaves(info: SurfaceInfo<'_>) -> Result<Vec<SpanLeaf>, String> {
323 let surface = info.surface;
324 let (p, q) = (surface.degree_u, surface.degree_v);
325 let rows = surface.control_points.len();
326 let columns = surface.control_points.first().map_or(0, Vec::len);
327 let spans = |knots: &[f64], degree: usize, count: usize| -> Vec<usize> {
328 (degree..count).filter(|&i| i + 1 < knots.len() && knots[i] < knots[i + 1]).collect()
329 };
330 let (spans_u, spans_v) = (spans(&surface.knots_u, p, rows), spans(&surface.knots_v, q, columns));
331 if spans_u.is_empty() || spans_v.is_empty() {
332 return Ok(vec![SpanLeaf {
333 bounds: Bounds::from_surface(surface)?,
334 centre: [0.5 * (info.u0 + info.u1), 0.5 * (info.v0 + info.v1)],
335 }]);
336 }
337 let mut leaves = Vec::with_capacity(spans_u.len() * spans_v.len());
338 for &i in &spans_u {
339 for &j in &spans_v {
340 let mut minimum = Vec3::new(f64::INFINITY, f64::INFINITY, f64::INFINITY);
341 let mut maximum = Vec3::new(f64::NEG_INFINITY, f64::NEG_INFINITY, f64::NEG_INFINITY);
342 for row in &surface.control_points[i - p..=i] {
343 for control in &row[j - q..=j] {
344 let point = control.point()?;
345 minimum.x = minimum.x.min(point.x);
346 minimum.y = minimum.y.min(point.y);
347 minimum.z = minimum.z.min(point.z);
348 maximum.x = maximum.x.max(point.x);
349 maximum.y = maximum.y.max(point.y);
350 maximum.z = maximum.z.max(point.z);
351 }
352 }
353 leaves.push(SpanLeaf {
354 bounds: Bounds { minimum, maximum },
355 centre: [
356 0.5 * (surface.knots_u[i] + surface.knots_u[i + 1]),
357 0.5 * (surface.knots_v[j] + surface.knots_v[j + 1]),
358 ],
359 });
360 }
361 }
362 Ok(leaves)
363}
364
365const TREE_SEED_PAIR_CAP: usize = 4096;
370
371fn tree_start_points(
378 first: SurfaceInfo<'_>,
379 second: SurfaceInfo<'_>,
380 tolerance: f64,
381) -> Result<Option<(Vec<IntersectionPoint>, usize)>, String> {
382 let first_box = Bounds::from_surface(first.surface)?;
383 let second_box = Bounds::from_surface(second.surface)?;
384 let gate = first_box.diagonal().min(second_box.diagonal()) * 0.15 + tolerance;
385 let grow = (tolerance * 100.0).max(gate);
386 let leaves_a = span_leaves(first)?;
387 let leaves_b = span_leaves(second)?;
388 let mut pairs = Vec::new();
389 for (ia, leaf_a) in leaves_a.iter().enumerate() {
390 for (ib, leaf_b) in leaves_b.iter().enumerate() {
391 if leaf_a.bounds.intersects(leaf_b.bounds, grow) {
392 pairs.push((ia, ib));
393 if pairs.len() > TREE_SEED_PAIR_CAP {
394 return Ok(None);
395 }
396 }
397 }
398 }
399 let mut starts = Vec::new();
400 for (ia, ib) in &pairs {
401 let (leaf_a, leaf_b) = (&leaves_a[*ia], &leaves_b[*ib]);
402 let reach = gate.max(leaf_a.bounds.diagonal().max(leaf_b.bounds.diagonal()));
408 for (from, onto, from_info, onto_info, from_first) in [
411 (leaf_a, leaf_b, first, second, true),
412 (leaf_b, leaf_a, second, first, false),
413 ] {
414 let point = from_info.surface.evaluate(from.centre[0], from.centre[1])?;
415 let projection = project_point_to_surface_from_seed(onto_info.surface, point, onto.centre)?;
416 if projection.distance > reach {
417 continue;
418 }
419 let refined = if from_first {
420 refine_point(first, second, from.centre[0], from.centre[1], projection.u, projection.v, tolerance)?
421 } else {
422 refine_point(first, second, projection.u, projection.v, from.centre[0], from.centre[1], tolerance)?
423 };
424 if let Some(refined) = refined {
425 starts.push(refined);
426 }
427 }
428 }
429 Ok(Some((starts, pairs.len())))
430}
431
432fn transverse_at(
436 first: SurfaceInfo<'_>,
437 second: SurfaceInfo<'_>,
438 point: IntersectionPoint,
439) -> bool {
440 match (
441 first.surface.normal(point.ua, point.va),
442 second.surface.normal(point.ub, point.vb),
443 ) {
444 (Ok(a), Ok(b)) => a.cross(b).length() >= TRANSVERSE_START_CROSS,
445 _ => false,
446 }
447}
448
449fn tangent_at(
450 first: SurfaceInfo<'_>,
451 second: SurfaceInfo<'_>,
452 point: IntersectionPoint,
453) -> Result<Option<Vec3>, String> {
454 let first_normal = first.surface.normal(point.ua, point.va).ok();
455 let second_normal = second.surface.normal(point.ub, point.vb).ok();
456 match (first_normal, second_normal) {
457 (Some(a), Some(b)) => {
458 let tangent = a.cross(b);
459 if tangent.length() <= 1e-9 {
460 Ok(None)
461 } else {
462 Ok(Some(tangent.normalized()?))
463 }
464 }
465 _ => Ok(None),
466 }
467}
468
469fn polish_boundary(
470 first: SurfaceInfo<'_>,
471 second: SurfaceInfo<'_>,
472 mut point: IntersectionPoint,
473 tolerance: f64,
474) -> Result<Option<IntersectionPoint>, String> {
475 let free = [
476 first.closed_u || (point.ua > first.u0 && point.ua < first.u1),
477 first.closed_v || (point.va > first.v0 && point.va < first.v1),
478 second.closed_u || (point.ub > second.u0 && point.ub < second.u1),
479 second.closed_v || (point.vb > second.v0 && point.vb < second.v1),
480 ];
481 for _ in 0..40 {
482 let da = first.surface.derivatives(point.ua, point.va, 1)?;
483 let db = second.surface.derivatives(point.ub, point.vb, 1)?;
484 let residual = da[0][0].sub(db[0][0]);
485 if residual.length() <= tolerance {
486 point.point = da[0][0].add(db[0][0]).scale(0.5);
487 return Ok(Some(point));
488 }
489 let all_columns = [
490 da[1][0],
491 da[0][1],
492 db[1][0].scale(-1.0),
493 db[0][1].scale(-1.0),
494 ];
495 let indices: Vec<usize> = (0..4).filter(|index| free[*index]).collect();
496 if indices.is_empty() {
497 return Ok(None);
498 }
499 let mut matrix = [[0.0f64; 4]; 4];
500 let mut rhs = [0.0f64; 4];
501 for (row, &r) in indices.iter().enumerate() {
502 rhs[row] = -all_columns[r].dot(residual);
503 for (column, &c) in indices.iter().enumerate() {
504 matrix[row][column] = all_columns[r].dot(all_columns[c]);
505 }
506 }
507 let changes = match solve_small(matrix, rhs, indices.len()) {
508 Ok(value) => value,
509 Err(_) => return Ok(None),
510 };
511 let mut update = [0.0; 4];
512 for (index, ¶meter_index) in indices.iter().enumerate() {
513 update[parameter_index] = changes[index];
514 }
515 point.ua = fit(point.ua + update[0], first.u0, first.u1, first.closed_u);
516 point.va = fit(point.va + update[1], first.v0, first.v1, first.closed_v);
517 point.ub = fit(point.ub + update[2], second.u0, second.u1, second.closed_u);
518 point.vb = fit(point.vb + update[3], second.v0, second.v1, second.closed_v);
519 }
520 Ok(None)
521}
522
523fn correct(
524 first: SurfaceInfo<'_>,
525 second: SurfaceInfo<'_>,
526 from: IntersectionPoint,
527 predicted: Vec3,
528 tangent: Vec3,
529 tolerance: f64,
530) -> Result<Option<(IntersectionPoint, bool)>, String> {
531 let mut point = from;
532 let mut boundary = false;
533 for _ in 0..30 {
534 let da = first.surface.derivatives(point.ua, point.va, 1)?;
535 let db = second.surface.derivatives(point.ub, point.vb, 1)?;
536 let residual = da[0][0].sub(db[0][0]);
537 let plane_residual = da[0][0].sub(predicted).dot(tangent);
538 if residual.length() <= tolerance * 0.01 + EPSILON
539 && plane_residual.abs() <= tolerance * 0.01 + EPSILON
540 {
541 point.point = da[0][0].add(db[0][0]).scale(0.5);
542 return Ok(Some((point, boundary)));
543 }
544 let matrix = [
545 [da[1][0].x, da[0][1].x, -db[1][0].x, -db[0][1].x],
546 [da[1][0].y, da[0][1].y, -db[1][0].y, -db[0][1].y],
547 [da[1][0].z, da[0][1].z, -db[1][0].z, -db[0][1].z],
548 [da[1][0].dot(tangent), da[0][1].dot(tangent), 0.0, 0.0],
549 ];
550 let changes = match solve_small(
551 matrix,
552 [-residual.x, -residual.y, -residual.z, -plane_residual],
553 4,
554 ) {
555 Ok(value) => value,
556 Err(_) => return Ok(None),
557 };
558 let scale = [
559 changes[0].abs() / ((first.u1 - first.u0) / 4.0),
560 changes[1].abs() / ((first.v1 - first.v0) / 4.0),
561 changes[2].abs() / ((second.u1 - second.u0) / 4.0),
562 changes[3].abs() / ((second.v1 - second.v0) / 4.0),
563 1.0,
564 ]
565 .into_iter()
566 .fold(1.0_f64, f64::max);
567 let raw = [
568 point.ua + changes[0] / scale,
569 point.va + changes[1] / scale,
570 point.ub + changes[2] / scale,
571 point.vb + changes[3] / scale,
572 ];
573 let domains = [
574 (first.u0, first.u1, first.closed_u),
575 (first.v0, first.v1, first.closed_v),
576 (second.u0, second.u1, second.closed_u),
577 (second.v0, second.v1, second.closed_v),
578 ];
579 boundary = false;
580 let mut next = [0.0; 4];
581 for index in 0..4 {
582 next[index] = fit(
583 raw[index],
584 domains[index].0,
585 domains[index].1,
586 domains[index].2,
587 );
588 if !domains[index].2
589 && (next[index] == domains[index].0 || next[index] == domains[index].1)
590 && raw[index] != next[index]
591 {
592 boundary = true;
593 }
594 }
595 let old = [point.ua, point.va, point.ub, point.vb];
596 let spans = [
597 first.u1 - first.u0,
598 first.v1 - first.v0,
599 second.u1 - second.u0,
600 second.v1 - second.v0,
601 ];
602 let stalled = (0..4).all(|index| (next[index] - old[index]).abs() <= 1e-14 * spans[index]);
603 point.ua = next[0];
604 point.va = next[1];
605 point.ub = next[2];
606 point.vb = next[3];
607 if stalled {
608 if boundary {
609 if let Some(polished) = polish_boundary(first, second, point, tolerance)? {
610 return Ok(Some((polished, true)));
611 }
612 }
613 return Ok(None);
614 }
615 }
616 Ok(None)
617}
618
619struct Trace {
620 points: Vec<IntersectionPoint>,
621 tangents: Vec<Vec3>,
623 closed: bool,
624 deviation: f64,
626}
627
628struct Spliced {
631 points: Vec<IntersectionPoint>,
632 tangents: Vec<Vec3>,
633 deviation: f64,
634}
635
636impl From<Trace> for Spliced {
637 fn from(trace: Trace) -> Self {
638 Self {
639 points: trace.points,
640 tangents: trace.tangents,
641 deviation: trace.deviation,
642 }
643 }
644}
645
646fn splice(backward: Trace, forward: Trace) -> Spliced {
649 let mut points: Vec<_> = backward.points.into_iter().skip(1).rev().collect();
650 let mut tangents: Vec<_> = backward
651 .tangents
652 .into_iter()
653 .skip(1)
654 .rev()
655 .map(|tangent| tangent.scale(-1.0))
656 .collect();
657 points.extend(forward.points);
658 tangents.extend(forward.tangents);
659 Spliced {
660 points,
661 tangents,
662 deviation: backward.deviation.max(forward.deviation),
663 }
664}
665
666fn curve_from(all: Spliced, closed: bool, measured: bool) -> SurfaceIntersectionCurve {
669 let complete = measured && all.tangents.len() == all.points.len();
670 SurfaceIntersectionCurve {
671 points: all.points.iter().map(|point| point.point).collect(),
672 params_a: all.points.iter().map(|point| [point.ua, point.va]).collect(),
673 params_b: all.points.iter().map(|point| [point.ub, point.vb]).collect(),
674 closed,
675 tangents: complete.then_some(all.tangents),
676 span_deviation: complete.then_some(all.deviation),
677 tangent: false,
678 }
679}
680
681fn hermite_at(p0: Vec3, d0: Vec3, p1: Vec3, d1: Vec3, s: f64) -> Vec3 {
684 let s2 = s * s;
685 let s3 = s2 * s;
686 p0.scale(2.0 * s3 - 3.0 * s2 + 1.0)
687 .add(d0.scale(s3 - 2.0 * s2 + s))
688 .add(p1.scale(-2.0 * s3 + 3.0 * s2))
689 .add(d1.scale(s3 - s2))
690}
691
692struct ParameterHermite {
698 p0: [f64; 2],
699 d0: [f64; 2],
700 p1: [f64; 2],
701 d1: [f64; 2],
702}
703
704impl ParameterHermite {
705 fn new(
706 info: SurfaceInfo<'_>,
707 p0: [f64; 2],
708 t0: Vec3,
709 p1: [f64; 2],
710 t1: Vec3,
711 chord: f64,
712 ) -> Result<Self, String> {
713 let image = |uv: [f64; 2], tangent: Vec3| -> Result<[f64; 2], String> {
714 let d = info.surface.derivatives(uv[0], uv[1], 1)?;
715 let (su, sv) = (d[1][0], d[0][1]);
716 let (e, f, g) = (su.dot(su), su.dot(sv), sv.dot(sv));
717 let (bu, bv) = (su.dot(tangent), sv.dot(tangent));
718 let det = e * g - f * f;
719 if det.abs() <= EPSILON * (1.0 + e * g) {
720 return Ok([0.0, 0.0]);
721 }
722 Ok([
723 (g * bu - f * bv) / det * chord,
724 (e * bv - f * bu) / det * chord,
725 ])
726 };
727 let mut p1 = p1;
728 let unwrap = |value: f64, near: f64, closed: bool, minimum: f64, maximum: f64| {
729 let period = maximum - minimum;
730 if closed && period > 0.0 && (value - near).abs() > 0.5 * period {
731 value - period * ((value - near) / period).round()
732 } else {
733 value
734 }
735 };
736 p1[0] = unwrap(p1[0], p0[0], info.closed_u, info.u0, info.u1);
737 p1[1] = unwrap(p1[1], p0[1], info.closed_v, info.v0, info.v1);
738 Ok(Self {
739 p0,
740 d0: image(p0, t0)?,
741 p1,
742 d1: image([fit(p1[0], info.u0, info.u1, info.closed_u), fit(p1[1], info.v0, info.v1, info.closed_v)], t1)?,
743 })
744 }
745
746 fn at(&self, s: f64) -> [f64; 2] {
747 let s2 = s * s;
748 let s3 = s2 * s;
749 let (h00, h10, h01, h11) = (
750 2.0 * s3 - 3.0 * s2 + 1.0,
751 s3 - 2.0 * s2 + s,
752 -2.0 * s3 + 3.0 * s2,
753 s3 - s2,
754 );
755 [
756 h00 * self.p0[0] + h10 * self.d0[0] + h01 * self.p1[0] + h11 * self.d1[0],
757 h00 * self.p0[1] + h10 * self.d0[1] + h01 * self.p1[1] + h11 * self.d1[1],
758 ]
759 }
760}
761
762fn span_deviation(
766 first: SurfaceInfo<'_>,
767 second: SurfaceInfo<'_>,
768 from: IntersectionPoint,
769 from_tangent: Vec3,
770 to: IntersectionPoint,
771 to_tangent: Vec3,
772) -> Result<f64, String> {
773 let chord = to.point.sub(from.point).length();
774 if chord <= EPSILON {
775 return Ok(0.0);
776 }
777 let on_a = ParameterHermite::new(first, [from.ua, from.va], from_tangent, [to.ua, to.va], to_tangent, chord)?;
778 let on_b = ParameterHermite::new(second, [from.ub, from.vb], from_tangent, [to.ub, to.vb], to_tangent, chord)?;
779 let (d0, d1) = (from_tangent.scale(chord), to_tangent.scale(chord));
780 let mut worst: f64 = 0.0;
781 for s in [0.17, 0.33, 0.5, 0.67, 0.83] {
782 let c = hermite_at(from.point, d0, to.point, d1, s);
783 let [ua, va] = on_a.at(s);
784 let a = first.surface.evaluate(fit(ua, first.u0, first.u1, first.closed_u), fit(va, first.v0, first.v1, first.closed_v))?;
785 let [ub, vb] = on_b.at(s);
786 let b = second.surface.evaluate(fit(ub, second.u0, second.u1, second.closed_u), fit(vb, second.v0, second.v1, second.closed_v))?;
787 worst = worst
788 .max(a.sub(b).length())
789 .max(a.sub(c).length())
790 .max(b.sub(c).length());
791 }
792 Ok(worst)
793}
794
795fn vector_angle(a: Vec3, b: Vec3) -> f64 {
796 (a.dot(b) / (a.length() * b.length()))
797 .clamp(-1.0, 1.0)
798 .acos()
799}
800
801fn normal_turn_step(
810 first: SurfaceInfo<'_>,
811 second: SurfaceInfo<'_>,
812 point: IntersectionPoint,
813 tangent: Vec3,
814 budget: f64,
815) -> Option<f64> {
816 let mut best: Option<f64> = None;
817 for (surface, u, v) in [
818 (first.surface, point.ua, point.va),
819 (second.surface, point.ub, point.vb),
820 ] {
821 let derivatives = surface.derivatives(u, v, 2).ok()?;
822 let normal = derivatives[1][0].cross(derivatives[0][1]);
823 let normal_length = normal.length();
824 if normal_length <= 1e-12 {
825 continue;
826 }
827 let unit_normal = normal.scale(1.0 / normal_length);
828 for (first_derivative, second_derivative) in [
829 (derivatives[1][0], derivatives[2][0]),
830 (derivatives[0][1], derivatives[0][2]),
831 ] {
832 let metric = first_derivative.dot(first_derivative);
833 if metric <= 1e-16 {
834 continue;
835 }
836 let curvature = second_derivative.dot(unit_normal).abs();
837 if curvature <= 1e-12 {
838 continue; }
840 let advance = budget * metric / curvature;
841 let alignment = first_derivative
842 .scale(1.0 / metric.sqrt())
843 .dot(tangent)
844 .abs();
845 if alignment <= 0.1 {
846 continue; }
848 let along = advance * alignment;
849 if best.map(|value| along < value).unwrap_or(true) {
850 best = Some(along);
851 }
852 }
853 }
854 best
855}
856
857#[derive(Clone, Copy)]
860struct TraceSwitches {
861 first_step_ceiling: bool,
864 debug: bool,
866}
867
868impl TraceSwitches {
869 fn from_env() -> Self {
870 Self {
871 first_step_ceiling: std::env::var("BREP_SSI_FIRST_STEP_BUDGET").as_deref() != Ok("0"),
872 debug: std::env::var_os("BREP_DEBUG_SSI_TRACE").is_some(),
873 }
874 }
875}
876
877#[allow(clippy::too_many_arguments)]
878fn trace(
879 first: SurfaceInfo<'_>,
880 second: SurfaceInfo<'_>,
881 start: IntersectionPoint,
882 direction: f64,
883 tolerance: f64,
884 initial_step: f64,
885 minimum_step: f64,
886 maximum_step: f64,
887 maximum_steps: usize,
888 switches: TraceSwitches,
889 span_budget: Option<f64>,
890) -> Result<Trace, String> {
891 let mut points = vec![start];
892 let mut current = start;
893 let Some(mut tangent) = tangent_at(first, second, start)? else {
894 return Ok(Trace {
895 points,
896 tangents: Vec::new(),
897 closed: false,
898 deviation: 0.0,
899 });
900 };
901 tangent = tangent.scale(direction);
902 let mut tangents = vec![tangent];
903 let mut deviation: f64 = 0.0;
904 let mut step_size = initial_step.min(maximum_step);
915 if switches.first_step_ceiling {
916 if let Some(budget) = normal_turn_step(first, second, start, tangent, 0.1) {
917 step_size = step_size.min(budget).clamp(minimum_step, maximum_step);
918 }
919 }
920 let mut terminated = false;
921 for _ in 0..maximum_steps {
922 let mut accepted = None;
923 while step_size >= minimum_step {
924 let predicted = current.point.add(tangent.scale(step_size));
925 if let Some((corrected, boundary)) =
926 correct(first, second, current, predicted, tangent, tolerance)?
927 {
928 if corrected.point.sub(current.point).length() <= 2.5 * step_size {
929 accepted = Some((corrected, boundary));
930 break;
931 }
932 }
933 step_size /= 2.0;
934 }
935 let Some((candidate, boundary)) = accepted else {
936 terminated = true;
937 break;
938 };
939 points.push(candidate);
940 if switches.debug {
941 let turn = tangent_at(first, second, candidate)
945 .ok()
946 .flatten()
947 .map(|next| vector_angle(tangent, if next.dot(tangent) < 0.0 { next.scale(-1.0) } else { next }))
948 .unwrap_or(f64::NAN);
949 let budget = normal_turn_step(first, second, current, tangent, 0.1).unwrap_or(f64::NAN);
950 let at = candidate.point;
951 eprintln!(
952 "SSI-TRACE dir {direction} step {step_size:.4e} jump {:.4e} turn {turn:.4} ceiling {budget:.4e} boundary {boundary} p ({:.6},{:.6},{:.6})",
953 at.sub(current.point).length(),
954 at.x,
955 at.y,
956 at.z
957 );
958 }
959 let next = tangent_at(first, second, candidate)?.map(|next| {
963 if next.dot(tangent) < 0.0 { next.scale(-1.0) } else { next }
964 });
965 if boundary {
966 tangents.push(next.unwrap_or(tangent));
967 terminated = true;
968 break;
969 }
970 let Some(next_tangent) = next else {
971 tangents.push(tangent);
972 terminated = true;
973 break;
974 };
975 let angle = vector_angle(tangent, next_tangent);
976 if angle > 0.35 {
977 points.pop();
978 step_size = minimum_step.max(step_size / 2.0);
979 if step_size <= minimum_step * 1.01 {
980 points.push(candidate);
981 } else {
982 continue;
983 }
984 }
985 let mut span_fine = true;
990 if let Some(budget) = span_budget {
991 let measured = span_deviation(first, second, current, tangent, candidate, next_tangent)?;
992 if measured > budget && step_size > minimum_step * 1.01 {
993 points.pop();
994 step_size = minimum_step.max(step_size / 2.0);
995 continue;
996 }
997 deviation = deviation.max(measured);
998 span_fine = measured < budget / 8.0;
999 }
1000 tangents.push(next_tangent);
1001 let reactive = if angle < 0.03 && span_fine {
1006 step_size * 1.5
1007 } else if angle > 0.15 {
1008 step_size * 0.6
1009 } else {
1010 step_size
1011 };
1012 step_size = match normal_turn_step(first, second, candidate, next_tangent, 0.1) {
1013 Some(budget) => reactive.min(budget),
1014 None => reactive,
1015 }
1016 .clamp(minimum_step, maximum_step);
1017 if points.len() > 4 && candidate.point.sub(start.point).length() <= step_size * 1.2 {
1018 let to_start = start.point.sub(candidate.point);
1019 if to_start.length() <= EPSILON || to_start.dot(next_tangent) >= 0.0 {
1020 points.push(start);
1021 tangents.push(tangents[0]);
1022 return Ok(Trace {
1023 points,
1024 tangents,
1025 closed: true,
1026 deviation,
1027 });
1028 }
1029 }
1030 current = candidate;
1031 tangent = next_tangent;
1032 }
1033 if !terminated {
1034 return Err(format!(
1038 "surface intersection trace exhausted {maximum_steps} steps \
1039 ({} points, step {step_size:.3e}) without reaching a boundary or closure",
1040 points.len()
1041 ));
1042 }
1043 Ok(Trace {
1044 points,
1045 tangents,
1046 closed: false,
1047 deviation,
1048 })
1049}
1050
1051fn point_segment_distance(point: Vec3, start: Vec3, end: Vec3) -> f64 {
1052 let segment = end.sub(start);
1053 let length_squared = segment.length_squared();
1054 if length_squared <= EPSILON {
1055 return point.sub(start).length();
1056 }
1057 let parameter = point.sub(start).dot(segment) / length_squared;
1058 let parameter = parameter.clamp(0.0, 1.0);
1059 point.sub(start.add(segment.scale(parameter))).length()
1060}
1061
1062#[derive(Clone, Debug, Serialize)]
1063pub struct SurfaceIntersectionCurve {
1064 pub points: Vec<Vec3>,
1065 pub params_a: Vec<[f64; 2]>,
1066 pub params_b: Vec<[f64; 2]>,
1067 pub closed: bool,
1068 #[serde(skip_serializing_if = "Option::is_none")]
1072 pub tangents: Option<Vec<Vec3>>,
1073 #[serde(skip_serializing_if = "Option::is_none")]
1076 pub span_deviation: Option<f64>,
1077 #[serde(skip_serializing_if = "std::ops::Not::not")]
1082 pub tangent: bool,
1083}
1084
1085#[derive(Clone, Debug, Deserialize)]
1086pub struct SurfaceIntersectionOptions {
1087 #[serde(default = "default_tolerance")]
1088 pub tolerance: f64,
1089 #[serde(default = "default_density")]
1090 pub seed_density: f64,
1091 #[serde(default = "default_maximum_steps")]
1092 pub maximum_steps: usize,
1093 pub maximum_step: Option<f64>,
1094 #[serde(default)]
1095 pub seed_points: Vec<Vec3>,
1096 #[serde(default)]
1097 pub seed_only: bool,
1098 pub seed_gate: Option<f64>,
1099 #[serde(default)]
1116 pub span_budget: Option<f64>,
1117 #[serde(default)]
1119 pub ordered_starts: bool,
1120 #[serde(default)]
1126 pub consume_starts: bool,
1127 #[serde(default)]
1129 pub tree_seeds: bool,
1130}
1131
1132fn default_tolerance() -> f64 {
1133 LINEAR_TOLERANCE
1134}
1135fn default_density() -> f64 {
1136 1.0
1137}
1138fn default_maximum_steps() -> usize {
1139 4000
1140}
1141
1142impl Default for SurfaceIntersectionOptions {
1143 fn default() -> Self {
1144 Self {
1145 tolerance: default_tolerance(),
1146 seed_density: default_density(),
1147 maximum_steps: default_maximum_steps(),
1148 maximum_step: None,
1149 seed_points: Vec::new(),
1150 seed_only: false,
1151 seed_gate: None,
1152 span_budget: None,
1153 ordered_starts: false,
1154 consume_starts: false,
1155 tree_seeds: false,
1156 }
1157 }
1158}
1159
1160pub(crate) fn refine_to_intersection(
1169 first_surface: &NurbsSurface,
1170 second_surface: &NurbsSurface,
1171 seed: Vec3,
1172 tolerance: f64,
1173 reach: f64,
1174) -> Result<Option<Vec3>, String> {
1175 let first = surface_info(first_surface)?;
1176 let second = surface_info(second_surface)?;
1177 let on_first = project_point_to_surface(first_surface, seed)?;
1178 let on_second = project_point_to_surface(second_surface, seed)?;
1179 let Some(refined) = refine_point(
1180 first,
1181 second,
1182 on_first.u,
1183 on_first.v,
1184 on_second.u,
1185 on_second.v,
1186 tolerance,
1187 )?
1188 else {
1189 return Ok(None);
1190 };
1191 if refined.point.sub(seed).length() > reach {
1192 return Ok(None);
1193 }
1194 Ok(Some(refined.point))
1195}
1196
1197pub fn intersect_surfaces(
1198 first_surface: &NurbsSurface,
1199 second_surface: &NurbsSurface,
1200 options: &SurfaceIntersectionOptions,
1201) -> Result<Vec<SurfaceIntersectionCurve>, String> {
1202 let first_box = Bounds::from_surface(first_surface)?;
1203 let second_box = Bounds::from_surface(second_surface)?;
1204 if !first_box.intersects(second_box, options.tolerance * 10.0) {
1205 return Ok(Vec::new());
1206 }
1207 let first = surface_info(first_surface)?;
1208 let second = surface_info(second_surface)?;
1209 let diagonal = first_box.diagonal().min(second_box.diagonal());
1210 let initial_step = diagonal / 100.0;
1211 let minimum_step = (options.tolerance * 100.0).max(diagonal * 1e-6);
1212 let maximum_step = (diagonal / 15.0).min(options.maximum_step.unwrap_or(f64::INFINITY));
1213 let maximum_steps = step_budget(options.maximum_steps, maximum_step, diagonal);
1214 let mut starts = if options.seed_only {
1215 Vec::new()
1216 } else if options.tree_seeds {
1217 match tree_start_points(first, second, options.tolerance)? {
1220 Some((starts, _pairs)) => starts,
1221 None => find_start_points(first, second, options.tolerance, options.seed_density)?,
1222 }
1223 } else {
1224 find_start_points(first, second, options.tolerance, options.seed_density)?
1225 };
1226 starts.retain(|start| transverse_at(first, second, *start));
1231 let seed_gate = options.seed_gate.unwrap_or(options.tolerance * 100.0);
1232 let grid_count = starts.len();
1234 for seed in &options.seed_points {
1235 let projection_a = project_point_to_surface(first_surface, *seed)?;
1236 if projection_a.distance > seed_gate {
1237 continue;
1238 }
1239 let projection_b = project_point_to_surface(second_surface, *seed)?;
1240 if projection_b.distance > seed_gate {
1241 continue;
1242 }
1243 let Some(refined) = refine_point(
1244 first,
1245 second,
1246 projection_a.u,
1247 projection_a.v,
1248 projection_b.u,
1249 projection_b.v,
1250 options.tolerance,
1251 )?
1252 else {
1253 continue;
1254 };
1255 if !transverse_at(first, second, refined) {
1256 continue;
1257 }
1258 starts.push(refined);
1259 }
1260 if options.ordered_starts {
1261 let crossing = |start: &IntersectionPoint| -> f64 {
1265 match (
1266 first.surface.normal(start.ua, start.va),
1267 second.surface.normal(start.ub, start.vb),
1268 ) {
1269 (Ok(a), Ok(b)) => a.cross(b).length(),
1270 _ => 0.0,
1271 }
1272 };
1273 let mut seeds = starts.split_off(grid_count);
1274 seeds.sort_by(|a, b| crossing(b).total_cmp(&crossing(a)));
1275 starts.sort_by(|a, b| crossing(b).total_cmp(&crossing(a)));
1276 seeds.append(&mut starts);
1277 starts = seeds;
1278 }
1279
1280 let switches = TraceSwitches::from_env();
1281 let mut curves: Vec<SurfaceIntersectionCurve> = Vec::new();
1282 for start in starts {
1283 let mut claimed = false;
1284 for curve in &mut curves {
1285 let Some((index, distance)) = curve
1286 .points
1287 .windows(2)
1288 .enumerate()
1289 .map(|(index, segment)| (index, point_segment_distance(start.point, segment[0], segment[1])))
1290 .filter(|(_, distance)| *distance <= initial_step * 1.5)
1291 .min_by(|a, b| a.1.total_cmp(&b.1))
1292 else {
1293 continue;
1294 };
1295 claimed = true;
1296 if options.consume_starts
1301 && distance > 0.0
1302 && start.point.sub(curve.points[index]).length() > minimum_step
1303 && start.point.sub(curve.points[index + 1]).length() > minimum_step
1304 {
1305 curve.points.insert(index + 1, start.point);
1306 if let Some(tangents) = curve.tangents.as_mut() {
1307 let along = curve.points[index + 2].sub(curve.points[index]);
1308 let tangent = match tangent_at(first, second, start)? {
1309 Some(tangent) if tangent.dot(along) < 0.0 => tangent.scale(-1.0),
1310 Some(tangent) => tangent,
1311 None => tangents[index],
1312 };
1313 tangents.insert(index + 1, tangent);
1314 }
1315 if curve.params_a.len() > index + 1 && curve.params_b.len() > index + 1 {
1316 curve.params_a.insert(index + 1, [start.ua, start.va]);
1317 curve.params_b.insert(index + 1, [start.ub, start.vb]);
1318 }
1319 }
1320 break;
1321 }
1322 if switches.debug {
1323 let at = start.point;
1324 eprintln!(
1325 "SSI-START claimed {claimed} p ({:.6},{:.6},{:.6}) initial {initial_step:.4e} minimum {minimum_step:.4e} maximum {maximum_step:.4e}",
1326 at.x, at.y, at.z
1327 );
1328 }
1329 if claimed {
1330 continue;
1331 }
1332 let forward = trace(
1333 first,
1334 second,
1335 start,
1336 1.0,
1337 options.tolerance,
1338 initial_step,
1339 minimum_step,
1340 maximum_step,
1341 maximum_steps,
1342 switches,
1343 options.span_budget,
1344 )?;
1345 let (all, closed) = if forward.closed {
1346 (Spliced::from(forward), true)
1347 } else {
1348 let backward = trace(
1349 first,
1350 second,
1351 start,
1352 -1.0,
1353 options.tolerance,
1354 initial_step,
1355 minimum_step,
1356 maximum_step,
1357 maximum_steps,
1358 switches,
1359 options.span_budget,
1360 )?;
1361 (splice(backward, forward), false)
1362 };
1363 if all.points.len() < 2 {
1364 continue;
1365 }
1366 curves.push(curve_from(all, closed, options.span_budget.is_some()));
1367 }
1368 Ok(curves)
1369}
1370
1371const SUPPLEMENTAL_TRANSVERSE_CROSS: f64 = TRANSVERSE_START_CROSS;
1375
1376pub fn intersect_surfaces_supplemental(
1403 first_surface: &NurbsSurface,
1404 second_surface: &NurbsSurface,
1405 options: &SurfaceIntersectionOptions,
1406) -> Result<Vec<SurfaceIntersectionCurve>, String> {
1407 let first_box = Bounds::from_surface(first_surface)?;
1408 let second_box = Bounds::from_surface(second_surface)?;
1409 if !first_box.intersects(second_box, options.tolerance * 10.0) {
1410 return Ok(Vec::new());
1411 }
1412 let first = surface_info(first_surface)?;
1413 let second = surface_info(second_surface)?;
1414 let diagonal = first_box.diagonal().min(second_box.diagonal());
1415 let initial_step = diagonal / 100.0;
1416 let minimum_step = (options.tolerance * 100.0).max(diagonal * 1e-6);
1417 let maximum_step = (diagonal / 15.0).min(options.maximum_step.unwrap_or(f64::INFINITY));
1418 let maximum_steps = step_budget(options.maximum_steps, maximum_step, diagonal);
1419
1420 let density = options.seed_density.max(2.0);
1425 let mut starts: Vec<IntersectionPoint> = Vec::new();
1426 for gridded_second in [false, true] {
1427 let (a, b) = if gridded_second {
1428 (second, first)
1429 } else {
1430 (first, second)
1431 };
1432 for start in find_start_points(a, b, options.tolerance, density)? {
1433 let start = if gridded_second {
1436 IntersectionPoint {
1437 ua: start.ub,
1438 va: start.vb,
1439 ub: start.ua,
1440 vb: start.va,
1441 point: start.point,
1442 }
1443 } else {
1444 start
1445 };
1446 starts.push(start);
1447 }
1448 }
1449 starts.retain(|start| {
1453 match (
1454 first.surface.normal(start.ua, start.va),
1455 second.surface.normal(start.ub, start.vb),
1456 ) {
1457 (Ok(na), Ok(nb)) => na.cross(nb).length() > SUPPLEMENTAL_TRANSVERSE_CROSS,
1458 _ => false,
1459 }
1460 });
1461
1462 let switches = TraceSwitches::from_env();
1463 let mut curves: Vec<SurfaceIntersectionCurve> = Vec::new();
1464 for start in starts {
1465 let claimed = curves.iter().any(|curve| {
1466 curve.points.windows(2).any(|segment| {
1467 point_segment_distance(start.point, segment[0], segment[1]) <= initial_step * 1.5
1468 })
1469 });
1470 if claimed {
1471 continue;
1472 }
1473 let Ok(forward) = trace(
1477 first,
1478 second,
1479 start,
1480 1.0,
1481 options.tolerance,
1482 initial_step,
1483 minimum_step,
1484 maximum_step,
1485 maximum_steps,
1486 switches,
1487 options.span_budget,
1488 ) else {
1489 continue;
1490 };
1491 let (all, closed) = if forward.closed {
1492 (Spliced::from(forward), true)
1493 } else {
1494 let Ok(backward) = trace(
1495 first,
1496 second,
1497 start,
1498 -1.0,
1499 options.tolerance,
1500 initial_step,
1501 minimum_step,
1502 maximum_step,
1503 maximum_steps,
1504 switches,
1505 options.span_budget,
1506 ) else {
1507 continue;
1508 };
1509 (splice(backward, forward), false)
1510 };
1511 if all.points.len() < 2 {
1512 continue;
1513 }
1514 let transverse = all.points.iter().step_by((all.points.len() / 5).max(1)).any(|point| {
1518 match (
1519 first.surface.normal(point.ua, point.va),
1520 second.surface.normal(point.ub, point.vb),
1521 ) {
1522 (Ok(na), Ok(nb)) => na.cross(nb).length() > SUPPLEMENTAL_TRANSVERSE_CROSS,
1523 _ => false,
1524 }
1525 });
1526 if !transverse {
1527 continue;
1528 }
1529 curves.push(curve_from(all, closed, options.span_budget.is_some()));
1530 }
1531 Ok(curves)
1532}
1533