1use ogeom_core::{OgeomResult, Tolerances, ogeom_bail};
45use ogeom_geom::{Surface, SurfaceGeometry};
46use ogeom_math::{Point, Vector, solve};
47
48#[derive(Debug, Clone, Copy, PartialEq)]
50pub struct Marching {
51 pub chord: f64,
53 pub grid: usize,
59 pub max_points: usize,
62}
63
64impl Default for Marching {
65 fn default() -> Self {
66 Self {
67 chord: 1e-4,
68 grid: 24,
69 max_points: 20_000,
70 }
71 }
72}
73
74impl Marching {
75 pub fn validate(&self) -> OgeomResult<()> {
83 if !self.chord.is_finite() || self.chord <= 0.0 {
84 ogeom_bail!(Construction, "a chord of {} is not a distance", self.chord);
85 }
86 if self.grid < 2 {
87 ogeom_bail!(Construction, "a sampling grid needs at least two steps");
88 }
89 if self.max_points < 2 {
90 ogeom_bail!(Construction, "a branch needs at least two points");
91 }
92 Ok(())
93 }
94}
95
96#[derive(Debug, Clone, Copy, PartialEq)]
98pub struct Contact {
99 pub on_a: (f64, f64),
101 pub on_b: (f64, f64),
103 pub point: Point,
105}
106
107#[derive(Debug, Clone, Copy, PartialEq, Eq)]
109pub enum Stopped {
110 Closed,
112 LeftTheDomain,
114 Stalled,
120 RanOut,
125}
126
127#[derive(Debug, Clone, PartialEq)]
129pub struct Traced {
130 pub points: Vec<Point>,
132 pub on_a: Vec<(f64, f64)>,
134 pub on_b: Vec<(f64, f64)>,
136 pub stopped: Stopped,
138}
139
140impl Traced {
141 #[must_use]
143 pub const fn complete(&self) -> bool {
144 !matches!(self.stopped, Stopped::RanOut)
145 }
146
147 #[must_use]
149 pub const fn closed(&self) -> bool {
150 matches!(self.stopped, Stopped::Closed)
151 }
152}
153
154pub fn seeds(
166 a: &SurfaceGeometry,
167 b: &SurfaceGeometry,
168 options: Marching,
169 tol: Tolerances,
170) -> OgeomResult<Vec<Contact>> {
171 options.validate()?;
172 let (mesh_a, mesh_b) = (sample(a, options.grid, tol), sample(b, options.grid, tol));
173 let apart = span(a).min(span(b)) / f64::from(u32::try_from(options.grid).unwrap_or(1));
174 let near = CellBins::over(&mesh_b, options.chord);
175
176 let mut found: Vec<Contact> = Vec::new();
177 let mut candidates: Vec<usize> = Vec::new();
178 for cell_a in &mesh_a {
179 near.candidates(cell_a, &mut candidates);
184 for &j in &candidates {
185 let cell_b = &mesh_b[j];
186 if !overlap(cell_a, cell_b, options.chord) {
187 continue;
188 }
189 let Some(guess) = triangles_cross(cell_a, cell_b) else {
190 continue;
191 };
192 let start = [cell_a.at.0, cell_a.at.1, cell_b.at.0, cell_b.at.1];
193 let Some(contact) = correct(a, b, start, guess, None, tol) else {
194 continue;
195 };
196 if found
204 .iter()
205 .any(|c| c.point.distance(contact.point) <= apart)
206 {
207 continue;
208 }
209 found.push(contact);
210 }
211 }
212 for (from_a, border_of, other) in [(true, a, b), (false, b, a)] {
218 for (border, at) in spline_borders(border_of, tol) {
219 let Ok(met) = crate::intersect_curve_surface(
220 &border,
221 other,
222 crate::CurveSurfaceOptions::default(),
223 tol,
224 ) else {
225 continue;
226 };
227 for piercing in met.crossings {
228 let on_border = at(piercing.on_curve);
229 let start = if from_a {
230 [
231 on_border.0,
232 on_border.1,
233 piercing.on_surface.0,
234 piercing.on_surface.1,
235 ]
236 } else {
237 [
238 piercing.on_surface.0,
239 piercing.on_surface.1,
240 on_border.0,
241 on_border.1,
242 ]
243 };
244 let Some(contact) = correct(a, b, start, piercing.point, None, tol) else {
245 continue;
246 };
247 if found
248 .iter()
249 .any(|c| c.point.distance(contact.point) <= apart)
250 {
251 continue;
252 }
253 found.push(contact);
254 }
255 }
256 }
257 Ok(found)
258}
259
260type Border = (ogeom_geom::Curve, Box<dyn Fn(f64) -> (f64, f64)>);
263
264fn spline_borders(surface: &SurfaceGeometry, tol: Tolerances) -> Vec<Border> {
266 let SurfaceGeometry::BSpline(spline) = surface else {
267 return Vec::new();
268 };
269 let ((u0, u1), (v0, v1)) = surface.domain();
270 let mut out: Vec<Border> = Vec::new();
271 if !surface.is_closed_u(tol) {
272 for u in [u0, u1] {
273 if let Ok(c) = spline.iso_u_curve(u, tol) {
274 out.push((ogeom_geom::Curve::BSpline(c), Box::new(move |t| (u, t))));
275 }
276 }
277 }
278 if !surface.is_closed_v(tol) {
279 for v in [v0, v1] {
280 if let Ok(c) = spline.iso_v_curve(v, tol) {
281 out.push((ogeom_geom::Curve::BSpline(c), Box::new(move |t| (t, v))));
282 }
283 }
284 }
285 out
286}
287
288pub fn branches(
302 a: &SurfaceGeometry,
303 b: &SurfaceGeometry,
304 options: Marching,
305 tol: Tolerances,
306) -> OgeomResult<Vec<Traced>> {
307 let found = seeds(a, b, options, tol)?;
308 let mut out: Vec<Traced> = Vec::new();
309 for seed in found {
310 let reach = options.chord.max(tol.confusion()) * 8.0;
312 if out
313 .iter()
314 .any(|branch| passes_near(branch, seed.point, reach))
315 {
316 continue;
317 }
318 if let Ok(branch) = trace(a, b, seed, options, tol)
322 && branch.points.len() >= 2
323 && !is_fragment(&branch, options)
324 {
325 let middle = branch.points[branch.points.len() / 2];
328 if out.iter().any(|other| passes_near(other, middle, reach)) {
329 continue;
330 }
331 out.push(branch);
332 }
333 }
334 let stitched = stitch_stalled(out, a, b, options, tol);
335 Ok(split_at_touches(stitched, a, b, options, tol))
336}
337
338pub(crate) const BRANCH_POINT_SINE: f64 = 0.05;
342
343fn crossing_sine(
345 a: &SurfaceGeometry,
346 b: &SurfaceGeometry,
347 on_a: (f64, f64),
348 on_b: (f64, f64),
349 tol: Tolerances,
350) -> f64 {
351 let Ok(na) = a.normal_at(on_a.0, on_a.1, tol) else {
352 return 0.0;
353 };
354 let Ok(nb) = b.normal_at(on_b.0, on_b.1, tol) else {
355 return 0.0;
356 };
357 na.vector().cross(nb.vector()).magnitude()
358}
359
360fn is_fragment(branch: &Traced, options: Marching) -> bool {
377 if branch.stopped != Stopped::Stalled {
378 return false;
379 }
380 let length: f64 = branch
381 .points
382 .windows(2)
383 .map(|pair| pair[0].distance(pair[1]))
384 .sum();
385 length < options.chord * 10.0
386}
387
388fn passes_near(branch: &Traced, p: Point, reach: f64) -> bool {
395 branch
396 .points
397 .windows(2)
398 .any(|pair| distance_to_segment(p, pair[0], pair[1]) <= reach)
399}
400
401fn distance_to_segment(p: Point, a: Point, b: Point) -> f64 {
403 let along = b - a;
404 let length = along.square_magnitude();
405 if length <= f64::MIN_POSITIVE {
406 return p.distance(a);
407 }
408 let t = ((p - a).dot(along) / length).clamp(0.0, 1.0);
409 p.distance(a + along * t)
410}
411
412pub fn trace(
420 a: &SurfaceGeometry,
421 b: &SurfaceGeometry,
422 from: Contact,
423 options: Marching,
424 tol: Tolerances,
425) -> OgeomResult<Traced> {
426 options.validate()?;
427 if tangent_at(a, b, from, tol).is_none() {
428 ogeom_bail!(
429 NotDone,
430 "the surfaces are tangent here, so the intersection has no single \
431 direction to follow; that is a branch point and needs the seed \
432 moved off it"
433 );
434 }
435
436 let ahead = walk(a, b, from, 1.0, options, tol)?;
439 if ahead.stopped == Stopped::Closed {
440 return Ok(ahead);
441 }
442 let behind = walk(a, b, from, -1.0, options, tol)?;
443 let last_step = |walked: &[Point]| -> f64 {
444 walked
445 .windows(2)
446 .last()
447 .map_or(0.0, |w| w[0].distance(w[1]))
448 };
449 let steps = last_step(&ahead.points).max(last_step(&behind.points));
450
451 let mut points = behind.points;
453 let mut on_a = behind.on_a;
454 let mut on_b = behind.on_b;
455 points.reverse();
456 on_a.reverse();
457 on_b.reverse();
458 points.pop();
459 on_a.pop();
460 on_b.pop();
461 points.extend(ahead.points);
462 on_a.extend(ahead.on_a);
463 on_b.extend(ahead.on_b);
464
465 let mut stopped = if ahead.stopped == Stopped::RanOut || behind.stopped == Stopped::RanOut {
468 Stopped::RanOut
469 } else if ahead.stopped == Stopped::Stalled || behind.stopped == Stopped::Stalled {
470 Stopped::Stalled
471 } else {
472 Stopped::LeftTheDomain
473 };
474 if stopped == Stopped::LeftTheDomain && points.len() > 3 {
484 let gap = points[0].distance(points[points.len() - 1]);
485 if gap <= (steps * 2.0).max(tol.confusion() * 10.0) {
486 if gap <= tol.confusion() {
488 points.pop();
489 on_a.pop();
490 on_b.pop();
491 }
492 points.push(points[0]);
493 on_a.push(on_a[0]);
494 on_b.push(on_b[0]);
495 stopped = Stopped::Closed;
496 }
497 }
498 Ok(Traced {
499 points,
500 on_a,
501 on_b,
502 stopped,
503 })
504}
505
506struct SurfacePair<'s> {
515 a: &'s SurfaceGeometry,
516 b: &'s SurfaceGeometry,
517}
518
519impl crate::walk::Condition for SurfacePair<'_> {
520 fn unknowns(&self) -> usize {
521 4
522 }
523
524 fn position(&self, x: &[f64], tol: Tolerances) -> Option<Point> {
525 self.a.point_at(x[0], x[1], tol).ok()
526 }
527
528 fn position_gradient(&self, x: &[f64], tol: Tolerances) -> Option<Vec<Vector>> {
529 let (au, av) = self.a.d1_at(x[0], x[1], tol).ok()?;
530 Some(vec![au, av, Vector::ZERO, Vector::ZERO])
533 }
534
535 fn system(&self, x: &[f64], tol: Tolerances) -> Option<(Vec<f64>, Vec<Vec<f64>>)> {
536 Some(self.system_at(x, tol)?.0)
537 }
538
539 fn system_at(
540 &self,
541 x: &[f64],
542 tol: Tolerances,
543 ) -> Option<((Vec<f64>, Vec<Vec<f64>>), Point, Vec<Vector>)> {
544 let (pa, au, av) = self.a.point_d1_at(x[0], x[1], tol).ok()?;
545 let (pb, bu, bv) = self.b.point_d1_at(x[2], x[3], tol).ok()?;
546 let gap = pa - pb;
547 Some((
548 (
549 vec![gap.x, gap.y, gap.z],
550 vec![
551 vec![au.x, av.x, -bu.x, -bv.x],
552 vec![au.y, av.y, -bu.y, -bv.y],
553 vec![au.z, av.z, -bu.z, -bv.z],
554 ],
555 ),
556 pa,
557 vec![au, av, Vector::ZERO, Vector::ZERO],
558 ))
559 }
560
561 fn clamp(&self, x: &mut [f64]) {
562 let (ua, va) = clamp(self.a, x[0], x[1]);
563 let (ub, vb) = clamp(self.b, x[2], x[3]);
564 x[0] = ua;
565 x[1] = va;
566 x[2] = ub;
567 x[3] = vb;
568 }
569
570 fn outside(&self, x: &[f64], tol: Tolerances) -> bool {
571 outside(self.a, (x[0], x[1]), tol) || outside(self.b, (x[2], x[3]), tol)
572 }
573
574 fn near_edge(&self, x: &[f64]) -> bool {
575 near_edge(self.a, (x[0], x[1])) || near_edge(self.b, (x[2], x[3]))
576 }
577
578 fn extent(&self) -> f64 {
579 span(self.a).max(span(self.b))
580 }
581
582 fn tangent_is_oriented(&self) -> bool {
583 true
586 }
587
588 fn tangent(&self, x: &[f64], tol: Tolerances) -> Option<Vector> {
589 tangent_at(
590 self.a,
591 self.b,
592 Contact {
593 on_a: (x[0], x[1]),
594 on_b: (x[2], x[3]),
595 point: Point::ORIGIN,
596 },
597 tol,
598 )
599 }
600
601 fn tangent_from(
602 &self,
603 x: &[f64],
604 jacobian: &[Vec<f64>],
605 gradient: &[Vector],
606 tol: Tolerances,
607 ) -> Option<Vector> {
608 if jacobian.len() != 3 || jacobian.iter().any(|row| row.len() != 4) {
611 return self.tangent(x, tol);
612 }
613 let _ = gradient;
614 let column = |c: usize| Vector::new(jacobian[0][c], jacobian[1][c], jacobian[2][c]);
615 let na = unit_normal(column(0), column(1), tol)?;
616 let nb = unit_normal(-column(2), -column(3), tol)?;
617 tangent_of_normals(na, nb, tol)
618 }
619}
620
621fn walk(
623 a: &SurfaceGeometry,
624 b: &SurfaceGeometry,
625 from: Contact,
626 sense: f64,
627 options: Marching,
628 tol: Tolerances,
629) -> OgeomResult<Traced> {
630 let pair = SurfacePair { a, b };
631 let start = [from.on_a.0, from.on_a.1, from.on_b.0, from.on_b.1];
632 let mut walked = crate::walk::walk_one_way(&pair, &start, sense, options, tol)?;
633 if walked.stopped == Stopped::LeftTheDomain {
634 land_on_edge(&pair, &mut walked, tol);
635 }
636 Ok(Traced {
637 on_a: walked.states.iter().map(|x| (x[0], x[1])).collect(),
638 on_b: walked.states.iter().map(|x| (x[2], x[3])).collect(),
639 points: walked.points,
640 stopped: walked.stopped,
641 })
642}
643
644fn land_on_edge(pair: &SurfacePair<'_>, walked: &mut crate::walk::Walked, tol: Tolerances) {
653 use crate::walk::Condition as _;
654 let n = walked.states.len();
655 if n < 2 {
656 return;
657 }
658 let (last, before) = (&walked.states[n - 1], &walked.states[n - 2]);
659 let step = walked.points[n - 1].distance(walked.points[n - 2]);
660 let heading = walked.points[n - 1] - walked.points[n - 2];
661 let mut bounds: Vec<(f64, usize, f64)> = Vec::new();
662 for (surface, first) in [(pair.a, 0_usize), (pair.b, 2)] {
663 let ((ua, ub), (va, vb)) = surface.domain();
664 for (k, (lo, hi), periodic) in [
665 (first, (ua, ub), surface.is_periodic_u()),
666 (first + 1, (va, vb), surface.is_periodic_v()),
667 ] {
668 let span = hi - lo;
669 if periodic || !span.is_finite() || span <= 0.0 {
670 continue;
671 }
672 let moving = last[k] - before[k];
675 let across = if k == first {
676 surface.domain().1
677 } else {
678 surface.domain().0
679 };
680 let collapses = |bound: f64| {
681 let at = |t: f64| {
682 if k == first {
683 surface.point_at(bound, t, tol)
684 } else {
685 surface.point_at(t, bound, tol)
686 }
687 };
688 match (at(across.0), at(f64::midpoint(across.0, across.1))) {
689 (Ok(p), Ok(q)) => p.distance(q) <= tol.confusion(),
690 _ => true,
691 }
692 };
693 for (bound, toward) in [(lo, moving < 0.0), (hi, moving > 0.0)] {
694 if (toward || (last[k] - bound).abs() <= span * 1e-4) && !collapses(bound) {
695 bounds.push(((last[k] - bound).abs() / span, k, bound));
696 }
697 }
698 }
699 }
700 bounds.sort_by(|x, y| x.0.total_cmp(&y.0));
701 for (_, k, bound) in bounds {
702 let system = |x: &[f64; 4]| {
703 let mut residual = [f64::INFINITY; 4];
704 let mut jacobian = [[0.0; 4]; 4];
705 let Some(((rows, matrix), _, _)) = pair.system_at(x, tol) else {
706 return (residual, jacobian);
707 };
708 residual[..3].copy_from_slice(&rows);
709 for (to, row) in jacobian.iter_mut().zip(&matrix) {
710 to.copy_from_slice(row);
711 }
712 residual[3] = x[k] - bound;
713 jacobian[3][k] = 1.0;
714 (residual, jacobian)
715 };
716 let criteria = solve::Criteria {
717 residual: tol.confusion() * 0.01,
718 step: tol.parametric(),
719 max_iterations: 40,
720 };
721 let Ok(start) = <[f64; 4]>::try_from(last.as_slice()) else {
722 return;
723 };
724 let Ok((at, norm, _, _)) = solve::newton_system_fixed(system, start, criteria) else {
725 continue;
726 };
727 if norm > tol.confusion() {
728 continue;
729 }
730 let within = [(pair.a, 0_usize), (pair.b, 2)]
731 .iter()
732 .all(|(surface, first)| {
733 let ((ua, ub), (va, vb)) = surface.domain();
734 [
735 (*first, ua, ub, surface.is_periodic_u()),
736 (first + 1, va, vb, surface.is_periodic_v()),
737 ]
738 .iter()
739 .all(|&(i, lo, hi, periodic)| {
740 periodic || (at[i] >= lo - tol.parametric() && at[i] <= hi + tol.parametric())
741 })
742 });
743 if !within {
744 continue;
745 }
746 let Ok(point) = pair.a.point_at(at[0], at[1], tol) else {
747 continue;
748 };
749 let ahead = point - walked.points[n - 1];
750 if ahead.magnitude() > (step * 2.0).max(tol.confusion())
751 || ahead.dot(heading) < -tol.confusion() * step
752 {
753 continue;
754 }
755 if ahead.magnitude() <= tol.confusion() {
756 walked.states[n - 1] = at.to_vec();
757 walked.points[n - 1] = point;
758 } else {
759 walked.states.push(at.to_vec());
760 walked.points.push(point);
761 }
762 return;
763 }
764}
765
766const SHALLOWEST: f64 = 1e-6;
784
785fn tangent_at(
792 a: &SurfaceGeometry,
793 b: &SurfaceGeometry,
794 at: Contact,
795 tol: Tolerances,
796) -> Option<Vector> {
797 let na = normal_at(a, at.on_a, tol)?;
798 let nb = normal_at(b, at.on_b, tol)?;
799 tangent_of_normals(na, nb, tol)
800}
801
802fn tangent_of_normals(na: Vector, nb: Vector, tol: Tolerances) -> Option<Vector> {
805 let cross = na.cross(nb);
806 let length = cross.magnitude();
807 let floor = tol.angular().max(SHALLOWEST);
815 let widen = |value: f64| ogeom_math::Interval::about(value, tol.confusion());
816 let (ax, ay, az) = (widen(na.x), widen(na.y), widen(na.z));
817 let (bx, by, bz) = (widen(nb.x), widen(nb.y), widen(nb.z));
818 let cx = ay.mul(&bz).sub(&az.mul(&by));
819 let cy = az.mul(&bx).sub(&ax.mul(&bz));
820 let cz = ax.mul(&by).sub(&ay.mul(&bx));
821 let magnitude2 = cx.square().add(&cy.square()).add(&cz.square());
822 let above = magnitude2.sub(&ogeom_math::Interval::point(floor * floor));
823 if above.certain_sign() != Some(ogeom_core::Sign::Positive) || length <= f64::MIN_POSITIVE {
824 return None;
825 }
826 Some(cross * (1.0 / length))
827}
828
829fn normal_at(surface: &SurfaceGeometry, at: (f64, f64), tol: Tolerances) -> Option<Vector> {
831 let (du, dv) = surface.d1_at(at.0, at.1, tol).ok()?;
832 unit_normal(du, dv, tol)
833}
834
835fn unit_normal(du: Vector, dv: Vector, tol: Tolerances) -> Option<Vector> {
837 let cross = du.cross(dv);
838 let length = cross.magnitude();
839 if length <= tol.confusion() {
840 return None;
841 }
842 Some(cross * (1.0 / length))
843}
844
845fn correct(
856 a: &SurfaceGeometry,
857 b: &SurfaceGeometry,
858 start: [f64; 4],
859 guess: Point,
860 constraint: Option<(Point, Vector, f64)>,
861 tol: Tolerances,
862) -> Option<Contact> {
863 let (anchor, along, reach) = match constraint {
864 Some(given) => given,
865 None => {
866 let at = Contact {
870 on_a: (start[0], start[1]),
871 on_b: (start[2], start[3]),
872 point: guess,
873 };
874 (guess, tangent_at(a, b, at, tol).unwrap_or(Vector::X), 0.0)
875 }
876 };
877
878 let system = |x: &[f64; 4]| {
879 let (ua, va) = clamp(a, x[0], x[1]);
880 let (ub, vb) = clamp(b, x[2], x[3]);
881 let (Ok((pa, au, av)), Ok((pb, bu, bv))) =
885 (a.point_d1_at(ua, va, tol), b.point_d1_at(ub, vb, tol))
886 else {
887 return ([f64::INFINITY; 4], [[0.0; 4]; 4]);
888 };
889
890 let gap = pa - pb;
891 let residual = [gap.x, gap.y, gap.z, (pa - anchor).dot(along) - reach];
892 let jacobian = [
893 [au.x, av.x, -bu.x, -bv.x],
894 [au.y, av.y, -bu.y, -bv.y],
895 [au.z, av.z, -bu.z, -bv.z],
896 [au.dot(along), av.dot(along), 0.0, 0.0],
897 ];
898 (residual, jacobian)
899 };
900
901 let criteria = solve::Criteria {
902 residual: tol.confusion() * 0.01,
903 step: tol.parametric(),
904 max_iterations: 40,
905 };
906 let found = solve::newton_system_fixed(system, start, criteria).ok()?;
907 if found.1 > tol.confusion() {
908 return None;
909 }
910 let (ua, va) = clamp(a, found.0[0], found.0[1]);
911 let (ub, vb) = clamp(b, found.0[2], found.0[3]);
912 Some(Contact {
913 on_a: (ua, va),
914 on_b: (ub, vb),
915 point: a.point_at(ua, va, tol).ok()?,
916 })
917}
918
919pub(crate) fn clamp(surface: &SurfaceGeometry, u: f64, v: f64) -> (f64, f64) {
924 let ((ua, ub), (va, vb)) = surface.domain();
925 let fold = |x: f64, lo: f64, hi: f64, periodic: bool| {
926 if !periodic {
927 return x.clamp(lo, hi);
928 }
929 let span = hi - lo;
930 if span <= 0.0 {
931 return x;
932 }
933 lo + (x - lo).rem_euclid(span)
934 };
935 (
936 fold(u, ua, ub, surface.is_periodic_u()),
937 fold(v, va, vb, surface.is_periodic_v()),
938 )
939}
940
941fn near_edge(surface: &SurfaceGeometry, at: (f64, f64)) -> bool {
948 let ((ua, ub), (va, vb)) = surface.domain();
949 let close = |x: f64, lo: f64, hi: f64, periodic: bool| {
950 !periodic && {
951 let band = (hi - lo).abs() * 1e-4;
952 x <= lo + band || x >= hi - band
953 }
954 };
955 close(at.0, ua, ub, surface.is_periodic_u()) || close(at.1, va, vb, surface.is_periodic_v())
956}
957
958fn outside(surface: &SurfaceGeometry, at: (f64, f64), tol: Tolerances) -> bool {
960 let ((ua, ub), (va, vb)) = surface.domain();
961 let past = |x: f64, lo: f64, hi: f64, periodic: bool| {
962 !periodic && (x <= lo + tol.parametric() || x >= hi - tol.parametric())
963 };
964 past(at.0, ua, ub, surface.is_periodic_u()) || past(at.1, va, vb, surface.is_periodic_v())
965}
966
967pub(crate) fn span(surface: &SurfaceGeometry) -> f64 {
969 let ((ua, ub), (va, vb)) = surface.domain();
970 let tol = Tolerances::millimetres();
971 let corners = [(ua, va), (ub, va), (ua, vb), (ub, vb)];
972 let mut low = Point::new(f64::MAX, f64::MAX, f64::MAX);
973 let mut high = Point::new(f64::MIN, f64::MIN, f64::MIN);
974 for (u, v) in corners {
975 if let Ok(p) = surface.point_at(u, v, tol) {
976 low = Point::new(low.x.min(p.x), low.y.min(p.y), low.z.min(p.z));
977 high = Point::new(high.x.max(p.x), high.y.max(p.y), high.z.max(p.z));
978 }
979 }
980 let size = (high - low).magnitude();
981 if size.is_finite() && size > 0.0 {
982 size
983 } else {
984 1.0
985 }
986}
987
988#[derive(Debug, Clone)]
990pub(crate) struct Cell {
991 pub(crate) corners: [Point; 3],
992 pub(crate) at: (f64, f64),
993 pub(crate) low: Point,
994 pub(crate) high: Point,
995 pub(crate) sag: f64,
998 pub(crate) params: [(f64, f64); 3],
1000}
1001
1002pub(crate) fn sample(surface: &SurfaceGeometry, grid: usize, tol: Tolerances) -> Vec<Cell> {
1004 sample_by(surface, (grid, grid), tol)
1005}
1006
1007pub(crate) fn sample_by(
1009 surface: &SurfaceGeometry,
1010 counts: (usize, usize),
1011 tol: Tolerances,
1012) -> Vec<Cell> {
1013 let ((ua, ub), (va, vb)) = surface.domain();
1014 let limit = 1.0e6;
1017 let (ua, ub) = (ua.max(-limit), ub.min(limit));
1018 let (va, vb) = (va.max(-limit), vb.min(limit));
1019
1020 let mut out = Vec::new();
1021 #[allow(clippy::cast_precision_loss)]
1022 let (nu, nv) = (counts.0 as f64, counts.1 as f64);
1023 let at = |s: f64, t: f64| {
1024 let (u, v) = (ua + (ub - ua) * s, va + (vb - va) * t);
1025 surface.point_at(u, v, tol).ok().map(|p| ((u, v), p))
1026 };
1027 #[allow(clippy::cast_precision_loss)]
1030 let row = |i: usize| -> Vec<_> {
1031 (0..=counts.1)
1032 .map(|j| at(i as f64 / nu, j as f64 / nv))
1033 .collect()
1034 };
1035 let mut below = row(0);
1036 for i in 0..counts.0 {
1037 let above = row(i + 1);
1038 for j in 0..counts.1 {
1039 #[allow(clippy::cast_precision_loss)]
1040 let (s0, s1) = (i as f64 / nu, (i + 1) as f64 / nu);
1041 #[allow(clippy::cast_precision_loss)]
1042 let (t0, t1) = (j as f64 / nv, (j + 1) as f64 / nv);
1043 let (Some((p00, a00)), Some((p10, a10)), Some((p01, a01)), Some((p11, a11))) =
1044 (below[j], above[j], below[j + 1], above[j + 1])
1045 else {
1046 continue;
1047 };
1048 let sag = at(f64::midpoint(s0, s1), f64::midpoint(t0, t1))
1049 .map_or(0.0, |(_, middle)| middle.distance(a00.midpoint(a11)));
1050 for (corners, params) in [
1051 ([a00, a10, a11], [p00, p10, p11]),
1052 ([a00, a11, a01], [p00, p11, p01]),
1053 ] {
1054 let low = Point::new(
1055 corners.iter().map(|p| p.x).fold(f64::MAX, f64::min),
1056 corners.iter().map(|p| p.y).fold(f64::MAX, f64::min),
1057 corners.iter().map(|p| p.z).fold(f64::MAX, f64::min),
1058 );
1059 let high = Point::new(
1060 corners.iter().map(|p| p.x).fold(f64::MIN, f64::max),
1061 corners.iter().map(|p| p.y).fold(f64::MIN, f64::max),
1062 corners.iter().map(|p| p.z).fold(f64::MIN, f64::max),
1063 );
1064 out.push(Cell {
1065 corners,
1066 at: p00,
1067 low,
1068 high,
1069 sag,
1070 params,
1071 });
1072 }
1073 }
1074 below = above;
1075 }
1076 out
1077}
1078
1079struct CellBins {
1083 low: Point,
1084 size: f64,
1085 counts: [usize; 3],
1086 bins: Vec<Vec<usize>>,
1087 margin: f64,
1088}
1089
1090impl CellBins {
1091 const MOST: usize = 48;
1094
1095 fn over(cells: &[Cell], margin: f64) -> Self {
1096 let mut low = Point::new(f64::INFINITY, f64::INFINITY, f64::INFINITY);
1097 let mut high = Point::new(f64::NEG_INFINITY, f64::NEG_INFINITY, f64::NEG_INFINITY);
1098 for c in cells {
1099 low = Point::new(low.x.min(c.low.x), low.y.min(c.low.y), low.z.min(c.low.z));
1100 high = Point::new(
1101 high.x.max(c.high.x),
1102 high.y.max(c.high.y),
1103 high.z.max(c.high.z),
1104 );
1105 }
1106 let extent = (high - low).magnitude();
1107 #[allow(clippy::cast_precision_loss)]
1108 let size = if extent.is_finite() && extent > 0.0 {
1109 (extent / Self::MOST as f64).max(margin)
1110 } else {
1111 1.0
1112 };
1113 let count = |lo: f64, hi: f64| -> usize {
1114 #[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
1115 let n = ((hi - lo) / size).floor() as usize + 1;
1116 n.clamp(1, Self::MOST + 1)
1117 };
1118 let counts = if extent.is_finite() {
1119 [
1120 count(low.x, high.x),
1121 count(low.y, high.y),
1122 count(low.z, high.z),
1123 ]
1124 } else {
1125 [1, 1, 1]
1126 };
1127 let mut bins = vec![Vec::new(); counts[0] * counts[1] * counts[2]];
1128 let mut this = Self {
1129 low,
1130 size,
1131 counts,
1132 bins: Vec::new(),
1133 margin,
1134 };
1135 for (i, c) in cells.iter().enumerate() {
1136 this.each_bin(c.low, c.high, |k| bins[k].push(i));
1137 }
1138 this.bins = bins;
1139 this
1140 }
1141
1142 fn each_bin(&self, low: Point, high: Point, mut visit: impl FnMut(usize)) {
1144 let index = |x: f64, lo: f64, n: usize| -> usize {
1145 if !x.is_finite() {
1146 return if x > 0.0 { n - 1 } else { 0 };
1147 }
1148 #[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
1149 let k = ((x - lo) / self.size).floor().max(0.0) as usize;
1150 k.min(n - 1)
1151 };
1152 let m = self.margin;
1153 let [nx, ny, nz] = self.counts;
1154 let (x0, x1) = (
1155 index(low.x - m, self.low.x, nx),
1156 index(high.x + m, self.low.x, nx),
1157 );
1158 let (y0, y1) = (
1159 index(low.y - m, self.low.y, ny),
1160 index(high.y + m, self.low.y, ny),
1161 );
1162 let (z0, z1) = (
1163 index(low.z - m, self.low.z, nz),
1164 index(high.z + m, self.low.z, nz),
1165 );
1166 for x in x0..=x1 {
1167 for y in y0..=y1 {
1168 for z in z0..=z1 {
1169 visit((x * ny + y) * nz + z);
1170 }
1171 }
1172 }
1173 }
1174
1175 fn candidates(&self, cell: &Cell, out: &mut Vec<usize>) {
1177 out.clear();
1178 self.each_bin(cell.low, cell.high, |k| {
1179 out.extend_from_slice(&self.bins[k])
1180 });
1181 out.sort_unstable();
1182 out.dedup();
1183 }
1184}
1185
1186fn overlap(a: &Cell, b: &Cell, margin: f64) -> bool {
1187 a.low.x <= b.high.x + margin
1188 && b.low.x <= a.high.x + margin
1189 && a.low.y <= b.high.y + margin
1190 && b.low.y <= a.high.y + margin
1191 && a.low.z <= b.high.z + margin
1192 && b.low.z <= a.high.z + margin
1193}
1194
1195fn triangles_cross(a: &Cell, b: &Cell) -> Option<Point> {
1201 for (edges, target) in [(a, b), (b, a)] {
1202 for k in 0..3 {
1203 let (from, to) = (edges.corners[k], edges.corners[(k + 1) % 3]);
1204 if let Some(hit) = segment_meets_triangle(from, to, target.corners) {
1205 return Some(hit);
1206 }
1207 }
1208 }
1209 None
1210}
1211
1212pub(crate) fn segment_meets_triangle(from: Point, to: Point, t: [Point; 3]) -> Option<Point> {
1214 let direction = to - from;
1215 let (e1, e2) = (t[1] - t[0], t[2] - t[0]);
1216 let h = direction.cross(e2);
1217 let determinant = e1.dot(h);
1218 if determinant.abs() <= f64::MIN_POSITIVE {
1219 return None;
1220 }
1221 let inverse = 1.0 / determinant;
1222 let s = from - t[0];
1223 let u = inverse * s.dot(h);
1224 if !(0.0..=1.0).contains(&u) {
1225 return None;
1226 }
1227 let q = s.cross(e1);
1228 let v = inverse * direction.dot(q);
1229 if v < 0.0 || u + v > 1.0 {
1230 return None;
1231 }
1232 let along = inverse * e2.dot(q);
1233 if !(0.0..=1.0).contains(&along) {
1234 return None;
1235 }
1236 Some(from + direction * along)
1237}
1238
1239struct Arc {
1241 points: Vec<Point>,
1242 on_a: Vec<(f64, f64)>,
1243 on_b: Vec<(f64, f64)>,
1244 head_bp: Option<usize>,
1246 tail_bp: Option<usize>,
1247}
1248
1249impl Arc {
1250 fn length(&self) -> f64 {
1251 self.points
1252 .windows(2)
1253 .map(|pair| pair[0].distance(pair[1]))
1254 .sum()
1255 }
1256
1257 fn outgoing(&self, tail: bool) -> Option<Vector> {
1260 let n = self.points.len();
1261 if n < 2 {
1262 return None;
1263 }
1264 let window = (n - 1).min(24);
1265 let (at, back) = if tail {
1266 (n - 1, n - 1 - window)
1267 } else {
1268 (0, window)
1269 };
1270 let out = self.points[at] - self.points[back];
1271 let m = out.magnitude();
1272 (m > f64::MIN_POSITIVE).then(|| out / m)
1273 }
1274}
1275
1276fn interior_is_transversal(
1282 branch: &Traced,
1283 a: &SurfaceGeometry,
1284 b: &SurfaceGeometry,
1285 tol: Tolerances,
1286) -> bool {
1287 let n = branch.points.len();
1288 if n < 5 {
1289 return false;
1290 }
1291 [n / 4, n / 2, 3 * n / 4]
1292 .into_iter()
1293 .any(|i| crossing_sine(a, b, branch.on_a[i], branch.on_b[i], tol) > BRANCH_POINT_SINE)
1294}
1295
1296fn stitch_stalled(
1315 found: Vec<Traced>,
1316 a: &SurfaceGeometry,
1317 b: &SurfaceGeometry,
1318 options: Marching,
1319 tol: Tolerances,
1320) -> Vec<Traced> {
1321 let reach = options.chord.max(tol.confusion()) * 60.0;
1322 const CONTINUES: f64 = 0.5;
1323
1324 let (candidates, mut out): (Vec<Traced>, Vec<Traced>) = found.into_iter().partition(|branch| {
1325 branch.stopped == Stopped::Stalled && interior_is_transversal(branch, a, b, tol)
1326 });
1327 if candidates.is_empty() {
1328 return out;
1329 }
1330
1331 let mut bps: Vec<Point> = Vec::new();
1333 for branch in &candidates {
1334 let n = branch.points.len();
1335 for at in [0, n - 1] {
1336 if crossing_sine(a, b, branch.on_a[at], branch.on_b[at], tol) < BRANCH_POINT_SINE {
1337 let p = branch.points[at];
1338 if !bps.iter().any(|held| held.distance(p) <= reach) {
1339 bps.push(p);
1340 }
1341 }
1342 }
1343 }
1344 if bps.is_empty() {
1345 out.extend(candidates);
1346 return out;
1347 }
1348 let bp_of =
1349 |p: Point| -> Option<usize> { bps.iter().position(|held| held.distance(p) <= reach) };
1350
1351 let mut arcs: Vec<Arc> = Vec::new();
1355 for branch in &candidates {
1356 let n = branch.points.len();
1357 let mut run_start: Option<usize> = None;
1358 for i in 0..=n {
1359 let near = i < n && bp_of(branch.points[i]).is_some();
1360 match (run_start, near, i == n) {
1361 (None, false, false) => run_start = Some(i),
1362 (Some(s), true, _) | (Some(s), _, true) => {
1363 let e = i;
1364 if e > s + 1 {
1365 let head_bp = if s > 0 {
1366 bp_of(branch.points[s - 1])
1367 } else {
1368 None
1369 };
1370 let tail_bp = if e < n { bp_of(branch.points[e]) } else { None };
1371 arcs.push(Arc {
1372 points: branch.points[s..e].to_vec(),
1373 on_a: branch.on_a[s..e].to_vec(),
1374 on_b: branch.on_b[s..e].to_vec(),
1375 head_bp,
1376 tail_bp,
1377 });
1378 }
1379 run_start = None;
1380 }
1381 _ => {}
1382 }
1383 }
1384 }
1385
1386 arcs.retain(|arc| arc.length() > options.chord * 10.0 && arc.points.len() >= 4);
1389 arcs.sort_by(|x, y| {
1390 y.length()
1391 .partial_cmp(&x.length())
1392 .unwrap_or(core::cmp::Ordering::Equal)
1393 });
1394 let mut kept: Vec<Arc> = Vec::new();
1395 'candidate: for arc in arcs {
1396 let n = arc.points.len();
1397 for probe in [n / 4, n / 2, 3 * n / 4] {
1398 let p = arc.points[probe];
1399 if kept.iter().any(|held| {
1400 held.points
1401 .windows(2)
1402 .any(|pair| distance_to_segment(p, pair[0], pair[1]) <= reach)
1403 }) {
1404 continue 'candidate;
1405 }
1406 }
1407 kept.push(arc);
1408 }
1409
1410 let ends: Vec<(usize, bool, usize, Vector)> = kept
1413 .iter()
1414 .enumerate()
1415 .flat_map(|(i, arc)| {
1416 [(false, arc.head_bp), (true, arc.tail_bp)]
1417 .into_iter()
1418 .filter_map(move |(tail, bp)| Some((i, tail, bp?, arc.outgoing(tail)?)))
1419 })
1420 .collect();
1421 let mut partner: Vec<Option<usize>> = vec![None; ends.len()];
1422 for bp in 0..bps.len() {
1423 loop {
1424 let mut best: Option<(usize, usize, f64)> = None;
1425 for x in 0..ends.len() {
1426 if partner[x].is_some() || ends[x].2 != bp {
1427 continue;
1428 }
1429 for y in (x + 1)..ends.len() {
1430 if partner[y].is_some() || ends[y].2 != bp {
1431 continue;
1432 }
1433 let score = -ends[x].3.dot(ends[y].3);
1434 if score > CONTINUES && best.is_none_or(|(_, _, held)| score > held) {
1435 best = Some((x, y, score));
1436 }
1437 }
1438 }
1439 let Some((x, y, _)) = best else { break };
1440 partner[x] = Some(y);
1441 partner[y] = Some(x);
1442 }
1443 }
1444
1445 let end_index = |arc: usize, tail: bool| -> Option<usize> {
1447 ends.iter().position(|e| e.0 == arc && e.1 == tail)
1448 };
1449 let mut used = vec![false; kept.len()];
1450 for start in 0..kept.len() {
1451 if used[start] {
1452 continue;
1453 }
1454 let mut first = start;
1456 let mut first_reversed = false;
1457 let mut seen_back = vec![false; kept.len()];
1458 loop {
1459 seen_back[first] = true;
1460 let Some(entry) = end_index(first, first_reversed) else {
1463 break;
1464 };
1465 let Some(p) = partner[entry] else { break };
1466 let (prev, prev_tail, _, _) = ends[p];
1467 if seen_back[prev] {
1468 break; }
1470 first = prev;
1471 first_reversed = !prev_tail;
1474 }
1475
1476 let mut points: Vec<Point> = Vec::new();
1478 let mut on_a: Vec<(f64, f64)> = Vec::new();
1479 let mut on_b: Vec<(f64, f64)> = Vec::new();
1480 let mut current = first;
1481 let mut reversed = first_reversed;
1482 let mut closed = false;
1483 loop {
1484 used[current] = true;
1485 let arc = &kept[current];
1486 type Run = (Vec<Point>, Vec<(f64, f64)>, Vec<(f64, f64)>);
1487 let (pts, pa, pb): Run = if reversed {
1488 (
1489 arc.points.iter().rev().copied().collect(),
1490 arc.on_a.iter().rev().copied().collect(),
1491 arc.on_b.iter().rev().copied().collect(),
1492 )
1493 } else {
1494 (arc.points.clone(), arc.on_a.clone(), arc.on_b.clone())
1495 };
1496 if !points.is_empty() {
1498 let joint_bp = if reversed { arc.tail_bp } else { arc.head_bp };
1499 if let Some(bp) = joint_bp {
1500 points.push(bps[bp]);
1501 on_a.push(pa[0]);
1502 on_b.push(pb[0]);
1503 }
1504 }
1505 points.extend(pts);
1506 on_a.extend(pa);
1507 on_b.extend(pb);
1508
1509 let leaving = end_index(current, !reversed);
1510 let Some(l) = leaving else { break };
1511 let Some(p) = partner[l] else { break };
1512 let (next, next_tail, _, _) = ends[p];
1513 if used[next] {
1514 closed = next == first;
1515 break;
1516 }
1517 current = next;
1518 reversed = next_tail;
1519 }
1520 if closed && points.len() > 3 {
1521 let bridge = points[0];
1522 let ba = on_a[0];
1523 let bb = on_b[0];
1524 points.push(bridge);
1525 on_a.push(ba);
1526 on_b.push(bb);
1527 }
1528 out.push(Traced {
1529 points,
1530 on_a,
1531 on_b,
1532 stopped: if closed {
1533 Stopped::Closed
1534 } else {
1535 Stopped::Stalled
1536 },
1537 });
1538 }
1539 out
1540}
1541
1542#[derive(Debug, Clone, Copy)]
1545struct Touch {
1546 point: Point,
1547 on_a: (f64, f64),
1548 on_b: (f64, f64),
1549}
1550
1551fn touching_point(
1561 a: &SurfaceGeometry,
1562 b: &SurfaceGeometry,
1563 on_a: (f64, f64),
1564 on_b: (f64, f64),
1565 tol: Tolerances,
1566) -> Option<Touch> {
1567 let eval = |x: &[f64]| -> Option<[f64; 5]> {
1568 let (ua, va) = clamp(a, x[0], x[1]);
1569 let (ub, vb) = clamp(b, x[2], x[3]);
1570 let pa = a.point_at(ua, va, tol).ok()?;
1571 let pb = b.point_at(ub, vb, tol).ok()?;
1572 let na = normal_at(a, (ua, va), tol)?;
1573 let (bu, bv) = b.d1_at(ub, vb, tol).ok()?;
1574 let (lu, lv) = (bu.magnitude(), bv.magnitude());
1575 if lu <= tol.confusion() || lv <= tol.confusion() {
1576 return None;
1577 }
1578 let gap = pa + na * x[4] - pb;
1579 Some([gap.x, gap.y, gap.z, na.dot(bu) / lu, na.dot(bv) / lv])
1580 };
1581 const STEP: f64 = 1e-7;
1584 let system = |x: &[f64; 5]| {
1585 let failed = || ([f64::INFINITY; 5], [[0.0; 5]; 5]);
1586 let Some(here) = eval(x) else {
1587 return failed();
1588 };
1589 let mut jacobian = [[0.0; 5]; 5];
1590 for k in 0..5 {
1591 let mut moved = *x;
1592 moved[k] += STEP;
1593 let Some(there) = eval(&moved) else {
1594 return failed();
1595 };
1596 for (row, (t, h)) in jacobian.iter_mut().zip(there.iter().zip(here.iter())) {
1597 row[k] = (t - h) / STEP;
1598 }
1599 }
1600 (here, jacobian)
1601 };
1602 let criteria = solve::Criteria {
1603 residual: tol.confusion() * 1e-3,
1604 step: tol.parametric() * 1e-3,
1605 max_iterations: 50,
1606 };
1607 let start = [on_a.0, on_a.1, on_b.0, on_b.1, 0.0];
1608 let (x, norm, _, _) = solve::newton_system_fixed(system, start, criteria).ok()?;
1609 if norm > tol.confusion() || x[4].abs() > tol.confusion() {
1610 return None;
1611 }
1612 let (on_a, on_b) = (clamp(a, x[0], x[1]), clamp(b, x[2], x[3]));
1613 let point = a.point_at(on_a.0, on_a.1, tol).ok()?;
1614 if b.point_at(on_b.0, on_b.1, tol).ok()?.distance(point) > tol.confusion() {
1615 return None;
1616 }
1617 Some(Touch { point, on_a, on_b })
1618}
1619
1620fn beside(surface: &SurfaceGeometry, at: (f64, f64), near: (f64, f64)) -> (f64, f64) {
1623 let ((ua, ub), (va, vb)) = surface.domain();
1624 let shift = |x: f64, to: f64, span: f64, periodic: bool| {
1625 if !periodic || !span.is_finite() || span <= 0.0 {
1626 return x;
1627 }
1628 x + ((to - x) / span).round() * span
1629 };
1630 (
1631 shift(at.0, near.0, ub - ua, surface.is_periodic_u()),
1632 shift(at.1, near.1, vb - va, surface.is_periodic_v()),
1633 )
1634}
1635
1636fn split_at_touches(
1660 found: Vec<Traced>,
1661 a: &SurfaceGeometry,
1662 b: &SurfaceGeometry,
1663 options: Marching,
1664 tol: Tolerances,
1665) -> Vec<Traced> {
1666 let reach = options.chord.max(tol.confusion()) * 60.0;
1667 let carry = reach * 40.0;
1670
1671 let mut touches: Vec<Touch> = Vec::new();
1672 for branch in &found {
1673 let n = branch.points.len();
1674 if n < 5 || !interior_is_transversal(branch, a, b, tol) {
1675 continue;
1676 }
1677 let sines: Vec<f64> = (0..n)
1678 .map(|i| crossing_sine(a, b, branch.on_a[i], branch.on_b[i], tol))
1679 .collect();
1680 for i in 0..n {
1681 let low = sines[i] < BRANCH_POINT_SINE
1682 && (i == 0 || sines[i] <= sines[i - 1])
1683 && (i + 1 == n || sines[i] <= sines[i + 1]);
1684 if !low
1685 || touches
1686 .iter()
1687 .any(|t| t.point.distance(branch.points[i]) <= reach)
1688 {
1689 continue;
1690 }
1691 let Some(touch) = touching_point(a, b, branch.on_a[i], branch.on_b[i], tol) else {
1692 continue;
1693 };
1694 if touch.point.distance(branch.points[i]) <= carry
1695 && !touches
1696 .iter()
1697 .any(|t| t.point.distance(touch.point) <= reach)
1698 {
1699 touches.push(touch);
1700 }
1701 }
1702 }
1703 if touches.is_empty() {
1704 return found;
1705 }
1706 let touch_near = |p: Point, within: f64| -> Option<usize> {
1707 touches.iter().position(|t| t.point.distance(p) <= within)
1708 };
1709
1710 let mut out = Vec::with_capacity(found.len());
1711 for branch in found {
1712 let n = branch.points.len();
1713 let mut visits: Vec<Option<usize>> = branch
1716 .points
1717 .iter()
1718 .map(|p| touch_near(*p, reach))
1719 .collect();
1720 for i in 1..n {
1721 let (p, q) = (branch.points[i - 1], branch.points[i]);
1722 if let Some(t) = touches
1723 .iter()
1724 .position(|t| distance_to_segment(t.point, p, q) <= reach)
1725 {
1726 visits[i - 1].get_or_insert(t);
1727 visits[i].get_or_insert(t);
1728 }
1729 }
1730 let stall_end = |at: usize| {
1731 branch.stopped == Stopped::Stalled && touch_near(branch.points[at], carry).is_some()
1732 };
1733 let touched =
1734 visits.iter().any(Option::is_some) || (n > 0 && (stall_end(0) || stall_end(n - 1)));
1735 if !touched || !interior_is_transversal(&branch, a, b, tol) {
1736 out.push(branch);
1737 continue;
1738 }
1739 let closed = branch.closed();
1740 let order: Vec<usize> = if closed {
1743 let last = if branch.points[0].distance(branch.points[n - 1]) <= tol.confusion() {
1744 n - 1
1745 } else {
1746 n
1747 };
1748 let Some(first) =
1749 (0..last).find(|&i| visits[i].is_some() && visits[(i + last - 1) % last].is_none())
1750 else {
1751 out.push(branch);
1752 continue;
1753 };
1754 (0..last).map(|k| (first + k) % last).collect()
1755 } else {
1756 (0..n).collect()
1757 };
1758 let mut runs: Vec<(Vec<usize>, Option<usize>, Option<usize>)> = Vec::new();
1759 let mut run: Vec<usize> = Vec::new();
1760 let mut head: Option<usize> = None;
1761 for (k, &i) in order.iter().enumerate() {
1762 if let Some(t) = visits[i] {
1763 if !run.is_empty() {
1764 runs.push((core::mem::take(&mut run), head, Some(t)));
1765 }
1766 head = Some(t);
1767 continue;
1768 }
1769 if run.is_empty() && k == 0 && !closed && stall_end(i) {
1770 head = touch_near(branch.points[i], carry);
1771 }
1772 run.push(i);
1773 }
1774 if !run.is_empty() {
1775 let last = *run.last().unwrap_or(&0);
1776 let tail = if closed {
1777 visits[order[0]]
1778 } else if stall_end(last) && last == n - 1 {
1779 touch_near(branch.points[last], carry)
1780 } else {
1781 None
1782 };
1783 runs.push((run, head, tail));
1784 }
1785 let unit = |v: Vector| {
1788 let m = v.magnitude();
1789 (m > tol.confusion()).then(|| v / m)
1790 };
1791 let pass = |before: &[usize], after: &[usize], t: usize| -> (usize, bool) {
1792 let (Some(&i), Some(&o)) = (before.last(), after.first()) else {
1793 return (t, false);
1794 };
1795 let into = unit(touches[t].point - branch.points[i]);
1796 let onward = unit(branch.points[o] - touches[t].point);
1797 (
1798 t,
1799 matches!((into, onward), (Some(x), Some(y)) if x.dot(y) > 0.5),
1800 )
1801 };
1802 let mut passes: Vec<(usize, bool)> = runs
1803 .windows(2)
1804 .filter_map(|w| Some(pass(&w[0].0, &w[1].0, w[0].2?)))
1805 .collect();
1806 if closed
1807 && let (Some(last), Some(first)) = (runs.last(), runs.first())
1808 && let Some(t) = last.2
1809 {
1810 passes.push(pass(&last.0, &first.0, t));
1811 }
1812 let mut met: Vec<usize> = passes.iter().map(|(t, _)| *t).collect();
1813 met.sort_unstable();
1814 let repeated = met.windows(2).any(|w| w[0] == w[1]);
1815 let straight = passes.iter().all(|(_, s)| *s);
1816 let stopped_short = !closed && (stall_end(0) || stall_end(n - 1));
1817 if !repeated && straight && !stopped_short {
1818 out.push(branch);
1819 continue;
1820 }
1821
1822 let mut arcs: Vec<Traced> = Vec::new();
1823 for (run, head, tail) in runs {
1824 let mut arc = Traced {
1825 points: run.iter().map(|&i| branch.points[i]).collect(),
1826 on_a: run.iter().map(|&i| branch.on_a[i]).collect(),
1827 on_b: run.iter().map(|&i| branch.on_b[i]).collect(),
1828 stopped: Stopped::Stalled,
1829 };
1830 if arc.points.len() < 2 {
1831 continue;
1832 }
1833 for (end, at_head) in [(tail, false), (head, true)] {
1834 if let Some(t) = end {
1835 carry_onto(&mut arc, &touches[t], at_head, a, b, reach, tol);
1836 }
1837 }
1838 let length: f64 = arc.points.windows(2).map(|w| w[0].distance(w[1])).sum();
1839 if length <= options.chord * 10.0 || arc.points.len() < 4 {
1840 continue;
1841 }
1842 let m = arc.points.len();
1843 if head.is_some() && head == tail && arc.points[0].distance(arc.points[m - 1]) <= reach
1844 {
1845 let middle = m * 382 / 1000;
1848 let part = |r: core::ops::RangeInclusive<usize>| Traced {
1849 points: arc.points[r.clone()].to_vec(),
1850 on_a: arc.on_a[r.clone()].to_vec(),
1851 on_b: arc.on_b[r].to_vec(),
1852 stopped: Stopped::Stalled,
1853 };
1854 arcs.push(part(0..=middle));
1855 arcs.push(part(middle..=m - 1));
1856 } else {
1857 arcs.push(arc);
1858 }
1859 }
1860 out.extend(arcs);
1861 }
1862 out
1863}
1864
1865fn carry_onto(
1870 arc: &mut Traced,
1871 touch: &Touch,
1872 at_head: bool,
1873 a: &SurfaceGeometry,
1874 b: &SurfaceGeometry,
1875 reach: f64,
1876 tol: Tolerances,
1877) {
1878 let n = arc.points.len();
1879 let (end, inner) = if at_head { (0, 1) } else { (n - 1, n - 2) };
1880 let from = arc.points[end];
1881 let gap = touch.point - from;
1882 let distance = gap.magnitude();
1883 let mut added: Vec<Contact> = Vec::new();
1884 if distance > tol.confusion() {
1885 let along = gap * (1.0 / distance);
1886 let heading = from - arc.points[inner];
1887 if distance > reach && heading.dot(along) < 0.5 * heading.magnitude() {
1888 return;
1889 }
1890 let pieces = (distance / reach).ceil().max(1.0);
1896 #[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
1897 let count = pieces as usize;
1898 #[allow(clippy::cast_precision_loss)]
1899 let mut short: Vec<f64> = (1..count)
1900 .map(|k| distance - distance * k as f64 / pieces)
1901 .collect();
1902 let mut close = short.last().copied().unwrap_or(distance).min(reach);
1903 for _ in 0..3 {
1904 close *= 0.5;
1905 short.push(close);
1906 }
1907 let (mut on_a, mut on_b) = (arc.on_a[end], arc.on_b[end]);
1908 for remaining in short {
1909 let s = distance - remaining;
1910 let guess = from + along * s;
1911 let start = [on_a.0, on_a.1, on_b.0, on_b.1];
1912 let settled = correct(a, b, start, guess, Some((from, along, s)), tol)
1913 .filter(|c| c.point.distance(guess) <= remaining * 0.5);
1914 let Some(c) = settled else {
1915 if remaining > reach {
1918 return;
1919 }
1920 continue;
1921 };
1922 on_a = beside(a, c.on_a, on_a);
1923 on_b = beside(b, c.on_b, on_b);
1924 added.push(Contact {
1925 on_a,
1926 on_b,
1927 point: c.point,
1928 });
1929 }
1930 added.push(Contact {
1931 on_a: beside(a, touch.on_a, on_a),
1932 on_b: beside(b, touch.on_b, on_b),
1933 point: touch.point,
1934 });
1935 } else {
1936 let (on_a, on_b) = (arc.on_a[end], arc.on_b[end]);
1937 arc.points[end] = touch.point;
1938 arc.on_a[end] = beside(a, touch.on_a, on_a);
1939 arc.on_b[end] = beside(b, touch.on_b, on_b);
1940 return;
1941 }
1942 if at_head {
1943 added.reverse();
1944 arc.points.splice(0..0, added.iter().map(|c| c.point));
1945 arc.on_a.splice(0..0, added.iter().map(|c| c.on_a));
1946 arc.on_b.splice(0..0, added.iter().map(|c| c.on_b));
1947 } else {
1948 arc.points.extend(added.iter().map(|c| c.point));
1949 arc.on_a.extend(added.iter().map(|c| c.on_a));
1950 arc.on_b.extend(added.iter().map(|c| c.on_b));
1951 }
1952}
1953
1954pub(crate) fn nearest_on(
1956 surface: &SurfaceGeometry,
1957 seed: (f64, f64),
1958 target: Point,
1959 tol: Tolerances,
1960) -> Option<((f64, f64), Point)> {
1961 let (mut u, mut v) = seed;
1962 for _ in 0..16 {
1963 let (u_ok, v_ok) = surface.normalize_parameters(u, v, tol).ok()?;
1964 u = u_ok;
1965 v = v_ok;
1966 let p = surface.point_at(u, v, tol).ok()?;
1967 let (su, sv) = surface.d1_at(u, v, tol).ok()?;
1968 let r = p - target;
1969 let (a11, a12, a22) = (su.dot(su), su.dot(sv), sv.dot(sv));
1970 let det = a11.mul_add(a22, -(a12 * a12));
1971 if det.abs() <= f64::MIN_POSITIVE {
1972 break;
1973 }
1974 let (b1, b2) = (-su.dot(r), -sv.dot(r));
1975 let du = b1.mul_add(a22, -(b2 * a12)) / det;
1976 let dv = a11.mul_add(b2, -(a12 * b1)) / det;
1977 u += du;
1978 v += dv;
1979 if du.hypot(dv) < 1e-14 {
1980 break;
1981 }
1982 }
1983 let (u, v) = surface.normalize_parameters(u, v, tol).ok()?;
1984 Some(((u, v), surface.point_at(u, v, tol).ok()?))
1985}
1986
1987#[cfg(test)]
1988#[allow(clippy::unwrap_used, clippy::print_stdout)]
1989mod tests {
1990 use super::*;
1991 use ogeom_geom::{CylinderSurface, PlaneSurface, SphereSurface};
1992 use ogeom_math::{Cylinder, Direction, Frame, Plane, Sphere};
1993
1994 const T: Tolerances = Tolerances::millimetres();
1995
1996 fn cylinder(origin: Point, axis: Vector, radius: f64, height: (f64, f64)) -> SurfaceGeometry {
1997 let frame = Frame::new(
1998 origin,
1999 Direction::new(axis, T).unwrap(),
2000 Direction::from_cross(axis, Vector::new(0.3, 0.5, 0.9), T).unwrap(),
2001 T,
2002 )
2003 .unwrap();
2004 CylinderSurface::new(Cylinder::new(frame, radius, T).unwrap(), height)
2005 .unwrap()
2006 .into()
2007 }
2008
2009 fn sphere(centre: Point, radius: f64) -> SurfaceGeometry {
2010 SphereSurface::new(Sphere::centred(centre, radius, T).unwrap()).into()
2011 }
2012
2013 fn plane(origin: Point, normal: Vector) -> SurfaceGeometry {
2014 PlaneSurface::over(
2015 Plane::through(origin, Direction::new(normal, T).unwrap()),
2016 (-8.0, 8.0),
2017 (-8.0, 8.0),
2018 )
2019 .unwrap()
2020 .into()
2021 }
2022
2023 fn off(surface: &SurfaceGeometry, p: Point) -> f64 {
2025 match surface {
2026 SurfaceGeometry::Plane(x) => x.plane().distance_to(p),
2027 SurfaceGeometry::Sphere(x) => x.sphere().distance_to(p),
2028 SurfaceGeometry::Cylinder(x) => x.cylinder().distance_to(p),
2029 _ => 0.0,
2030 }
2031 }
2032
2033 fn deviation(a: &SurfaceGeometry, b: &SurfaceGeometry, traced: &Traced) -> f64 {
2035 traced
2036 .points
2037 .iter()
2038 .map(|p| off(a, *p).abs().max(off(b, *p).abs()))
2039 .fold(0.0_f64, f64::max)
2040 }
2041
2042 #[test]
2043 fn a_plane_through_a_bent_strip_seeds_both_branches() {
2044 use ogeom_geom::BSplineSurface;
2050 use ogeom_math::{ControlGrid, KnotVector};
2051 let mut points = Vec::new();
2052 for i in 0..7 {
2053 let a = core::f64::consts::PI * f64::from(i) / 6.0;
2054 for j in 0..2 {
2055 points.push(Point::new(2.0 * a.cos(), f64::from(j), 2.0 * a.sin()));
2056 }
2057 }
2058 let grid = ControlGrid::new(points, 7, 2).unwrap();
2059 let strip: SurfaceGeometry = BSplineSurface::new(
2060 KnotVector::clamped_uniform(3, 7).unwrap(),
2061 KnotVector::clamped_uniform(1, 2).unwrap(),
2062 &grid,
2063 T,
2064 )
2065 .unwrap()
2066 .into();
2067 let level: SurfaceGeometry = PlaneSurface::over(
2068 Plane::through(Point::new(0.0, 0.0, 1.0), Direction::Z),
2069 (-1.0e9, 1.0e9),
2070 (-1.0e9, 1.0e9),
2071 )
2072 .unwrap()
2073 .into();
2074 let options = Marching {
2075 chord: 1e-5,
2076 ..Marching::default()
2077 };
2078 let found = branches(&strip, &level, options, T).unwrap();
2079 assert_eq!(
2080 found.len(),
2081 2,
2082 "the arch crosses the level twice: {}",
2083 found.len()
2084 );
2085 for branch in &found {
2086 assert!(!branch.closed());
2087 for p in &branch.points {
2088 assert!((p.z - 1.0).abs() < 1e-4, "on the level: {p:?}");
2089 }
2090 }
2091 }
2092
2093 #[test]
2094 fn two_crossed_cylinders_are_traced_onto_both_of_them() {
2095 let a = cylinder(Point::ORIGIN, Vector::Z, 1.0, (-4.0, 4.0));
2099 let b = cylinder(Point::ORIGIN, Vector::X, 1.0, (-4.0, 4.0));
2100 let options = Marching {
2101 chord: 1e-5,
2102 ..Marching::default()
2103 };
2104
2105 let found = branches(&a, &b, options, T).unwrap();
2110 assert_eq!(found.len(), 4, "the two ellipses' halves");
2111
2112 let touches = [Point::new(0.0, -1.0, 0.0), Point::new(0.0, 1.0, 0.0)];
2113 let mut worst = 0.0_f64;
2114 for branch in &found {
2115 let ends = [branch.points[0], branch.points[branch.points.len() - 1]];
2116 for touch in touches {
2117 assert!(
2118 ends.iter().any(|end| end.distance(touch) < 1e-9),
2119 "an arc from {:?} to {:?} misses the touch {touch:?}",
2120 ends[0],
2121 ends[1]
2122 );
2123 }
2124 assert!(
2125 branch.points.len() > 100,
2126 "a branch of only {} points",
2127 branch.points.len()
2128 );
2129 worst = worst.max(deviation(&a, &b, branch));
2130 }
2131 println!(
2132 "crossed cylinders: {} branches, worst deviation {worst:e}",
2133 found.len()
2134 );
2135 assert!(worst < 1e-7, "traced off the surfaces by {worst:e}");
2136 }
2137
2138 #[test]
2144 fn a_figure_eight_is_cut_at_its_double_point() {
2145 let radius = 4.353_623_591_855_474;
2146 let torus: SurfaceGeometry = ogeom_geom::TorusSurface::new(
2147 ogeom_math::Torus::new(Frame::WORLD, 10.0, 3.0, T).unwrap(),
2148 )
2149 .into();
2150 let frame = Frame::new(
2151 Point::new(13.0 - radius, 0.0, -10.0),
2152 Direction::Z,
2153 Direction::X,
2154 T,
2155 )
2156 .unwrap();
2157 let drill: SurfaceGeometry =
2158 CylinderSurface::new(Cylinder::new(frame, radius, T).unwrap(), (0.0, 20.0))
2159 .unwrap()
2160 .into();
2161 let options = Marching {
2162 chord: 6e-6,
2163 ..Marching::default()
2164 };
2165 let steps = 200_000;
2170 let mut expected = 0.0;
2171 let at = |t: f64| {
2172 let (x, y) = (13.0 - radius + radius * t.cos(), radius * t.sin());
2173 let off = x.hypot(y) - 10.0;
2174 ((9.0 - off * off).max(0.0).sqrt(), x, y)
2175 };
2176 for k in 0..steps {
2177 let (t0, t1) = (
2178 core::f64::consts::TAU * f64::from(k) / f64::from(steps),
2179 core::f64::consts::TAU * f64::from(k + 1) / f64::from(steps),
2180 );
2181 let ((z0, x0, y0), (z1, x1, y1)) = (at(t0), at(t1));
2182 if z0 > 0.0 || z1 > 0.0 {
2183 expected += 2.0 * Point::new(x0, y0, z0).distance(Point::new(x1, y1, z1));
2184 }
2185 }
2186 let touch = Point::new(13.0, 0.0, 0.0);
2187 for (a, b) in [(&torus, &drill), (&drill, &torus)] {
2188 let found = branches(a, b, options, T).unwrap();
2189 assert_eq!(found.len(), 4, "the figure eight's four halves");
2190 let mut length = 0.0;
2191 for branch in &found {
2192 let ends = [branch.points[0], branch.points[branch.points.len() - 1]];
2193 assert!(
2194 ends.iter().filter(|end| end.distance(touch) < 1e-9).count() == 1,
2195 "a half from {:?} to {:?} does not end once on the touch",
2196 ends[0],
2197 ends[1]
2198 );
2199 length += branch
2200 .points
2201 .windows(2)
2202 .map(|w| w[0].distance(w[1]))
2203 .sum::<f64>();
2204 assert!(deviation(a, b, branch) < 1e-6, "traced off the surfaces");
2205 }
2206 assert!(
2207 (length - expected).abs() < 1e-3,
2208 "the halves run {length} against the section's {expected}"
2209 );
2210 }
2211 }
2212
2213 #[test]
2214 fn unequal_crossed_cylinders_meet_in_two_curves_as_well() {
2215 let a = cylinder(Point::ORIGIN, Vector::Z, 1.0, (-4.0, 4.0));
2220 let b = cylinder(Point::ORIGIN, Vector::X, 1.6, (-4.0, 4.0));
2221 let options = Marching {
2222 chord: 1e-5,
2223 ..Marching::default()
2224 };
2225
2226 let found = branches(&a, &b, options, T).unwrap();
2227 assert_eq!(found.len(), 2);
2228 for branch in &found {
2229 assert!(branch.closed());
2230 assert!(deviation(&a, &b, branch) < 1e-7);
2231 }
2232 }
2233
2234 #[test]
2235 fn a_traced_circle_agrees_with_the_circle_it_should_be() {
2236 let s = sphere(Point::ORIGIN, 3.0);
2241 let cut = plane(Point::ORIGIN, Vector::Z);
2242 let options = Marching {
2243 chord: 1e-6,
2244 ..Marching::default()
2245 };
2246
2247 let found = seeds(&s, &cut, options, T).unwrap();
2248 assert!(!found.is_empty());
2249 let branch = trace(&s, &cut, found[0], options, T).unwrap();
2250
2251 assert!(
2252 branch.closed(),
2253 "a plane through a sphere gives a closed loop"
2254 );
2255 for p in &branch.points {
2256 let radius = (p.x * p.x + p.y * p.y).sqrt();
2257 assert!(
2258 (radius - 3.0).abs() < 1e-7,
2259 "a point at radius {radius} on a circle of 3"
2260 );
2261 assert!(p.z.abs() < 1e-7, "off the cutting plane by {}", p.z);
2262 }
2263 }
2264
2265 #[test]
2266 fn a_branch_that_leaves_the_surface_says_so_rather_than_stopping_quietly() {
2267 let s = sphere(Point::ORIGIN, 3.0);
2270 let cut = plane(Point::new(0.0, 0.0, 0.0), Vector::Z);
2271 let options = Marching {
2272 chord: 1e-4,
2273 max_points: 8,
2274 ..Marching::default()
2275 };
2276 let found = seeds(&s, &cut, options, T).unwrap();
2277 let branch = trace(&s, &cut, found[0], options, T).unwrap();
2278 assert_eq!(branch.stopped, Stopped::RanOut);
2279 assert!(!branch.complete(), "a truncated branch is not complete");
2280 }
2281
2282 #[test]
2283 fn tangent_surfaces_are_refused_rather_than_followed_onto_a_guess() {
2284 let s = sphere(Point::new(0.0, 0.0, 3.0), 3.0);
2288 let ground = plane(Point::ORIGIN, Vector::Z);
2289 let touch = Contact {
2290 on_a: (0.0, -core::f64::consts::FRAC_PI_2),
2291 on_b: (0.0, 0.0),
2292 point: Point::ORIGIN,
2293 };
2294 let err = trace(&s, &ground, touch, Marching::default(), T).unwrap_err();
2295 assert!(err.to_string().contains("tangent"), "unexpected: {err}");
2296 }
2297
2298 #[test]
2299 fn the_number_of_branches_is_the_number_there_are() {
2300 let options = Marching {
2304 chord: 1e-5,
2305 ..Marching::default()
2306 };
2307
2308 let one = branches(
2310 &sphere(Point::ORIGIN, 3.0),
2311 &plane(Point::new(0.0, 0.0, 1.0), Vector::Z),
2312 options,
2313 T,
2314 )
2315 .unwrap();
2316 assert_eq!(one.len(), 1, "one plane through a sphere cuts one circle");
2317 assert!(one[0].closed());
2318
2319 let two = branches(
2322 &sphere(Point::ORIGIN, 3.0),
2323 &cylinder(Point::ORIGIN, Vector::Z, 1.5, (-4.0, 4.0)),
2324 options,
2325 T,
2326 )
2327 .unwrap();
2328 assert_eq!(two.len(), 2, "a coaxial cylinder cuts a sphere twice");
2329 for branch in &two {
2330 assert!(branch.closed(), "each is a closed circle");
2331 }
2332 let heights: Vec<f64> = two.iter().map(|b| b.points[0].z).collect();
2334 assert!(
2335 heights[0] * heights[1] < 0.0,
2336 "both branches came back on the same side: {heights:?}"
2337 );
2338 }
2339
2340 #[test]
2341 fn a_branch_thinner_than_the_sampling_is_missed_and_the_knob_finds_it() {
2342 let a = sphere(Point::ORIGIN, 3.0);
2346 let b = sphere(Point::new(5.98, 0.0, 0.0), 3.0);
2347
2348 let coarse = seeds(
2349 &a,
2350 &b,
2351 Marching {
2352 grid: 6,
2353 ..Marching::default()
2354 },
2355 T,
2356 )
2357 .unwrap();
2358 let fine = seeds(
2359 &a,
2360 &b,
2361 Marching {
2362 grid: 120,
2363 ..Marching::default()
2364 },
2365 T,
2366 )
2367 .unwrap();
2368 assert!(
2369 coarse.len() < fine.len(),
2370 "a finer grid should find what a coarse one steps over: {} against {}",
2371 coarse.len(),
2372 fine.len()
2373 );
2374 assert!(!fine.is_empty(), "the branch is there to be found");
2375 }
2376
2377 #[test]
2378 fn settings_that_could_not_work_are_refused() {
2379 let a = sphere(Point::ORIGIN, 1.0);
2380 let b = plane(Point::ORIGIN, Vector::Z);
2381 for options in [
2382 Marching {
2383 chord: 0.0,
2384 ..Marching::default()
2385 },
2386 Marching {
2387 grid: 1,
2388 ..Marching::default()
2389 },
2390 Marching {
2391 max_points: 1,
2392 ..Marching::default()
2393 },
2394 ] {
2395 assert!(seeds(&a, &b, options, T).is_err());
2396 }
2397 }
2398}