1use ogeom_core::{OgeomResult, Tolerances, ogeom_bail};
36use ogeom_geom::{Surface as _, SurfaceGeometry};
37use ogeom_math::{Point, Vector, solve};
38
39use crate::march::{
40 BRANCH_POINT_SINE, Contact, Marching, Stopped, Traced, clamp, distance_to_segment, reversed,
41 span,
42};
43
44const ACCEPT: f64 = 100.0;
47
48const DISTINCT: f64 = 0.25;
51
52#[derive(Debug, Clone, Copy)]
54struct Jet {
55 p: Point,
56 du: Vector,
57 dv: Vector,
58 duu: Vector,
59 duv: Vector,
60 dvv: Vector,
61 n: Vector,
62 area: f64,
64}
65
66fn jet(surface: &SurfaceGeometry, at: (f64, f64), tol: Tolerances) -> Option<Jet> {
67 let p = surface.point_at(at.0, at.1, tol).ok()?;
68 let (du, dv) = surface.d1_at(at.0, at.1, tol).ok()?;
69 let (duu, duv, dvv) = surface.d2_at(at.0, at.1, tol).ok()?;
70 let cross = du.cross(dv);
71 let area = cross.magnitude();
72 if area.is_nan() || area <= tol.confusion() {
73 return None;
74 }
75 Some(Jet {
76 p,
77 du,
78 dv,
79 duu,
80 duv,
81 dvv,
82 n: cross * (1.0 / area),
83 area,
84 })
85}
86
87impl Jet {
88 fn lift(&self, e: Vector) -> Option<(f64, f64)> {
90 let (g11, g12, g22) = (
91 self.du.dot(self.du),
92 self.du.dot(self.dv),
93 self.dv.dot(self.dv),
94 );
95 let det = g11.mul_add(g22, -(g12 * g12));
96 if det <= f64::MIN_POSITIVE {
97 return None;
98 }
99 let (r1, r2) = (e.dot(self.du), e.dot(self.dv));
100 Some((
101 r1.mul_add(g22, -(r2 * g12)) / det,
102 g11.mul_add(r2, -(g12 * r1)) / det,
103 ))
104 }
105
106 fn second_form(&self, n: Vector, e1: Vector, e2: Vector) -> Option<[f64; 3]> {
110 let (a1, b1) = self.lift(e1)?;
111 let (a2, b2) = self.lift(e2)?;
112 let (l, m, nn) = (n.dot(self.duu), n.dot(self.duv), n.dot(self.dvv));
113 let form =
114 |a: f64, b: f64, c: f64, d: f64| a * c * l + a.mul_add(d, b * c) * m + b * d * nn;
115 Some([
116 form(a1, b1, a1, b1),
117 form(a1, b1, a2, b2),
118 form(a2, b2, a2, b2),
119 ])
120 }
121
122 fn normal_rates(&self) -> (Vector, Vector) {
124 let rate = |dn: Vector| (dn - self.n * self.n.dot(dn)) * (1.0 / self.area);
125 (
126 rate(self.duu.cross(self.dv) + self.du.cross(self.duv)),
127 rate(self.duv.cross(self.dv) + self.du.cross(self.dvv)),
128 )
129 }
130}
131
132fn aligned(ja: &Jet, jb: &Jet) -> (Vector, f64) {
134 let sign = if ja.n.dot(jb.n) < 0.0 { -1.0 } else { 1.0 };
135 (jb.n * sign, sign)
136}
137
138fn contact_direction(ja: &Jet, jb: &Jet, previous: Option<Vector>) -> Option<(Vector, f64)> {
146 let n = ja.n;
147 let (nb, _) = aligned(ja, jb);
148 let flat_u = ja.du - n * n.dot(ja.du);
149 let e1 = flat_u * (1.0 / flat_u.magnitude());
150 if !e1.x.is_finite() {
151 return None;
152 }
153 let e2 = n.cross(e1);
154 let fa = ja.second_form(n, e1, e2)?;
155 let fb = jb.second_form(nb, e1, e2)?;
156 let (p, q, r) = (fa[0] - fb[0], fa[1] - fb[1], fa[2] - fb[2]);
157 let half_sum = f64::midpoint(p, r);
158 let radius = ((p - r) * 0.5).hypot(q);
159 let (low, high) = (half_sum - radius, half_sum + radius);
160 let (small, large) = if low.abs() <= high.abs() {
161 (low, high)
162 } else {
163 (high, low)
164 };
165 let carried = previous.and_then(|d| {
166 let flat = d - n * n.dot(d);
167 let m = flat.magnitude();
168 (m > f64::MIN_POSITIVE).then(|| flat * (1.0 / m))
169 });
170 let direction = if large.abs() > f64::MIN_POSITIVE && small.abs() <= DISTINCT * large.abs() {
171 let first = (q, small - p);
174 let second = (small - r, q);
175 let (x, y) = if first.0.hypot(first.1) >= second.0.hypot(second.1) {
176 first
177 } else {
178 second
179 };
180 let m = x.hypot(y);
181 if m <= f64::MIN_POSITIVE {
182 return None;
183 }
184 let t = e1 * (x / m) + e2 * (y / m);
185 match carried {
186 Some(d) if d.dot(t) < 0.0 => -t,
187 _ => t,
188 }
189 } else {
190 carried?
191 };
192 let (x, y) = (direction.dot(e1), direction.dot(e2));
193 let bend = (fa[0] * x).mul_add(x, (2.0 * fa[1] * x).mul_add(y, fa[2] * y * y));
194 Some((direction, bend))
195}
196
197fn form_difference(ja: &Jet, jb: &Jet) -> Option<[(f64, Vector); 2]> {
202 let n = ja.n;
203 let (nb, _) = aligned(ja, jb);
204 let flat_u = ja.du - n * n.dot(ja.du);
205 let e1 = flat_u * (1.0 / flat_u.magnitude());
206 if !e1.x.is_finite() {
207 return None;
208 }
209 let e2 = n.cross(e1);
210 let fa = ja.second_form(n, e1, e2)?;
211 let fb = jb.second_form(nb, e1, e2)?;
212 let (p, q, r) = (fa[0] - fb[0], fa[1] - fb[1], fa[2] - fb[2]);
213 let half_sum = f64::midpoint(p, r);
214 let radius = ((p - r) * 0.5).hypot(q);
215 let (low, high) = (half_sum - radius, half_sum + radius);
216 let (small, large) = if low.abs() <= high.abs() {
217 (low, high)
218 } else {
219 (high, low)
220 };
221 let along = |value: f64| -> Option<Vector> {
222 let first = (q, value - p);
223 let second = (value - r, q);
224 let (x, y) = if first.0.hypot(first.1) >= second.0.hypot(second.1) {
225 first
226 } else {
227 second
228 };
229 let m = x.hypot(y);
230 (m > f64::MIN_POSITIVE).then(|| e1 * (x / m) + e2 * (y / m))
231 };
232 let weak = along(small)?;
233 Some([(small, weak), (large, n.cross(weak))])
234}
235
236fn shape_bounds(surface: &SurfaceGeometry, near: Point, within: f64) -> Option<(f64, f64)> {
263 let from_axis = |frame: ogeom_math::Frame| {
264 let d = near - frame.origin();
265 let z = frame.z().vector();
266 (d - z * d.dot(z)).magnitude()
267 };
268 match surface {
269 SurfaceGeometry::Plane(_) => Some((0.0, 0.0)),
270 SurfaceGeometry::Sphere(s) => {
271 let k = 1.0 / s.sphere().radius();
272 Some((k, k * k))
273 }
274 SurfaceGeometry::Cylinder(c) => {
275 let k = 1.0 / c.cylinder().radius();
276 Some((k, k * k))
277 }
278 SurfaceGeometry::Cone(c) => {
279 let cone = c.cone();
280 let rho = from_axis(cone.frame()) - within;
281 let (sine, cosine) = cone.half_angle().sin_cos();
282 (rho > 0.0).then(|| {
283 let k = cosine.abs() / rho;
284 (
285 k,
286 cosine.abs() * sine.mul_add(sine, 1.0).sqrt() / (rho * rho),
287 )
288 })
289 }
290 SurfaceGeometry::Torus(t) => {
291 let torus = t.torus();
292 let (big, small) = (torus.major_radius(), torus.minor_radius());
293 let rho = (from_axis(torus.frame()) - within).max(big - small);
294 (big > small && rho > 0.0).then(|| {
295 let k = (1.0 / small).max(1.0 / rho);
296 let meridian = (1.0 / (small * small)).max(big / (small * rho * rho));
297 let parallel = ((1.0 / small + 1.0 / rho) / rho).hypot(1.0 / (rho * rho));
298 (k, meridian.hypot(parallel))
299 })
300 }
301 SurfaceGeometry::Trimmed(t) => shape_bounds(t.basis(), near, within),
302 _ => None,
303 }
304}
305
306#[derive(Debug, Clone, Copy)]
308pub(crate) struct Touch {
309 pub at: Contact,
311 pub reach: f64,
314 pub walks: bool,
317}
318
319fn names_none(small: f64, large: f64) -> bool {
322 !(large.abs() > f64::MIN_POSITIVE && small.abs() <= DISTINCT * large.abs())
323}
324
325pub(crate) fn isolated_touch(
365 a: &SurfaceGeometry,
366 b: &SurfaceGeometry,
367 seed: Contact,
368 options: Marching,
369 tol: Tolerances,
370) -> Option<Touch> {
371 let eps = tol.confusion();
372 let (ja, jb) = (jet(a, seed.on_a, tol)?, jet(b, seed.on_b, tol)?);
373 if ja.n.cross(jb.n).magnitude() > BRANCH_POINT_SINE {
374 return None;
375 }
376 let (ua, va, ub, vb) = crate::extrema::stationary_surface_surface(
377 a,
378 b,
379 seed.on_a,
380 seed.on_b,
381 crate::march::STATIONARY_STEPS,
382 tol,
383 )?;
384 let (ta, tb) = (jet(a, (ua, va), tol)?, jet(b, (ub, vb), tol)?);
385 if ta.p.distance(tb.p) > eps {
386 return None;
387 }
388 let [(small, _), (large, _)] = form_difference(&ta, &tb)?;
389 let kappa = small.abs();
390 if small * large <= 0.0 || kappa <= f64::MIN_POSITIVE {
391 return None;
392 }
393 let sine = ta.n.cross(tb.n).magnitude();
394 if sine * sine > 2.0 * kappa * eps {
395 return None;
396 }
397 let reach = (4.0 * core::f64::consts::SQRT_2 + 56.0_f64.sqrt()) * (eps / kappa).sqrt();
398 let ball = reach + crate::march::joining_reach(options, tol);
399 let (ka, la) = shape_bounds(a, ta.p, ball)?;
400 let (kb, lb) = shape_bounds(b, tb.p, ball)?;
401 let k = ka.max(kb);
402 let along = |rho: f64| -> Option<f64> {
403 let length = if k > 0.0 {
404 2.0 / k * (k * rho * 0.5).min(1.0).asin()
405 } else {
406 rho
407 };
408 (k * length <= 0.5).then_some(length)
409 };
410 let far = (la + lb).mul_add(along(ball)?, 2.0 * large.abs() * ka * along(ball)?);
411 let tangential = (ka + kb) * along(reach)? <= crate::march::TANGENTIAL_SINE;
412 if far > kappa / 2.0 || !tangential || ta.p.distance(seed.point) > reach {
413 return None;
414 }
415 Some(Touch {
416 at: Contact {
417 point: ta.p.midpoint(tb.p),
418 on_a: (ua, va),
419 on_b: (ub, vb),
420 },
421 reach,
422 walks: !names_none(small, large),
423 })
424}
425
426#[derive(Debug, Clone, Copy)]
429enum Pin {
430 Step {
431 anchor: Point,
432 along: Vector,
433 reach: f64,
434 },
435 Bound {
436 index: usize,
437 value: f64,
438 },
439}
440
441#[derive(Debug, Clone, Copy)]
444struct Corrected {
445 raw: [f64; 4],
446 contact: Contact,
447 gap: f64,
448 sine: f64,
449}
450
451fn correct(
454 a: &SurfaceGeometry,
455 b: &SurfaceGeometry,
456 start: [f64; 4],
457 across: Vector,
458 pin: Pin,
459 tol: Tolerances,
460) -> Option<Corrected> {
461 let system = |x: &[f64; 4]| {
462 let (ua, va) = clamp(a, x[0], x[1]);
463 let (ub, vb) = clamp(b, x[2], x[3]);
464 let (Some(ja), Some(jb)) = (jet(a, (ua, va), tol), jet(b, (ub, vb), tol)) else {
465 return ([f64::INFINITY; 4], [[0.0; 4]; 4]);
466 };
467 let d = ja.p - jb.p;
468 let (lu, lv) = (ja.du.magnitude(), ja.dv.magnitude());
469 let (nb, sign) = aligned(&ja, &jb);
470 let (na_u, na_v) = ja.normal_rates();
471 let (nb_u, nb_v) = jb.normal_rates();
472 let (last, last_row) = match pin {
473 Pin::Step {
474 anchor,
475 along,
476 reach,
477 } => (
478 (ja.p - anchor).dot(along) - reach,
479 [ja.du.dot(along), ja.dv.dot(along), 0.0, 0.0],
480 ),
481 Pin::Bound { index, value } => {
482 let mut row = [0.0; 4];
483 row[index] = 1.0;
484 (x[index] - value, row)
485 }
486 };
487 let residual = [
488 d.dot(ja.du) / lu,
489 d.dot(ja.dv) / lv,
490 (nb - ja.n).dot(across),
491 last,
492 ];
493 let jacobian = [
494 [
495 (ja.du.dot(ja.du) + d.dot(ja.duu)) / lu,
496 (ja.dv.dot(ja.du) + d.dot(ja.duv)) / lu,
497 -jb.du.dot(ja.du) / lu,
498 -jb.dv.dot(ja.du) / lu,
499 ],
500 [
501 (ja.du.dot(ja.dv) + d.dot(ja.duv)) / lv,
502 (ja.dv.dot(ja.dv) + d.dot(ja.dvv)) / lv,
503 -jb.du.dot(ja.dv) / lv,
504 -jb.dv.dot(ja.dv) / lv,
505 ],
506 [
507 -na_u.dot(across),
508 -na_v.dot(across),
509 nb_u.dot(across) * sign,
510 nb_v.dot(across) * sign,
511 ],
512 last_row,
513 ];
514 (residual, jacobian)
515 };
516 let criteria = solve::Criteria {
517 residual: tol.confusion() * 1e-3,
518 step: tol.parametric() * 1e-3,
519 max_iterations: 40,
520 };
521 let (raw, ..) = solve::newton_system_fixed(system, start, criteria).ok()?;
522 let on_a = clamp(a, raw[0], raw[1]);
523 let on_b = clamp(b, raw[2], raw[3]);
524 let ja = jet(a, on_a, tol)?;
525 let jb = jet(b, on_b, tol)?;
526 Some(Corrected {
527 raw,
528 contact: Contact {
529 on_a,
530 on_b,
531 point: ja.p,
532 },
533 gap: ja.p.distance(jb.p),
534 sine: ja.n.cross(jb.n).magnitude(),
535 })
536}
537
538fn crossed_bound(
542 a: &SurfaceGeometry,
543 b: &SurfaceGeometry,
544 from: [f64; 4],
545 to: [f64; 4],
546 tol: Tolerances,
547) -> Option<(usize, f64)> {
548 let mut first: Option<(f64, usize, f64)> = None;
549 for (surface, offset) in [(a, 0_usize), (b, 2)] {
550 let ((ua, ub), (va, vb)) = surface.domain();
551 for (k, (lo, hi), periodic) in [
552 (offset, (ua, ub), surface.is_periodic_u()),
553 (offset + 1, (va, vb), surface.is_periodic_v()),
554 ] {
555 if periodic || !(hi - lo).is_finite() {
556 continue;
557 }
558 let slack = tol.parametric();
559 let bound = if to[k] < lo - slack {
560 lo
561 } else if to[k] > hi + slack {
562 hi
563 } else {
564 continue;
565 };
566 let moved = to[k] - from[k];
567 let fraction = if moved.abs() > f64::MIN_POSITIVE {
568 ((bound - from[k]) / moved).clamp(0.0, 1.0)
569 } else {
570 0.0
571 };
572 if first.is_none_or(|(f, ..)| fraction < f) {
573 first = Some((fraction, k, bound));
574 }
575 }
576 }
577 first.map(|(_, k, bound)| (k, bound))
578}
579
580fn predicted(ja: &Jet, jb: &Jet, state: [f64; 4], by: Vector) -> [f64; 4] {
584 match (ja.lift(by), jb.lift(by)) {
585 (Some((da, ea)), Some((db, eb))) => {
586 [state[0] + da, state[1] + ea, state[2] + db, state[3] + eb]
587 }
588 _ => state,
589 }
590}
591
592struct Walked {
594 points: Vec<Point>,
595 on_a: Vec<(f64, f64)>,
596 on_b: Vec<(f64, f64)>,
597 stopped: Stopped,
598}
599
600struct Walk<'s> {
602 a: &'s SurfaceGeometry,
603 b: &'s SurfaceGeometry,
604 options: Marching,
605 accept: f64,
606 longest: f64,
607 shortest: f64,
608 tol: Tolerances,
609}
610
611impl Walk<'_> {
612 fn holds(&self, c: &Corrected) -> bool {
614 c.gap <= self.accept && c.sine <= BRANCH_POINT_SINE
615 }
616
617 fn step_for(&self, curvature: f64) -> f64 {
619 let curvature = curvature
620 .abs()
621 .max(1.0 / self.longest.max(f64::MIN_POSITIVE));
622 (8.0 * self.options.chord / curvature)
623 .sqrt()
624 .clamp(self.shortest, self.longest)
625 }
626
627 fn closes(
639 &self,
640 at: Contact,
641 (ja, jb): (&Jet, &Jet),
642 along: Vector,
643 step: f64,
644 from: Point,
645 ) -> bool {
646 let reach = (from - at.point).dot(along);
647 if reach <= 0.0 || reach > step {
648 return false;
649 }
650 let state = [at.on_a.0, at.on_a.1, at.on_b.0, at.on_b.1];
651 let across = ja.n.cross(along);
652 let land = |reach: f64| {
653 let pin = Pin::Step {
654 anchor: at.point,
655 along,
656 reach,
657 };
658 let guess = predicted(ja, jb, state, along * reach);
659 correct(self.a, self.b, guess, across, pin, self.tol)
660 .filter(|c| self.holds(c))
661 .map(|c| c.contact.point)
662 };
663 let near = self.options.chord.max(self.tol.confusion());
664 land(reach).is_some_and(|landed| landed.distance(from) <= near)
665 && land(reach * 0.5)
666 .is_some_and(|middle| distance_to_segment(middle, at.point, from) <= near)
667 }
668
669 fn one_way(&self, from: Contact, sense: f64) -> OgeomResult<Walked> {
670 let (a, b, tol) = (self.a, self.b, self.tol);
671 let mut out = Walked {
672 points: vec![from.point],
673 on_a: vec![from.on_a],
674 on_b: vec![from.on_b],
675 stopped: Stopped::RanOut,
676 };
677 let mut at = from;
678 let mut previous: Option<(Vector, f64)> = None;
679 let mut travelled = 0.0;
680 while out.points.len() < self.options.max_points {
681 ogeom_core::progress::checkpoint()?;
682 let (Some(ja), Some(jb)) = (jet(a, at.on_a, tol), jet(b, at.on_b, tol)) else {
683 out.stopped = Stopped::Stalled;
684 break;
685 };
686 let Some((mut along, bend)) = contact_direction(&ja, &jb, previous.map(|(d, _)| d))
687 else {
688 out.stopped = Stopped::Stalled;
689 break;
690 };
691 if previous.is_none() {
692 along *= sense;
693 }
694 let mut step = match previous {
698 Some((d, last)) => {
699 let turning = d.cross(along).magnitude().atan2(d.dot(along)) / last;
700 self.step_for(bend.abs().max(turning)).min(last * 2.0)
701 }
702 None => self.step_for(bend),
703 };
704 let across = ja.n.cross(along);
705 let state = [at.on_a.0, at.on_a.1, at.on_b.0, at.on_b.1];
706 let next = loop {
707 let guess = predicted(&ja, &jb, state, along * step);
708 let pin = Pin::Step {
709 anchor: at.point,
710 along,
711 reach: step,
712 };
713 if let Some(c) = correct(a, b, guess, across, pin, tol) {
714 if let Some((index, value)) = crossed_bound(a, b, state, c.raw, tol) {
715 let pin = Pin::Bound { index, value };
721 if let Some(edge) = correct(a, b, state, across, pin, tol)
722 && self.holds(&edge)
723 && (edge.contact.point - at.point).dot(along) > -tol.confusion()
724 && edge.contact.point.distance(at.point) <= step * 2.0
725 {
726 break Err(Some(edge.contact));
727 }
728 } else if self.holds(&c) && c.contact.point.distance(at.point) > step * 0.5 {
729 break Ok(c.contact);
730 }
731 }
732 step *= 0.5;
733 if step < self.shortest {
734 break Err(None);
735 }
736 };
737 let next = match next {
738 Ok(next) => next,
739 Err(Some(edge)) => {
740 let taken = edge.point.distance(at.point);
741 if taken <= tol.confusion() {
742 let last = out.points.len() - 1;
743 out.points[last] = edge.point;
744 out.on_a[last] = edge.on_a;
745 out.on_b[last] = edge.on_b;
746 } else {
747 out.points.push(edge.point);
748 out.on_a.push(edge.on_a);
749 out.on_b.push(edge.on_b);
750 }
751 out.stopped = Stopped::LeftTheDomain;
752 break;
753 }
754 Err(None) => {
755 out.stopped = Stopped::Stalled;
756 break;
757 }
758 };
759 let taken = next.point.distance(at.point);
760 if out.points.len() > 3
764 && travelled > taken * 2.0
765 && at.point.distance(from.point) <= step * 2.0
766 && self.closes(at, (&ja, &jb), along, step, from.point)
767 {
768 out.points.push(from.point);
769 out.on_a.push(from.on_a);
770 out.on_b.push(from.on_b);
771 out.stopped = Stopped::Closed;
772 break;
773 }
774 travelled += taken;
775 previous = Some((next.point - at.point, taken));
776 out.points.push(next.point);
777 out.on_a.push(next.on_a);
778 out.on_b.push(next.on_b);
779 at = next;
780 }
781 Ok(out)
782 }
783}
784
785pub fn trace_tangential(
803 a: &SurfaceGeometry,
804 b: &SurfaceGeometry,
805 from: Contact,
806 options: Marching,
807 tol: Tolerances,
808) -> OgeomResult<Traced> {
809 options.validate()?;
810 let (Some(ja), Some(jb)) = (jet(a, from.on_a, tol), jet(b, from.on_b, tol)) else {
811 ogeom_bail!(
812 Construction,
813 "the seed cannot be evaluated on both surfaces"
814 );
815 };
816 let sine = ja.n.cross(jb.n).magnitude();
817 if sine > BRANCH_POINT_SINE {
818 ogeom_bail!(
819 Construction,
820 "the surfaces cross here at sine {sine}; tangential tracing wants a contact"
821 );
822 }
823 let longest = span(a).max(span(b)) / 16.0;
824 let walk = Walk {
825 a,
826 b,
827 options,
828 accept: tol.confusion() * ACCEPT,
829 longest,
830 shortest: (longest * 1e-6).max(tol.confusion() * 10.0),
831 tol,
832 };
833 let Some((along, _)) = contact_direction(&ja, &jb, None) else {
834 ogeom_bail!(
835 Construction,
836 "the surfaces touch at an isolated point here, or osculate in every \
837 direction: the contact has no direction to follow"
838 );
839 };
840 let start = [from.on_a.0, from.on_a.1, from.on_b.0, from.on_b.1];
841 let pin = Pin::Step {
842 anchor: from.point,
843 along,
844 reach: 0.0,
845 };
846 let seed = match correct(a, b, start, ja.n.cross(along), pin, tol) {
847 Some(c) if walk.holds(&c) => c.contact,
848 Some(c) => ogeom_bail!(
849 Construction,
850 "the surfaces come within {} of each other here, at normal sine {}, \
851 without touching",
852 c.gap,
853 c.sine
854 ),
855 None => ogeom_bail!(Construction, "the seed does not correct onto a contact"),
856 };
857
858 let ahead = walk.one_way(seed, 1.0)?;
859 if ahead.stopped == Stopped::Closed {
860 return Ok(Traced {
861 points: ahead.points,
862 on_a: ahead.on_a,
863 on_b: ahead.on_b,
864 stopped: Stopped::Closed,
865 });
866 }
867 let behind = walk.one_way(seed, -1.0)?;
868 if behind.stopped == Stopped::Closed {
871 return Ok(reversed(Traced {
872 points: behind.points,
873 on_a: behind.on_a,
874 on_b: behind.on_b,
875 stopped: Stopped::Closed,
876 }));
877 }
878 let mut points = behind.points;
879 let mut on_a = behind.on_a;
880 let mut on_b = behind.on_b;
881 points.reverse();
882 on_a.reverse();
883 on_b.reverse();
884 points.pop();
885 on_a.pop();
886 on_b.pop();
887 points.extend(ahead.points);
888 on_a.extend(ahead.on_a);
889 on_b.extend(ahead.on_b);
890 let mut stopped = if ahead.stopped == Stopped::RanOut || behind.stopped == Stopped::RanOut {
891 Stopped::RanOut
892 } else if ahead.stopped == Stopped::Stalled || behind.stopped == Stopped::Stalled {
893 Stopped::Stalled
894 } else {
895 Stopped::LeftTheDomain
896 };
897 if stopped == Stopped::LeftTheDomain && points.len() > 3 {
903 let gap = points[0].distance(points[points.len() - 1]);
904 if gap <= options.chord.max(tol.confusion() * 10.0) {
905 if gap <= tol.confusion() {
906 points.pop();
907 on_a.pop();
908 on_b.pop();
909 }
910 points.push(points[0]);
911 on_a.push(on_a[0]);
912 on_b.push(on_b[0]);
913 stopped = Stopped::Closed;
914 }
915 }
916 Ok(Traced {
917 points,
918 on_a,
919 on_b,
920 stopped,
921 })
922}
923
924#[cfg(test)]
925#[allow(clippy::unwrap_used)]
926mod tests {
927 use super::*;
928 use ogeom_geom::{CylinderSurface, PlaneSurface, SphereSurface, TorusSurface};
929 use ogeom_math::{Cylinder, Direction, Frame, Plane, Sphere, Torus};
930
931 const T: Tolerances = Tolerances::millimetres();
932
933 fn ball(radius: f64) -> SurfaceGeometry {
934 SphereSurface::new(Sphere::centred(Point::ORIGIN, radius, T).unwrap()).into()
935 }
936
937 fn locate(surface: &SurfaceGeometry, p: Point) -> (f64, f64) {
940 let ((ua, ub), (va, vb)) = surface.domain();
941 let mut best = ((ua, va), f64::INFINITY);
942 for i in 0..=32 {
943 for j in 0..=32 {
944 let u = ua + (ub - ua) * f64::from(i) / 32.0;
945 let v = va + (vb - va) * f64::from(j) / 32.0;
946 let d = surface.point_at(u, v, T).unwrap().distance(p);
947 if d < best.1 {
948 best = ((u, v), d);
949 }
950 }
951 }
952 crate::march::nearest_on(surface, best.0, p, T).unwrap().0
953 }
954
955 fn seed(a: &SurfaceGeometry, b: &SurfaceGeometry, p: Point) -> Contact {
956 Contact {
957 on_a: locate(a, p),
958 on_b: locate(b, p),
959 point: p,
960 }
961 }
962
963 #[test]
964 fn a_ball_on_a_plane_touches_at_a_point_and_is_refused() {
965 let floor: SurfaceGeometry = PlaneSurface::over(
968 Plane::through(
969 Point::new(0.6, 0.0, 0.8),
970 Direction::new(Vector::new(0.6, 0.0, 0.8), T).unwrap(),
971 ),
972 (-4.0, 4.0),
973 (-4.0, 4.0),
974 )
975 .unwrap()
976 .into();
977 let ball = ball(1.0);
978 let at = Point::new(0.6, 0.0, 0.8);
979 let err = trace_tangential(
980 &floor,
981 &ball,
982 seed(&floor, &ball, at),
983 Marching::default(),
984 T,
985 )
986 .unwrap_err();
987 assert!(err.to_string().contains("isolated point"), "{err}");
988 }
989
990 fn torus_touching(drum: f64) -> (SurfaceGeometry, SurfaceGeometry, Point) {
994 let wall: SurfaceGeometry =
995 CylinderSurface::new(Cylinder::new(Frame::WORLD, drum, T).unwrap(), (0.0, 10.0))
996 .unwrap()
997 .into();
998 let at = Frame::new(Point::new(9.0, 0.0, 6.0), Direction::Z, Direction::X, T).unwrap();
999 let band: SurfaceGeometry = TorusSurface::new(Torus::new(at, 2.0, 1.0, T).unwrap()).into();
1000 (wall, band, Point::new(drum, 0.0, 6.0))
1001 }
1002
1003 #[test]
1004 fn a_touch_whose_second_forms_differ_definitely_is_isolated_and_decided_there() {
1005 let fine = Marching {
1014 chord: 1e-5,
1015 ..Marching::default()
1016 };
1017 for (drum, kappa) in [(12.0, 0.25), (6.0, 0.5)] {
1018 let (wall, band, at) = torus_touching(drum);
1019 let from = seed(&wall, &band, at);
1020 assert!(isolated_touch(&wall, &band, from, Marching::default(), T).is_none());
1021 let touch = isolated_touch(&wall, &band, from, fine, T).unwrap();
1022 assert!(touch.at.point.distance(at) < 1e-6, "{:?}", touch.at.point);
1023 let reach = (4.0 * core::f64::consts::SQRT_2 + 56.0_f64.sqrt())
1024 * (T.confusion() / kappa).sqrt();
1025 assert!(
1026 (touch.reach - reach).abs() < 1e-6 * reach,
1027 "{}",
1028 touch.reach
1029 );
1030 let walked = trace_tangential(&wall, &band, touch.at, fine, T);
1031 assert_eq!(touch.walks, walked.is_ok(), "drum {drum}: {walked:?}");
1032 if drum < 10.0 {
1033 assert!(!touch.walks);
1034 }
1035 for p in walked.map(|w| w.points).unwrap_or_default() {
1036 for s in [&wall, &band] {
1037 let off = ogeom_geom::project_on_surface(s, p, 16, T)
1038 .unwrap()
1039 .distance;
1040 assert!(off <= T.confusion() * ACCEPT, "{off}");
1041 }
1042 }
1043 }
1044 let at = Point::new(0.6, 0.0, 0.8);
1047 let floor: SurfaceGeometry = PlaneSurface::over(
1048 Plane::through(at, Direction::new(Vector::new(0.6, 0.0, 0.8), T).unwrap()),
1049 (-4.0, 4.0),
1050 (-4.0, 4.0),
1051 )
1052 .unwrap()
1053 .into();
1054 let ball = ball(1.0);
1055 let touch = isolated_touch(&floor, &ball, seed(&floor, &ball, at), fine, T).unwrap();
1056 assert!(!touch.walks);
1057 }
1058
1059 #[test]
1060 fn a_plane_cutting_a_ball_shallowly_is_a_crossing_not_a_touch() {
1061 let ball: SurfaceGeometry =
1066 SphereSurface::new(Sphere::centred(Point::ORIGIN, 3.0, T).unwrap()).into();
1067 let lid: SurfaceGeometry = PlaneSurface::over(
1068 Plane::through(Point::new(0.0, 0.0, 2.999), Direction::Z),
1069 (-4.0, 4.0),
1070 (-4.0, 4.0),
1071 )
1072 .unwrap()
1073 .into();
1074 let rim = (3.0_f64 * 3.0 - 2.999 * 2.999).sqrt();
1075 let at = Point::new(rim * 0.6, rim * 0.8, 2.999);
1076 let fine = Marching {
1077 chord: 1e-5,
1078 ..Marching::default()
1079 };
1080 assert!(isolated_touch(&ball, &lid, seed(&ball, &lid, at), fine, T).is_none());
1081 }
1082
1083 #[test]
1084 fn a_drum_lying_on_a_plane_touches_along_a_line_not_at_a_point() {
1085 let floor: SurfaceGeometry = PlaneSurface::over(
1088 Plane::through(Point::ORIGIN, Direction::Z),
1089 (-8.0, 8.0),
1090 (-8.0, 8.0),
1091 )
1092 .unwrap()
1093 .into();
1094 let along = Frame::new(Point::new(0.0, 0.0, 1.0), Direction::X, Direction::Y, T).unwrap();
1095 let drum: SurfaceGeometry =
1096 CylinderSurface::new(Cylinder::new(along, 1.0, T).unwrap(), (-3.0, 3.0))
1097 .unwrap()
1098 .into();
1099 let at = Point::new(0.5, 0.0, 0.0);
1100 assert!(
1101 isolated_touch(
1102 &floor,
1103 &drum,
1104 seed(&floor, &drum, at),
1105 Marching {
1106 chord: 1e-5,
1107 ..Marching::default()
1108 },
1109 T
1110 )
1111 .is_none()
1112 );
1113 use ogeom_geom::Curve3d as _;
1115 let crate::SurfaceIntersection::Along(curves) =
1116 crate::intersect_surfaces(&floor, &drum, crate::IntersectOptions::default(), T)
1117 .unwrap()
1118 else {
1119 panic!("a drum resting on a plane meets it along no curve");
1120 };
1121 assert_eq!(curves.len(), 1);
1122 let (t0, t1) = curves[0].curve.domain();
1123 for k in 0..=8 {
1124 let p = curves[0]
1125 .curve
1126 .point_at(t0 + (t1 - t0) * f64::from(k) / 8.0, T)
1127 .unwrap();
1128 assert!(
1129 p.y.abs() <= T.confusion() && p.z.abs() <= T.confusion(),
1130 "{p:?}"
1131 );
1132 }
1133 }
1134
1135 #[test]
1136 fn a_ball_short_of_its_drum_is_refused_with_the_gap() {
1137 let drum: SurfaceGeometry =
1140 CylinderSurface::new(Cylinder::new(Frame::WORLD, 1.001, T).unwrap(), (-2.0, 2.0))
1141 .unwrap()
1142 .into();
1143 let ball = ball(1.0);
1144 let at = Point::new(1.0005, 0.0, 0.0);
1145 let err = trace_tangential(&drum, &ball, seed(&drum, &ball, at), Marching::default(), T)
1146 .unwrap_err();
1147 assert!(err.to_string().contains("without touching"), "{err}");
1148 }
1149
1150 fn ramp(rise: f64) -> SurfaceGeometry {
1159 use ogeom_geom::BSplineSurface;
1160 use ogeom_math::{ControlGrid, KnotVector, Weighted};
1161 let corners = [
1162 (1.0, 0.0),
1163 (1.0, 1.0),
1164 (0.0, 1.0),
1165 (-1.0, 1.0),
1166 (-1.0, 0.0),
1167 (-1.0, -1.0),
1168 (0.0, -1.0),
1169 (1.0, -1.0),
1170 (1.0, 0.0),
1171 ];
1172 let mut grid = Vec::new();
1173 for (i, (x, y)) in corners.into_iter().enumerate() {
1174 let weight = if i % 2 == 0 {
1175 1.0
1176 } else {
1177 core::f64::consts::FRAC_1_SQRT_2
1178 };
1179 #[allow(clippy::cast_precision_loss)]
1180 let on = Point::new(x, y, rise * i as f64 / 8.0);
1181 let rule = Vector::new(-y, x, 1.0);
1182 for v in [-0.5, 0.5] {
1183 grid.push(Weighted::new(on + rule * v, weight, T).unwrap());
1184 }
1185 }
1186 BSplineSurface::rational(
1187 KnotVector::new(
1188 vec![
1189 0.0, 0.0, 0.0, 0.25, 0.25, 0.5, 0.5, 0.75, 0.75, 1.0, 1.0, 1.0,
1190 ],
1191 2,
1192 )
1193 .unwrap(),
1194 KnotVector::new(vec![-0.5, -0.5, 0.5, 0.5], 1).unwrap(),
1195 ControlGrid::new(grid, 9, 2).unwrap(),
1196 )
1197 .unwrap()
1198 .into()
1199 }
1200
1201 fn off_contact(strip: &SurfaceGeometry, p: Point) -> f64 {
1206 let tau = core::f64::consts::TAU;
1207 let on = |u: f64| strip.point_at(u, 0.0, T).unwrap();
1208 let angle = p.y.atan2(p.x).rem_euclid(tau);
1209 [angle, angle + tau]
1210 .into_iter()
1211 .filter(|a| *a <= tau)
1212 .map(|a| {
1213 let (mut lo, mut hi) = (0.0, 1.0);
1214 for _ in 0..60 {
1215 let u = f64::midpoint(lo, hi);
1216 let q = on(u);
1217 if q.y.atan2(q.x).rem_euclid(tau) < a {
1218 lo = u;
1219 } else {
1220 hi = u;
1221 }
1222 }
1223 p.distance(on(f64::midpoint(lo, hi)))
1224 })
1225 .fold(f64::INFINITY, f64::min)
1226 }
1227
1228 #[test]
1237 fn a_contact_closes_only_through_a_step_it_solved() {
1238 let chord = 1e-4;
1239 let options = Marching {
1240 chord,
1241 ..Marching::default()
1242 };
1243 let drum: SurfaceGeometry =
1244 CylinderSurface::new(Cylinder::new(Frame::WORLD, 1.0, T).unwrap(), (-2.0, 2.0))
1245 .unwrap()
1246 .into();
1247 let starts = [0.0, 1e-4, 0.3, 0.9999, 1.0];
1248 for (rise, closed) in [(0.004, false), (0.0, true)] {
1249 let strip = ramp(rise);
1250 for u in starts {
1251 let p = strip.point_at(u, 0.0, T).unwrap();
1252 let from = Contact {
1253 on_a: locate(&drum, p),
1254 on_b: (u, 0.0),
1255 point: p,
1256 };
1257 let traced = trace_tangential(&drum, &strip, from, options, T).unwrap();
1258 if closed {
1259 assert_eq!(
1260 traced.stopped,
1261 Stopped::Closed,
1262 "from {u}: a contact met across the seam is a loop"
1263 );
1264 } else {
1265 assert_eq!(
1266 traced.stopped,
1267 Stopped::LeftTheDomain,
1268 "from {u}: a contact broken at the seam is not a loop"
1269 );
1270 let (first, last) = (traced.on_b[0].0, traced.on_b[traced.on_b.len() - 1].0);
1271 assert!(
1272 first.min(last) <= 1e-9 && first.max(last) >= 1.0 - 1e-9,
1273 "from {u}: the whole way round, seam to seam: {first} to {last}"
1274 );
1275 }
1276 for pair in traced.points.windows(2) {
1277 let miss = off_contact(&strip, pair[0] + (pair[1] - pair[0]) * 0.5);
1278 assert!(
1279 miss <= chord,
1280 "rise {rise}, from {u}: the chord {:?} to {:?} misses the contact by \
1281 {miss:e}",
1282 pair[0],
1283 pair[1]
1284 );
1285 }
1286 }
1287 }
1288 }
1289
1290 #[test]
1291 fn a_ball_in_its_drum_is_traced_on_the_contact_conditions() {
1292 let drum: SurfaceGeometry =
1295 CylinderSurface::new(Cylinder::new(Frame::WORLD, 1.0, T).unwrap(), (-2.0, 2.0))
1296 .unwrap()
1297 .into();
1298 let ball = ball(1.0);
1299 let at = Point::new(0.6, 0.8, 0.0);
1300 let traced =
1301 trace_tangential(&drum, &ball, seed(&drum, &ball, at), Marching::default(), T).unwrap();
1302 assert_eq!(traced.stopped, Stopped::Closed);
1303 for (i, p) in traced.points.iter().enumerate() {
1304 let (ja, jb) = (
1305 jet(&drum, traced.on_a[i], T).unwrap(),
1306 jet(&ball, traced.on_b[i], T).unwrap(),
1307 );
1308 assert!(ja.p.distance(*p) <= 1e-12 && jb.p.distance(*p) <= 1e-9);
1309 assert!(ja.n.cross(jb.n).magnitude() <= 1e-9);
1310 assert!(p.z.abs() <= 1e-9 && (p.x.hypot(p.y) - 1.0).abs() <= 1e-9);
1311 }
1312 }
1313}