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 crossing_at(first, second, point) >= TRANSVERSE_START_CROSS
441}
442
443fn crossing_at(
445 first: SurfaceInfo<'_>,
446 second: SurfaceInfo<'_>,
447 point: IntersectionPoint,
448) -> f64 {
449 match (
450 first.surface.normal(point.ua, point.va),
451 second.surface.normal(point.ub, point.vb),
452 ) {
453 (Ok(a), Ok(b)) => a.cross(b).length(),
454 _ => 0.0,
455 }
456}
457
458fn tangent_at(
459 first: SurfaceInfo<'_>,
460 second: SurfaceInfo<'_>,
461 point: IntersectionPoint,
462) -> Result<Option<Vec3>, String> {
463 let first_normal = first.surface.normal(point.ua, point.va).ok();
464 let second_normal = second.surface.normal(point.ub, point.vb).ok();
465 match (first_normal, second_normal) {
466 (Some(a), Some(b)) => {
467 let tangent = a.cross(b);
468 if tangent.length() <= 1e-9 {
469 Ok(None)
470 } else {
471 Ok(Some(tangent.normalized()?))
472 }
473 }
474 _ => Ok(None),
475 }
476}
477
478fn polish_boundary(
479 first: SurfaceInfo<'_>,
480 second: SurfaceInfo<'_>,
481 mut point: IntersectionPoint,
482 tolerance: f64,
483) -> Result<Option<IntersectionPoint>, String> {
484 let free = [
485 first.closed_u || (point.ua > first.u0 && point.ua < first.u1),
486 first.closed_v || (point.va > first.v0 && point.va < first.v1),
487 second.closed_u || (point.ub > second.u0 && point.ub < second.u1),
488 second.closed_v || (point.vb > second.v0 && point.vb < second.v1),
489 ];
490 for _ in 0..40 {
491 let da = first.surface.derivatives(point.ua, point.va, 1)?;
492 let db = second.surface.derivatives(point.ub, point.vb, 1)?;
493 let residual = da[0][0].sub(db[0][0]);
494 if residual.length() <= tolerance {
495 point.point = da[0][0].add(db[0][0]).scale(0.5);
496 return Ok(Some(point));
497 }
498 let all_columns = [
499 da[1][0],
500 da[0][1],
501 db[1][0].scale(-1.0),
502 db[0][1].scale(-1.0),
503 ];
504 let indices: Vec<usize> = (0..4).filter(|index| free[*index]).collect();
505 if indices.is_empty() {
506 return Ok(None);
507 }
508 let mut matrix = [[0.0f64; 4]; 4];
509 let mut rhs = [0.0f64; 4];
510 for (row, &r) in indices.iter().enumerate() {
511 rhs[row] = -all_columns[r].dot(residual);
512 for (column, &c) in indices.iter().enumerate() {
513 matrix[row][column] = all_columns[r].dot(all_columns[c]);
514 }
515 }
516 let changes = match solve_small(matrix, rhs, indices.len()) {
517 Ok(value) => value,
518 Err(_) => return Ok(None),
519 };
520 let mut update = [0.0; 4];
521 for (index, ¶meter_index) in indices.iter().enumerate() {
522 update[parameter_index] = changes[index];
523 }
524 point.ua = fit(point.ua + update[0], first.u0, first.u1, first.closed_u);
525 point.va = fit(point.va + update[1], first.v0, first.v1, first.closed_v);
526 point.ub = fit(point.ub + update[2], second.u0, second.u1, second.closed_u);
527 point.vb = fit(point.vb + update[3], second.v0, second.v1, second.closed_v);
528 }
529 Ok(None)
530}
531
532fn correct(
533 first: SurfaceInfo<'_>,
534 second: SurfaceInfo<'_>,
535 from: IntersectionPoint,
536 predicted: Vec3,
537 tangent: Vec3,
538 tolerance: f64,
539) -> Result<Option<(IntersectionPoint, bool)>, String> {
540 let mut point = from;
541 let mut boundary = false;
542 for _ in 0..30 {
543 let da = first.surface.derivatives(point.ua, point.va, 1)?;
544 let db = second.surface.derivatives(point.ub, point.vb, 1)?;
545 let residual = da[0][0].sub(db[0][0]);
546 let plane_residual = da[0][0].sub(predicted).dot(tangent);
547 if residual.length() <= tolerance * 0.01 + EPSILON
548 && plane_residual.abs() <= tolerance * 0.01 + EPSILON
549 {
550 point.point = da[0][0].add(db[0][0]).scale(0.5);
551 return Ok(Some((point, boundary)));
552 }
553 let matrix = [
554 [da[1][0].x, da[0][1].x, -db[1][0].x, -db[0][1].x],
555 [da[1][0].y, da[0][1].y, -db[1][0].y, -db[0][1].y],
556 [da[1][0].z, da[0][1].z, -db[1][0].z, -db[0][1].z],
557 [da[1][0].dot(tangent), da[0][1].dot(tangent), 0.0, 0.0],
558 ];
559 let changes = match solve_small(
560 matrix,
561 [-residual.x, -residual.y, -residual.z, -plane_residual],
562 4,
563 ) {
564 Ok(value) => value,
565 Err(_) => return Ok(None),
566 };
567 let scale = [
568 changes[0].abs() / ((first.u1 - first.u0) / 4.0),
569 changes[1].abs() / ((first.v1 - first.v0) / 4.0),
570 changes[2].abs() / ((second.u1 - second.u0) / 4.0),
571 changes[3].abs() / ((second.v1 - second.v0) / 4.0),
572 1.0,
573 ]
574 .into_iter()
575 .fold(1.0_f64, f64::max);
576 let raw = [
577 point.ua + changes[0] / scale,
578 point.va + changes[1] / scale,
579 point.ub + changes[2] / scale,
580 point.vb + changes[3] / scale,
581 ];
582 let domains = [
583 (first.u0, first.u1, first.closed_u),
584 (first.v0, first.v1, first.closed_v),
585 (second.u0, second.u1, second.closed_u),
586 (second.v0, second.v1, second.closed_v),
587 ];
588 boundary = false;
589 let mut next = [0.0; 4];
590 for index in 0..4 {
591 next[index] = fit(
592 raw[index],
593 domains[index].0,
594 domains[index].1,
595 domains[index].2,
596 );
597 if !domains[index].2
598 && (next[index] == domains[index].0 || next[index] == domains[index].1)
599 && raw[index] != next[index]
600 {
601 boundary = true;
602 }
603 }
604 let old = [point.ua, point.va, point.ub, point.vb];
605 let spans = [
606 first.u1 - first.u0,
607 first.v1 - first.v0,
608 second.u1 - second.u0,
609 second.v1 - second.v0,
610 ];
611 let stalled = (0..4).all(|index| (next[index] - old[index]).abs() <= 1e-14 * spans[index]);
612 point.ua = next[0];
613 point.va = next[1];
614 point.ub = next[2];
615 point.vb = next[3];
616 if stalled {
617 if boundary {
618 if let Some(polished) = polish_boundary(first, second, point, tolerance)? {
619 return Ok(Some((polished, true)));
620 }
621 }
622 return Ok(None);
623 }
624 }
625 Ok(None)
626}
627
628struct Trace {
629 points: Vec<IntersectionPoint>,
630 tangents: Vec<Vec3>,
632 closed: bool,
633 deviation: f64,
635}
636
637struct Spliced {
640 points: Vec<IntersectionPoint>,
641 tangents: Vec<Vec3>,
642 deviation: f64,
643}
644
645impl From<Trace> for Spliced {
646 fn from(trace: Trace) -> Self {
647 Self {
648 points: trace.points,
649 tangents: trace.tangents,
650 deviation: trace.deviation,
651 }
652 }
653}
654
655fn splice(backward: Trace, forward: Trace) -> Spliced {
658 let mut points: Vec<_> = backward.points.into_iter().skip(1).rev().collect();
659 let mut tangents: Vec<_> = backward
660 .tangents
661 .into_iter()
662 .skip(1)
663 .rev()
664 .map(|tangent| tangent.scale(-1.0))
665 .collect();
666 points.extend(forward.points);
667 tangents.extend(forward.tangents);
668 Spliced {
669 points,
670 tangents,
671 deviation: backward.deviation.max(forward.deviation),
672 }
673}
674
675fn curve_from(all: Spliced, closed: bool, measured: bool) -> SurfaceIntersectionCurve {
678 let complete = measured && all.tangents.len() == all.points.len();
679 SurfaceIntersectionCurve {
680 points: all.points.iter().map(|point| point.point).collect(),
681 params_a: all.points.iter().map(|point| [point.ua, point.va]).collect(),
682 params_b: all.points.iter().map(|point| [point.ub, point.vb]).collect(),
683 closed,
684 tangents: complete.then_some(all.tangents),
685 span_deviation: complete.then_some(all.deviation),
686 tangent: false,
687 }
688}
689
690fn hermite_at(p0: Vec3, d0: Vec3, p1: Vec3, d1: Vec3, s: f64) -> Vec3 {
693 let s2 = s * s;
694 let s3 = s2 * s;
695 p0.scale(2.0 * s3 - 3.0 * s2 + 1.0)
696 .add(d0.scale(s3 - 2.0 * s2 + s))
697 .add(p1.scale(-2.0 * s3 + 3.0 * s2))
698 .add(d1.scale(s3 - s2))
699}
700
701struct ParameterHermite {
707 p0: [f64; 2],
708 d0: [f64; 2],
709 p1: [f64; 2],
710 d1: [f64; 2],
711}
712
713impl ParameterHermite {
714 fn new(
715 info: SurfaceInfo<'_>,
716 p0: [f64; 2],
717 t0: Vec3,
718 p1: [f64; 2],
719 t1: Vec3,
720 chord: f64,
721 ) -> Result<Self, String> {
722 let image = |uv: [f64; 2], tangent: Vec3| -> Result<[f64; 2], String> {
723 let d = info.surface.derivatives(uv[0], uv[1], 1)?;
724 let (su, sv) = (d[1][0], d[0][1]);
725 let (e, f, g) = (su.dot(su), su.dot(sv), sv.dot(sv));
726 let (bu, bv) = (su.dot(tangent), sv.dot(tangent));
727 let det = e * g - f * f;
728 if det.abs() <= EPSILON * (1.0 + e * g) {
729 return Ok([0.0, 0.0]);
730 }
731 Ok([
732 (g * bu - f * bv) / det * chord,
733 (e * bv - f * bu) / det * chord,
734 ])
735 };
736 let mut p1 = p1;
737 let unwrap = |value: f64, near: f64, closed: bool, minimum: f64, maximum: f64| {
738 let period = maximum - minimum;
739 if closed && period > 0.0 && (value - near).abs() > 0.5 * period {
740 value - period * ((value - near) / period).round()
741 } else {
742 value
743 }
744 };
745 p1[0] = unwrap(p1[0], p0[0], info.closed_u, info.u0, info.u1);
746 p1[1] = unwrap(p1[1], p0[1], info.closed_v, info.v0, info.v1);
747 Ok(Self {
748 p0,
749 d0: image(p0, t0)?,
750 p1,
751 d1: image([fit(p1[0], info.u0, info.u1, info.closed_u), fit(p1[1], info.v0, info.v1, info.closed_v)], t1)?,
752 })
753 }
754
755 fn at(&self, s: f64) -> [f64; 2] {
756 let s2 = s * s;
757 let s3 = s2 * s;
758 let (h00, h10, h01, h11) = (
759 2.0 * s3 - 3.0 * s2 + 1.0,
760 s3 - 2.0 * s2 + s,
761 -2.0 * s3 + 3.0 * s2,
762 s3 - s2,
763 );
764 [
765 h00 * self.p0[0] + h10 * self.d0[0] + h01 * self.p1[0] + h11 * self.d1[0],
766 h00 * self.p0[1] + h10 * self.d0[1] + h01 * self.p1[1] + h11 * self.d1[1],
767 ]
768 }
769}
770
771fn span_deviation(
775 first: SurfaceInfo<'_>,
776 second: SurfaceInfo<'_>,
777 from: IntersectionPoint,
778 from_tangent: Vec3,
779 to: IntersectionPoint,
780 to_tangent: Vec3,
781) -> Result<f64, String> {
782 let chord = to.point.sub(from.point).length();
783 if chord <= EPSILON {
784 return Ok(0.0);
785 }
786 let on_a = ParameterHermite::new(first, [from.ua, from.va], from_tangent, [to.ua, to.va], to_tangent, chord)?;
787 let on_b = ParameterHermite::new(second, [from.ub, from.vb], from_tangent, [to.ub, to.vb], to_tangent, chord)?;
788 let (d0, d1) = (from_tangent.scale(chord), to_tangent.scale(chord));
789 let mut worst: f64 = 0.0;
790 for s in [0.17, 0.33, 0.5, 0.67, 0.83] {
791 let c = hermite_at(from.point, d0, to.point, d1, s);
792 let [ua, va] = on_a.at(s);
793 let a = first.surface.evaluate(fit(ua, first.u0, first.u1, first.closed_u), fit(va, first.v0, first.v1, first.closed_v))?;
794 let [ub, vb] = on_b.at(s);
795 let b = second.surface.evaluate(fit(ub, second.u0, second.u1, second.closed_u), fit(vb, second.v0, second.v1, second.closed_v))?;
796 worst = worst
797 .max(a.sub(b).length())
798 .max(a.sub(c).length())
799 .max(b.sub(c).length());
800 }
801 Ok(worst)
802}
803
804fn vector_angle(a: Vec3, b: Vec3) -> f64 {
805 (a.dot(b) / (a.length() * b.length()))
806 .clamp(-1.0, 1.0)
807 .acos()
808}
809
810fn normal_turn_step(
819 first: SurfaceInfo<'_>,
820 second: SurfaceInfo<'_>,
821 point: IntersectionPoint,
822 tangent: Vec3,
823 budget: f64,
824) -> Option<f64> {
825 let mut best: Option<f64> = None;
826 for (surface, u, v) in [
827 (first.surface, point.ua, point.va),
828 (second.surface, point.ub, point.vb),
829 ] {
830 let derivatives = surface.derivatives(u, v, 2).ok()?;
831 let normal = derivatives[1][0].cross(derivatives[0][1]);
832 let normal_length = normal.length();
833 if normal_length <= 1e-12 {
834 continue;
835 }
836 let unit_normal = normal.scale(1.0 / normal_length);
837 for (first_derivative, second_derivative) in [
838 (derivatives[1][0], derivatives[2][0]),
839 (derivatives[0][1], derivatives[0][2]),
840 ] {
841 let metric = first_derivative.dot(first_derivative);
842 if metric <= 1e-16 {
843 continue;
844 }
845 let curvature = second_derivative.dot(unit_normal).abs();
846 if curvature <= 1e-12 {
847 continue; }
849 let advance = budget * metric / curvature;
850 let alignment = first_derivative
851 .scale(1.0 / metric.sqrt())
852 .dot(tangent)
853 .abs();
854 if alignment <= 0.1 {
855 continue; }
857 let along = advance * alignment;
858 if best.map(|value| along < value).unwrap_or(true) {
859 best = Some(along);
860 }
861 }
862 }
863 best
864}
865
866#[derive(Clone, Copy)]
869struct TraceSwitches {
870 first_step_ceiling: bool,
873 debug: bool,
875}
876
877impl TraceSwitches {
878 fn from_env() -> Self {
879 Self {
880 first_step_ceiling: std::env::var("BREP_SSI_FIRST_STEP_BUDGET").as_deref() != Ok("0"),
881 debug: std::env::var_os("BREP_DEBUG_SSI_TRACE").is_some(),
882 }
883 }
884}
885
886#[allow(clippy::too_many_arguments)]
887fn trace(
888 first: SurfaceInfo<'_>,
889 second: SurfaceInfo<'_>,
890 start: IntersectionPoint,
891 direction: f64,
892 tolerance: f64,
893 initial_step: f64,
894 minimum_step: f64,
895 maximum_step: f64,
896 maximum_steps: usize,
897 switches: TraceSwitches,
898 span_budget: Option<f64>,
899) -> Result<Trace, String> {
900 let mut points = vec![start];
901 let mut current = start;
902 let Some(mut tangent) = tangent_at(first, second, start)? else {
903 return Ok(Trace {
904 points,
905 tangents: Vec::new(),
906 closed: false,
907 deviation: 0.0,
908 });
909 };
910 tangent = tangent.scale(direction);
911 let mut tangents = vec![tangent];
912 let mut deviation: f64 = 0.0;
913 let mut step_size = initial_step.min(maximum_step);
924 if switches.first_step_ceiling {
925 if let Some(budget) = normal_turn_step(first, second, start, tangent, 0.1) {
926 step_size = step_size.min(budget).clamp(minimum_step, maximum_step);
927 }
928 }
929 let mut terminated = false;
930 for _ in 0..maximum_steps {
931 let mut accepted = None;
932 while step_size >= minimum_step {
933 let predicted = current.point.add(tangent.scale(step_size));
934 if let Some((corrected, boundary)) =
935 correct(first, second, current, predicted, tangent, tolerance)?
936 {
937 if corrected.point.sub(current.point).length() <= 2.5 * step_size {
938 accepted = Some((corrected, boundary));
939 break;
940 }
941 }
942 step_size /= 2.0;
943 }
944 let Some((candidate, boundary)) = accepted else {
945 terminated = true;
946 break;
947 };
948 points.push(candidate);
949 if switches.debug {
950 let turn = tangent_at(first, second, candidate)
954 .ok()
955 .flatten()
956 .map(|next| vector_angle(tangent, if next.dot(tangent) < 0.0 { next.scale(-1.0) } else { next }))
957 .unwrap_or(f64::NAN);
958 let budget = normal_turn_step(first, second, current, tangent, 0.1).unwrap_or(f64::NAN);
959 let at = candidate.point;
960 eprintln!(
961 "SSI-TRACE dir {direction} step {step_size:.4e} jump {:.4e} turn {turn:.4} ceiling {budget:.4e} boundary {boundary} p ({:.6},{:.6},{:.6})",
962 at.sub(current.point).length(),
963 at.x,
964 at.y,
965 at.z
966 );
967 }
968 let next = tangent_at(first, second, candidate)?.map(|next| {
972 if next.dot(tangent) < 0.0 { next.scale(-1.0) } else { next }
973 });
974 if boundary {
975 tangents.push(next.unwrap_or(tangent));
976 terminated = true;
977 break;
978 }
979 let Some(next_tangent) = next else {
980 tangents.push(tangent);
981 terminated = true;
982 break;
983 };
984 let angle = vector_angle(tangent, next_tangent);
985 if angle > 0.35 {
986 points.pop();
987 step_size = minimum_step.max(step_size / 2.0);
988 if step_size <= minimum_step * 1.01 {
989 points.push(candidate);
990 } else {
991 continue;
992 }
993 }
994 let mut span_fine = true;
999 if let Some(budget) = span_budget {
1000 let measured = span_deviation(first, second, current, tangent, candidate, next_tangent)?;
1001 if measured > budget && step_size > minimum_step * 1.01 {
1002 points.pop();
1003 step_size = minimum_step.max(step_size / 2.0);
1004 continue;
1005 }
1006 deviation = deviation.max(measured);
1007 span_fine = measured < budget / 8.0;
1008 }
1009 tangents.push(next_tangent);
1010 let reactive = if angle < 0.03 && span_fine {
1015 step_size * 1.5
1016 } else if angle > 0.15 {
1017 step_size * 0.6
1018 } else {
1019 step_size
1020 };
1021 step_size = match normal_turn_step(first, second, candidate, next_tangent, 0.1) {
1022 Some(budget) => reactive.min(budget),
1023 None => reactive,
1024 }
1025 .clamp(minimum_step, maximum_step);
1026 if points.len() > 4 && candidate.point.sub(start.point).length() <= step_size * 1.2 {
1027 let to_start = start.point.sub(candidate.point);
1028 if to_start.length() <= EPSILON || to_start.dot(next_tangent) >= 0.0 {
1029 points.push(start);
1030 tangents.push(tangents[0]);
1031 return Ok(Trace {
1032 points,
1033 tangents,
1034 closed: true,
1035 deviation,
1036 });
1037 }
1038 }
1039 current = candidate;
1040 tangent = next_tangent;
1041 }
1042 if !terminated {
1043 return Err(format!(
1047 "surface intersection trace exhausted {maximum_steps} steps \
1048 ({} points, step {step_size:.3e}) without reaching a boundary or closure",
1049 points.len()
1050 ));
1051 }
1052 Ok(Trace {
1053 points,
1054 tangents,
1055 closed: false,
1056 deviation,
1057 })
1058}
1059
1060fn point_segment_distance(point: Vec3, start: Vec3, end: Vec3) -> f64 {
1061 let segment = end.sub(start);
1062 let length_squared = segment.length_squared();
1063 if length_squared <= EPSILON {
1064 return point.sub(start).length();
1065 }
1066 let parameter = point.sub(start).dot(segment) / length_squared;
1067 let parameter = parameter.clamp(0.0, 1.0);
1068 point.sub(start.add(segment.scale(parameter))).length()
1069}
1070
1071#[derive(Clone, Debug, Serialize)]
1072pub struct SurfaceIntersectionCurve {
1073 pub points: Vec<Vec3>,
1074 pub params_a: Vec<[f64; 2]>,
1075 pub params_b: Vec<[f64; 2]>,
1076 pub closed: bool,
1077 #[serde(skip_serializing_if = "Option::is_none")]
1081 pub tangents: Option<Vec<Vec3>>,
1082 #[serde(skip_serializing_if = "Option::is_none")]
1085 pub span_deviation: Option<f64>,
1086 #[serde(skip_serializing_if = "std::ops::Not::not")]
1091 pub tangent: bool,
1092}
1093
1094#[derive(Clone, Debug, Deserialize)]
1095pub struct SurfaceIntersectionOptions {
1096 #[serde(default = "default_tolerance")]
1097 pub tolerance: f64,
1098 #[serde(default = "default_density")]
1099 pub seed_density: f64,
1100 #[serde(default = "default_maximum_steps")]
1101 pub maximum_steps: usize,
1102 pub maximum_step: Option<f64>,
1103 #[serde(default)]
1104 pub seed_points: Vec<Vec3>,
1105 #[serde(default)]
1106 pub seed_only: bool,
1107 pub seed_gate: Option<f64>,
1108 #[serde(default)]
1125 pub span_budget: Option<f64>,
1126 #[serde(default)]
1128 pub ordered_starts: bool,
1129 #[serde(default)]
1135 pub consume_starts: bool,
1136 #[serde(default)]
1138 pub tree_seeds: bool,
1139 #[serde(default)]
1155 pub seed_start_cross: Option<f64>,
1156}
1157
1158fn default_tolerance() -> f64 {
1159 LINEAR_TOLERANCE
1160}
1161fn default_density() -> f64 {
1162 1.0
1163}
1164fn default_maximum_steps() -> usize {
1165 4000
1166}
1167
1168impl Default for SurfaceIntersectionOptions {
1169 fn default() -> Self {
1170 Self {
1171 tolerance: default_tolerance(),
1172 seed_density: default_density(),
1173 maximum_steps: default_maximum_steps(),
1174 maximum_step: None,
1175 seed_points: Vec::new(),
1176 seed_only: false,
1177 seed_gate: None,
1178 span_budget: None,
1179 ordered_starts: false,
1180 consume_starts: false,
1181 tree_seeds: false,
1182 seed_start_cross: None,
1183 }
1184 }
1185}
1186
1187pub(crate) fn refine_to_intersection(
1196 first_surface: &NurbsSurface,
1197 second_surface: &NurbsSurface,
1198 seed: Vec3,
1199 tolerance: f64,
1200 reach: f64,
1201) -> Result<Option<Vec3>, String> {
1202 let first = surface_info(first_surface)?;
1203 let second = surface_info(second_surface)?;
1204 let on_first = project_point_to_surface(first_surface, seed)?;
1205 let on_second = project_point_to_surface(second_surface, seed)?;
1206 let Some(refined) = refine_point(
1207 first,
1208 second,
1209 on_first.u,
1210 on_first.v,
1211 on_second.u,
1212 on_second.v,
1213 tolerance,
1214 )?
1215 else {
1216 return Ok(None);
1217 };
1218 if refined.point.sub(seed).length() > reach {
1219 return Ok(None);
1220 }
1221 Ok(Some(refined.point))
1222}
1223
1224pub fn intersect_surfaces(
1225 first_surface: &NurbsSurface,
1226 second_surface: &NurbsSurface,
1227 options: &SurfaceIntersectionOptions,
1228) -> Result<Vec<SurfaceIntersectionCurve>, String> {
1229 let first_box = Bounds::from_surface(first_surface)?;
1230 let second_box = Bounds::from_surface(second_surface)?;
1231 if !first_box.intersects(second_box, options.tolerance * 10.0) {
1232 return Ok(Vec::new());
1233 }
1234 let first = surface_info(first_surface)?;
1235 let second = surface_info(second_surface)?;
1236 let diagonal = first_box.diagonal().min(second_box.diagonal());
1237 let initial_step = diagonal / 100.0;
1238 let minimum_step = (options.tolerance * 100.0).max(diagonal * 1e-6);
1239 let maximum_step = (diagonal / 15.0).min(options.maximum_step.unwrap_or(f64::INFINITY));
1240 let maximum_steps = step_budget(options.maximum_steps, maximum_step, diagonal);
1241 let mut starts = if options.seed_only {
1242 Vec::new()
1243 } else if options.tree_seeds {
1244 match tree_start_points(first, second, options.tolerance)? {
1247 Some((starts, _pairs)) => starts,
1248 None => find_start_points(first, second, options.tolerance, options.seed_density)?,
1249 }
1250 } else {
1251 find_start_points(first, second, options.tolerance, options.seed_density)?
1252 };
1253 starts.retain(|start| transverse_at(first, second, *start));
1258 let seed_gate = options.seed_gate.unwrap_or(options.tolerance * 100.0);
1259 let grid_count = starts.len();
1261 for seed in &options.seed_points {
1262 let projection_a = project_point_to_surface(first_surface, *seed)?;
1263 if projection_a.distance > seed_gate {
1264 continue;
1265 }
1266 let projection_b = project_point_to_surface(second_surface, *seed)?;
1267 if projection_b.distance > seed_gate {
1268 continue;
1269 }
1270 let Some(refined) = refine_point(
1271 first,
1272 second,
1273 projection_a.u,
1274 projection_a.v,
1275 projection_b.u,
1276 projection_b.v,
1277 options.tolerance,
1278 )?
1279 else {
1280 continue;
1281 };
1282 let admitted = match options.seed_start_cross {
1285 Some(bound) => crossing_at(first, second, refined) >= bound.min(TRANSVERSE_START_CROSS),
1286 None => transverse_at(first, second, refined),
1287 };
1288 if !admitted {
1289 continue;
1290 }
1291 starts.push(refined);
1292 }
1293 if options.ordered_starts {
1294 let crossing = |start: &IntersectionPoint| -> f64 {
1298 match (
1299 first.surface.normal(start.ua, start.va),
1300 second.surface.normal(start.ub, start.vb),
1301 ) {
1302 (Ok(a), Ok(b)) => a.cross(b).length(),
1303 _ => 0.0,
1304 }
1305 };
1306 let mut seeds = starts.split_off(grid_count);
1307 seeds.sort_by(|a, b| crossing(b).total_cmp(&crossing(a)));
1308 starts.sort_by(|a, b| crossing(b).total_cmp(&crossing(a)));
1309 seeds.append(&mut starts);
1310 starts = seeds;
1311 }
1312
1313 let switches = TraceSwitches::from_env();
1314 let mut curves: Vec<SurfaceIntersectionCurve> = Vec::new();
1315 for start in starts {
1316 let mut claimed = false;
1317 for curve in &mut curves {
1318 let Some((index, distance)) = curve
1319 .points
1320 .windows(2)
1321 .enumerate()
1322 .map(|(index, segment)| (index, point_segment_distance(start.point, segment[0], segment[1])))
1323 .filter(|(_, distance)| *distance <= initial_step * 1.5)
1324 .min_by(|a, b| a.1.total_cmp(&b.1))
1325 else {
1326 continue;
1327 };
1328 claimed = true;
1329 if options.consume_starts
1334 && distance > 0.0
1335 && start.point.sub(curve.points[index]).length() > minimum_step
1336 && start.point.sub(curve.points[index + 1]).length() > minimum_step
1337 {
1338 curve.points.insert(index + 1, start.point);
1339 if let Some(tangents) = curve.tangents.as_mut() {
1340 let along = curve.points[index + 2].sub(curve.points[index]);
1341 let tangent = match tangent_at(first, second, start)? {
1342 Some(tangent) if tangent.dot(along) < 0.0 => tangent.scale(-1.0),
1343 Some(tangent) => tangent,
1344 None => tangents[index],
1345 };
1346 tangents.insert(index + 1, tangent);
1347 }
1348 if curve.params_a.len() > index + 1 && curve.params_b.len() > index + 1 {
1349 curve.params_a.insert(index + 1, [start.ua, start.va]);
1350 curve.params_b.insert(index + 1, [start.ub, start.vb]);
1351 }
1352 }
1353 break;
1354 }
1355 if switches.debug {
1356 let at = start.point;
1357 eprintln!(
1358 "SSI-START claimed {claimed} p ({:.6},{:.6},{:.6}) initial {initial_step:.4e} minimum {minimum_step:.4e} maximum {maximum_step:.4e}",
1359 at.x, at.y, at.z
1360 );
1361 }
1362 if claimed {
1363 continue;
1364 }
1365 let forward = trace(
1366 first,
1367 second,
1368 start,
1369 1.0,
1370 options.tolerance,
1371 initial_step,
1372 minimum_step,
1373 maximum_step,
1374 maximum_steps,
1375 switches,
1376 options.span_budget,
1377 )?;
1378 let (all, closed) = if forward.closed {
1379 (Spliced::from(forward), true)
1380 } else {
1381 let backward = trace(
1382 first,
1383 second,
1384 start,
1385 -1.0,
1386 options.tolerance,
1387 initial_step,
1388 minimum_step,
1389 maximum_step,
1390 maximum_steps,
1391 switches,
1392 options.span_budget,
1393 )?;
1394 (splice(backward, forward), false)
1395 };
1396 if all.points.len() < 2 {
1397 continue;
1398 }
1399 curves.push(curve_from(all, closed, options.span_budget.is_some()));
1400 }
1401 Ok(curves)
1402}
1403
1404const SUPPLEMENTAL_TRANSVERSE_CROSS: f64 = TRANSVERSE_START_CROSS;
1408
1409pub fn intersect_surfaces_supplemental(
1436 first_surface: &NurbsSurface,
1437 second_surface: &NurbsSurface,
1438 options: &SurfaceIntersectionOptions,
1439) -> Result<Vec<SurfaceIntersectionCurve>, String> {
1440 let first_box = Bounds::from_surface(first_surface)?;
1441 let second_box = Bounds::from_surface(second_surface)?;
1442 if !first_box.intersects(second_box, options.tolerance * 10.0) {
1443 return Ok(Vec::new());
1444 }
1445 let first = surface_info(first_surface)?;
1446 let second = surface_info(second_surface)?;
1447 let diagonal = first_box.diagonal().min(second_box.diagonal());
1448 let initial_step = diagonal / 100.0;
1449 let minimum_step = (options.tolerance * 100.0).max(diagonal * 1e-6);
1450 let maximum_step = (diagonal / 15.0).min(options.maximum_step.unwrap_or(f64::INFINITY));
1451 let maximum_steps = step_budget(options.maximum_steps, maximum_step, diagonal);
1452
1453 let density = options.seed_density.max(2.0);
1458 let mut starts: Vec<IntersectionPoint> = Vec::new();
1459 for gridded_second in [false, true] {
1460 let (a, b) = if gridded_second {
1461 (second, first)
1462 } else {
1463 (first, second)
1464 };
1465 for start in find_start_points(a, b, options.tolerance, density)? {
1466 let start = if gridded_second {
1469 IntersectionPoint {
1470 ua: start.ub,
1471 va: start.vb,
1472 ub: start.ua,
1473 vb: start.va,
1474 point: start.point,
1475 }
1476 } else {
1477 start
1478 };
1479 starts.push(start);
1480 }
1481 }
1482 starts.retain(|start| {
1486 match (
1487 first.surface.normal(start.ua, start.va),
1488 second.surface.normal(start.ub, start.vb),
1489 ) {
1490 (Ok(na), Ok(nb)) => na.cross(nb).length() > SUPPLEMENTAL_TRANSVERSE_CROSS,
1491 _ => false,
1492 }
1493 });
1494
1495 let switches = TraceSwitches::from_env();
1496 let mut curves: Vec<SurfaceIntersectionCurve> = Vec::new();
1497 for start in starts {
1498 let claimed = curves.iter().any(|curve| {
1499 curve.points.windows(2).any(|segment| {
1500 point_segment_distance(start.point, segment[0], segment[1]) <= initial_step * 1.5
1501 })
1502 });
1503 if claimed {
1504 continue;
1505 }
1506 let Ok(forward) = trace(
1510 first,
1511 second,
1512 start,
1513 1.0,
1514 options.tolerance,
1515 initial_step,
1516 minimum_step,
1517 maximum_step,
1518 maximum_steps,
1519 switches,
1520 options.span_budget,
1521 ) else {
1522 continue;
1523 };
1524 let (all, closed) = if forward.closed {
1525 (Spliced::from(forward), true)
1526 } else {
1527 let Ok(backward) = trace(
1528 first,
1529 second,
1530 start,
1531 -1.0,
1532 options.tolerance,
1533 initial_step,
1534 minimum_step,
1535 maximum_step,
1536 maximum_steps,
1537 switches,
1538 options.span_budget,
1539 ) else {
1540 continue;
1541 };
1542 (splice(backward, forward), false)
1543 };
1544 if all.points.len() < 2 {
1545 continue;
1546 }
1547 let transverse = all.points.iter().step_by((all.points.len() / 5).max(1)).any(|point| {
1551 match (
1552 first.surface.normal(point.ua, point.va),
1553 second.surface.normal(point.ub, point.vb),
1554 ) {
1555 (Ok(na), Ok(nb)) => na.cross(nb).length() > SUPPLEMENTAL_TRANSVERSE_CROSS,
1556 _ => false,
1557 }
1558 });
1559 if !transverse {
1560 continue;
1561 }
1562 curves.push(curve_from(all, closed, options.span_budget.is_some()));
1563 }
1564 Ok(curves)
1565}
1566