1use ogeom_core::{OgeomResult, Tolerances, ogeom_bail};
34use ogeom_geom::{BSpline2d, BSplineCurve, Surface, SurfaceGeometry};
35use ogeom_math::Point2;
36
37use crate::march::Traced;
38
39#[derive(Debug, Clone, PartialEq)]
41pub struct IntersectionCurve {
42 pub curve: BSplineCurve,
44 pub on_a: BSpline2d,
46 pub on_b: BSpline2d,
48 pub fit_error: f64,
57 pub met: bool,
59 pub closed: bool,
61}
62
63pub fn approximate_branch(
70 a: &SurfaceGeometry,
71 b: &SurfaceGeometry,
72 branch: &Traced,
73 tolerance: f64,
74 tol: Tolerances,
75) -> OgeomResult<IntersectionCurve> {
76 if branch.points.len() < 2 {
77 ogeom_bail!(
78 Construction,
79 "a branch of {} points is not a curve",
80 branch.points.len()
81 );
82 }
83
84 let mut points: Vec<ogeom_math::Point> = Vec::with_capacity(branch.points.len());
90 let mut kept_a = Vec::with_capacity(branch.on_a.len());
91 let mut kept_b = Vec::with_capacity(branch.on_b.len());
92 let agrees = |i: usize, p: &ogeom_math::Point| -> bool {
98 let limit = tolerance.max(tol.confusion());
99 let (ua, va) = branch.on_a[i];
100 let (ub, vb) = branch.on_b[i];
101 a.point_at(ua, va, tol)
102 .is_ok_and(|q| q.distance(*p) <= limit)
103 && b.point_at(ub, vb, tol)
104 .is_ok_and(|q| q.distance(*p) <= limit)
105 };
106 for (i, p) in branch.points.iter().enumerate() {
107 let end = i == 0 || i + 1 == branch.points.len();
108 if let Some(last) = points.last()
109 && last.distance(*p) <= tol.confusion() * 10.0
110 && i + 1 != branch.points.len()
111 {
112 continue;
113 }
114 if !end && !agrees(i, p) {
115 continue;
116 }
117 points.push(*p);
118 kept_a.push(branch.on_a[i]);
119 kept_b.push(branch.on_b[i]);
120 }
121 if points.len() < 2 {
122 ogeom_bail!(Construction, "a branch of coincident points is not a curve");
123 }
124
125 let unwrapped_a = unwrap_periodic(a, &kept_a, tol);
133 let unwrapped_b = unwrap_periodic(b, &kept_b, tol);
134 let winds_periodically = |surface: &SurfaceGeometry, image: &[Point2]| {
145 let (first, last) = (image[0], image[image.len() - 1]);
146 ((last.x - first.x).abs() <= tol.parametric() || surface.is_periodic_u())
147 && ((last.y - first.y).abs() <= tol.parametric() || surface.is_periodic_v())
148 };
149 let (space, on_a, on_b) = if branch.closed() {
150 let closed = ogeom_geom::fit::fit_points_joint_closed(
151 &points,
152 &unwrapped_a,
153 &unwrapped_b,
154 3,
155 tolerance,
156 tol,
157 )?;
158 if !closed.0.met
159 && winds_periodically(a, &unwrapped_a)
160 && winds_periodically(b, &unwrapped_b)
161 {
162 let winding = ogeom_geom::fit::fit_points_joint_winding(
163 &points,
164 &unwrapped_a,
165 &unwrapped_b,
166 3,
167 tolerance,
168 tol,
169 )?;
170 if winding.0.error < closed.0.error {
171 winding
172 } else {
173 closed
174 }
175 } else {
176 closed
177 }
178 } else {
179 ogeom_geom::fit::fit_points_joint(&points, &unwrapped_a, &unwrapped_b, 3, tolerance, tol)?
180 };
181
182 let lifted = lift_error(a, b, &on_a, &on_b, &space.curve, tol);
186 let traced = trace_error(&space.curve, &points, space.error, tol);
187 let charted =
194 space_error(a, &on_a, space.error, tol).max(space_error(b, &on_b, space.error, tol));
195 let fit_error = traced
196 .max(lifted.along)
197 .max(lifted.across.min(charted.max(space.error)));
198 Ok(IntersectionCurve {
199 fit_error,
200 met: space.met,
201 curve: space.curve,
202 on_a,
203 on_b,
204 closed: branch.closed(),
205 })
206}
207
208struct Lifted {
210 along: f64,
213 across: f64,
216}
217
218fn lift_error(
224 a: &SurfaceGeometry,
225 b: &SurfaceGeometry,
226 on_a: &BSpline2d,
227 on_b: &BSpline2d,
228 curve: &BSplineCurve,
229 tol: Tolerances,
230) -> Lifted {
231 use ogeom_geom::{Curve2d as _, Curve3d as _};
232 let (lo, hi) = curve.knots().domain();
233 let spans = curve.knots().distinct().len().saturating_sub(1).max(1);
234 let stations = (4 * spans).max(200);
235 let mut out = Lifted {
236 along: 0.0,
237 across: 0.0,
238 };
239 for k in 0..=stations {
240 #[allow(clippy::cast_precision_loss)]
241 let t = lo + (hi - lo) * k as f64 / stations as f64;
242 let Ok(on) = curve.point_at(t, tol) else {
243 continue;
244 };
245 let mut gap = 0.0_f64;
246 let mut normals = Vec::with_capacity(2);
247 for (surface, pcurve) in [(a, on_a), (b, on_b)] {
248 let Ok(at) = pcurve.point_at(t, tol) else {
249 continue;
250 };
251 let Ok(lifted) = surface.point_at(at.x, at.y, tol) else {
252 continue;
253 };
254 gap = gap.max(lifted.distance(on));
255 if let Ok(normal) = surface.normal_at(at.x, at.y, tol) {
256 normals.push(normal.vector());
257 }
258 }
259 out.along = out.along.max(gap);
260 if let [na, nb] = normals[..] {
261 let sine = na.cross(nb).magnitude().max(tol.angular());
262 out.across = out.across.max(gap / sine);
263 }
264 }
265 out
266}
267
268fn trace_error(
278 curve: &BSplineCurve,
279 samples: &[ogeom_math::Point],
280 bound: f64,
281 tol: Tolerances,
282) -> f64 {
283 use ogeom_geom::Curve3d as _;
284 let (lo, hi) = curve.knots().domain();
285 let spans = curve.knots().distinct().len().saturating_sub(1).max(1);
286 let count = (4 * spans).max(2 * samples.len()).max(200);
287 #[allow(clippy::cast_precision_loss)]
288 let at = |k: usize| lo + (hi - lo) * k as f64 / count as f64;
289 let mut stations: Option<Vec<(f64, ogeom_math::Point)>> = None;
290 let foot = |p: ogeom_math::Point, mut t: f64| -> (f64, f64) {
291 let mut best = (t, f64::INFINITY);
292 for _ in 0..8 {
293 let (Ok(q), Ok(d)) = (curve.point_at(t, tol), curve.d1_at(t, tol)) else {
294 break;
295 };
296 let gap = q.distance(p);
297 if gap < best.1 {
298 best = (t, gap);
299 }
300 let speed = d.dot(d);
301 if speed <= f64::MIN_POSITIVE {
302 break;
303 }
304 let next = (t + (p - q).dot(d) / speed).clamp(lo, hi);
305 if (next - t).abs() <= (hi - lo) * 1e-12 {
306 break;
307 }
308 t = next;
309 }
310 if let Ok(q) = curve.point_at(t, tol)
311 && q.distance(p) < best.1
312 {
313 best = (t, q.distance(p));
314 }
315 best
316 };
317 let mut t = lo;
318 let mut worst = 0.0_f64;
319 for p in samples {
320 let mut found = foot(*p, t);
321 if found.1 > bound {
322 let stations = stations.get_or_insert_with(|| {
323 (0..=count)
324 .filter_map(|k| curve.point_at(at(k), tol).ok().map(|q| (at(k), q)))
325 .collect()
326 });
327 if let Some(k) = (0..stations.len()).min_by(|&x, &y| {
328 stations[x]
329 .1
330 .distance(*p)
331 .total_cmp(&stations[y].1.distance(*p))
332 }) {
333 let gap = |u: f64| {
339 curve
340 .point_at(u, tol)
341 .map_or(f64::INFINITY, |q| q.distance(*p))
342 };
343 let (from, to) = (
344 stations[k.saturating_sub(2)].0,
345 stations[(k + 2).min(stations.len() - 1)].0,
346 );
347 const FINE: u32 = 256;
348 let h = (to - from) / f64::from(FINE);
349 let start = (0..=FINE)
350 .map(|j| from + h * f64::from(j))
351 .min_by(|&x, &y| gap(x).total_cmp(&gap(y)))
352 .unwrap_or(from);
353 let (mut a, mut b) = ((start - h).max(lo), (start + h).min(hi));
354 let ratio = 0.5 * (5.0_f64.sqrt() - 1.0);
355 for _ in 0..60 {
356 let (x, y) = (b - ratio * (b - a), a + ratio * (b - a));
357 if gap(x) <= gap(y) {
358 b = y;
359 } else {
360 a = x;
361 }
362 }
363 let again = foot(*p, 0.5 * (a + b));
364 if again.1 < found.1 {
365 found = again;
366 }
367 }
368 }
369 t = found.0;
370 worst = worst.max(found.1.min(bound));
371 }
372 worst
373}
374
375fn space_error(
380 surface: &SurfaceGeometry,
381 pcurve: &BSpline2d,
382 parameter_error: f64,
383 tol: Tolerances,
384) -> f64 {
385 use ogeom_geom::Curve2d;
386 let (lo, hi) = pcurve.domain();
389 let mut worst = 0.0_f64;
390 for i in 0..=16 {
391 #[allow(clippy::cast_precision_loss)]
392 let u = lo + (hi - lo) * f64::from(i) / 16.0;
393 let Ok(at) = pcurve.point_at(u, tol) else {
394 continue;
395 };
396 let Ok((du, dv)) = surface.d1_at(at.x, at.y, tol) else {
397 continue;
398 };
399 let stretch = du.magnitude().max(dv.magnitude());
400 worst = worst.max(parameter_error * stretch);
401 }
402 worst
403}
404
405fn unwrap_periodic(
412 surface: &SurfaceGeometry,
413 samples: &[(f64, f64)],
414 tol: Tolerances,
415) -> Vec<Point2> {
416 let ((ua, ub), (va, vb)) = surface.domain();
417 let u_period = if surface.is_periodic_u() || surface.is_closed_u(tol) {
424 Some(ub - ua)
425 } else {
426 None
427 };
428 let v_period = if surface.is_periodic_v() || surface.is_closed_v(tol) {
429 Some(vb - va)
430 } else {
431 None
432 };
433 let fold = |previous: f64, next: f64, period: Option<f64>| match period {
434 None => next,
435 Some(period) => {
436 let mut candidate = next;
437 while candidate - previous > period * 0.5 {
438 candidate -= period;
439 }
440 while previous - candidate > period * 0.5 {
441 candidate += period;
442 }
443 candidate
444 }
445 };
446
447 let mut out = Vec::with_capacity(samples.len());
448 let mut at = Point2::new(samples[0].0, samples[0].1);
449 out.push(at);
450 for sample in &samples[1..] {
451 at = Point2::new(
452 fold(at.x, sample.0, u_period),
453 fold(at.y, sample.1, v_period),
454 );
455 out.push(at);
456 }
457 out
458}
459
460#[cfg(test)]
461#[allow(clippy::unwrap_used)]
462mod tests {
463 use super::*;
464 use crate::march::{Marching, branches};
465 use ogeom_geom::{Curve2d, Curve3d, CylinderSurface, PlaneSurface, SphereSurface};
466 use ogeom_math::{Cylinder, Direction, Frame, Plane, Point, Sphere, Vector};
467
468 const T: Tolerances = Tolerances::millimetres();
469
470 fn sphere(radius: f64) -> SurfaceGeometry {
471 SphereSurface::new(Sphere::centred(Point::ORIGIN, radius, T).unwrap()).into()
472 }
473
474 fn cylinder(radius: f64) -> SurfaceGeometry {
475 CylinderSurface::new(Cylinder::new(Frame::WORLD, radius, T).unwrap(), (-4.0, 4.0))
476 .unwrap()
477 .into()
478 }
479
480 fn plane(origin: Point, normal: Vector) -> SurfaceGeometry {
481 PlaneSurface::over(
482 Plane::through(origin, Direction::new(normal, T).unwrap()),
483 (-6.0, 6.0),
484 (-6.0, 6.0),
485 )
486 .unwrap()
487 .into()
488 }
489
490 fn options() -> Marching {
491 Marching {
492 chord: 1e-5,
493 ..Marching::default()
494 }
495 }
496
497 fn fitted_deviation(a: &SurfaceGeometry, b: &SurfaceGeometry, curve: &BSplineCurve) -> f64 {
503 let off = |surface: &SurfaceGeometry, p: Point| match surface {
504 SurfaceGeometry::Plane(x) => x.plane().distance_to(p),
505 SurfaceGeometry::Sphere(x) => x.sphere().distance_to(p),
506 SurfaceGeometry::Cylinder(x) => x.cylinder().distance_to(p),
507 _ => 0.0,
508 };
509 let (lo, hi) = curve.knots().domain();
510 let mut worst = 0.0_f64;
511 for i in 0..=800 {
512 #[allow(clippy::cast_precision_loss)]
513 let u = lo + (hi - lo) * f64::from(i) / 800.0;
514 if let Ok(p) = curve.point_at(u, T) {
515 worst = worst.max(off(a, p).abs().max(off(b, p).abs()));
516 }
517 }
518 worst
519 }
520
521 #[test]
522 fn a_fitted_branch_lies_on_both_surfaces_to_the_stated_total() {
523 let a = sphere(3.0);
527 let b = cylinder(1.5);
528 let found = branches(&a, &b, options(), T).unwrap();
529 assert_eq!(found.len(), 2);
530
531 for branch in &found {
532 let fitted = approximate_branch(&a, &b, branch, 1e-4, T).unwrap();
533 assert!(fitted.met, "fit error {:e}", fitted.fit_error);
534 assert!(fitted.closed);
535 let off = fitted_deviation(&a, &b, &fitted.curve);
536 assert!(
537 off <= 1e-4 + 1e-5,
538 "the fitted curve is {off:e} off the surfaces"
539 );
540 assert!(
542 fitted.curve.control_points().len() * 4 < branch.points.len(),
543 "{} control points for {} samples",
544 fitted.curve.control_points().len(),
545 branch.points.len()
546 );
547 }
548 }
549
550 #[test]
551 fn the_pcurves_lift_back_onto_the_curve() {
552 let a = sphere(3.0);
556 let b = cylinder(1.5);
557 let found = branches(&a, &b, options(), T).unwrap();
558 let branch = &found[0];
559 let fitted = approximate_branch(&a, &b, branch, 1e-4, T).unwrap();
560
561 for (surface, pcurve) in [(&a, &fitted.on_a), (&b, &fitted.on_b)] {
562 let (lo, hi) = pcurve.domain();
563 for i in 0..=200 {
564 #[allow(clippy::cast_precision_loss)]
565 let u = lo + (hi - lo) * f64::from(i) / 200.0;
566 let at = pcurve.point_at(u, T).unwrap();
567 let lifted = surface.point_at(at.x, at.y, T).unwrap();
568 let off = match (surface as &SurfaceGeometry, &a, &b) {
572 _ if core::ptr::eq(surface, &a) => match &b {
573 SurfaceGeometry::Cylinder(c) => c.cylinder().distance_to(lifted),
574 _ => 0.0,
575 },
576 _ => match &a {
577 SurfaceGeometry::Sphere(s) => s.sphere().distance_to(lifted),
578 _ => 0.0,
579 },
580 };
581 assert!(
582 off.abs() < 5e-4,
583 "a lifted pcurve point is {off:e} off the intersection"
584 );
585 }
586 }
587 }
588
589 #[test]
590 fn a_branch_across_the_seam_gets_a_continuous_pcurve() {
591 let a = cylinder(2.0);
596 let b = plane(Point::ORIGIN, Vector::new(0.0, 0.4, 1.0));
597 let found = branches(&a, &b, options(), T).unwrap();
598 assert_eq!(found.len(), 1, "an oblique plane cuts one ellipse");
599 let fitted = approximate_branch(&a, &b, &found[0], 1e-4, T).unwrap();
600
601 let (lo, hi) = fitted.on_a.domain();
604 let mut previous = fitted.on_a.point_at(lo, T).unwrap();
605 for i in 1..=400 {
606 #[allow(clippy::cast_precision_loss)]
607 let u = lo + (hi - lo) * f64::from(i) / 400.0;
608 let at = fitted.on_a.point_at(u, T).unwrap();
609 assert!(
610 (at.x - previous.x).abs() < 1.0,
611 "the pcurve tears at the seam: {} to {}",
612 previous.x,
613 at.x
614 );
615 previous = at;
616 }
617 }
618
619 #[test]
629 fn a_loop_cut_at_a_converted_drum_s_seam_is_closed() {
630 let drum: SurfaceGeometry = cylinder(2.0).to_bspline(T).unwrap().into();
631 assert!(matches!(drum, SurfaceGeometry::BSpline(_)));
632 let cut = plane(Point::new(0.0, 0.0, 1.0), Vector::new(0.0, 0.2, 1.0));
633 let found = branches(&drum, &cut, options(), T).unwrap();
634 assert_eq!(found.len(), 1, "an oblique plane cuts one loop");
635 assert!(found[0].closed(), "the loop closes on the seam");
636 let fitted = approximate_branch(&drum, &cut, &found[0], 1e-4, T).unwrap();
637 assert!(fitted.closed);
638 assert!(
639 fitted.fit_error < 1e-3,
640 "the loop fits as one: {}",
641 fitted.fit_error
642 );
643 let (lo, hi) = fitted.on_a.domain();
644 let mut previous = fitted.on_a.point_at(lo, T).unwrap();
645 for i in 1..=400 {
646 let u = lo + (hi - lo) * f64::from(i) / 400.0;
647 let at = fitted.on_a.point_at(u, T).unwrap();
648 assert!(
649 (at.x - previous.x).abs() < 0.5,
650 "the chart image tears at the seam: {} to {}",
651 previous.x,
652 at.x
653 );
654 previous = at;
655 }
656 }
657
658 #[test]
666 fn a_section_across_a_wide_drum_states_its_measured_error() {
667 let radius = 23.6;
668 let wall = CylinderSurface::new(
669 Cylinder::new(Frame::WORLD, radius, T).unwrap(),
670 (-30.0, 30.0),
671 )
672 .unwrap();
673 let drum: SurfaceGeometry = SurfaceGeometry::from(wall).to_bspline(T).unwrap().into();
674 let bore = Cylinder::new(
675 Frame::new(
676 Point::new(0.0, 0.0, 3.0),
677 Direction::new(Vector::X, T).unwrap(),
678 Direction::new(Vector::Y, T).unwrap(),
679 T,
680 )
681 .unwrap(),
682 9.0,
683 T,
684 )
685 .unwrap();
686 let drill: SurfaceGeometry = CylinderSurface::new(bore, (-40.0, 40.0)).unwrap().into();
687 let marching = Marching {
688 chord: 1e-4,
689 ..Marching::default()
690 };
691 let found = branches(&drum, &drill, marching, T).unwrap();
692 assert!(!found.is_empty());
693 for branch in &found {
694 let fitted = approximate_branch(&drum, &drill, branch, 1e-4, T).unwrap();
695 let (lo, hi) = fitted.curve.knots().domain();
696 let mut off = 0.0_f64;
697 for i in 0..=2000 {
698 let t = lo + (hi - lo) * f64::from(i) / 2000.0;
699 let p = fitted.curve.point_at(t, T).unwrap();
700 off = off.max(
701 wall.cylinder()
702 .distance_to(p)
703 .abs()
704 .max(bore.distance_to(p).abs()),
705 );
706 }
707 assert!(
710 fitted.fit_error + marching.chord >= off,
711 "states {:e} for a curve {off:e} off its surfaces",
712 fitted.fit_error
713 );
714 assert!(
715 fitted.fit_error <= 10.0 * off.max(marching.chord),
716 "states {:e} for a curve {off:e} off its surfaces",
717 fitted.fit_error
718 );
719 }
720 }
721
722 #[test]
723 fn what_cannot_be_fitted_is_refused() {
724 let a = sphere(1.0);
725 let b = plane(Point::ORIGIN, Vector::Z);
726 let found = branches(&a, &b, options(), T).unwrap();
727 assert!(approximate_branch(&a, &b, &found[0], 0.0, T).is_err());
728 assert!(approximate_branch(&a, &b, &found[0], -1.0, T).is_err());
729
730 let empty = Traced {
731 points: vec![],
732 on_a: vec![],
733 on_b: vec![],
734 stopped: crate::march::Stopped::Stalled,
735 };
736 assert!(approximate_branch(&a, &b, &empty, 1e-4, T).is_err());
737 }
738}