1use ogeom_core::{OgeomResult, Tolerances, ogeom_bail};
37use ogeom_geom::{Curve, Curve2d, Curve3d, PlanarCurve};
38use ogeom_math::{Point, Point2, solve};
39
40#[derive(Debug, Clone, Copy, PartialEq)]
42pub struct Crossing<P> {
43 pub on_a: f64,
45 pub on_b: f64,
47 pub point: P,
49 pub gap: f64,
55 pub reach: f64,
61}
62
63#[derive(Debug, Clone, Copy, PartialEq)]
65pub struct Overlap {
66 pub on_a: (f64, f64),
68 pub on_b: (f64, f64),
70}
71
72#[derive(Debug, Clone, PartialEq)]
74pub struct CurveIntersection<P> {
75 pub crossings: Vec<Crossing<P>>,
77 pub overlaps: Vec<Overlap>,
89}
90
91impl<P> CurveIntersection<P> {
92 #[must_use]
94 pub fn is_empty(&self) -> bool {
95 self.crossings.is_empty() && self.overlaps.is_empty()
96 }
97
98 const fn empty() -> Self {
99 Self {
100 crossings: Vec::new(),
101 overlaps: Vec::new(),
102 }
103 }
104}
105
106#[derive(Debug, Clone, Copy, PartialEq)]
108pub struct CurveCurveOptions {
109 pub samples: usize,
113 pub gap: f64,
118}
119
120impl Default for CurveCurveOptions {
121 fn default() -> Self {
122 Self {
123 samples: 128,
124 gap: 1e-7,
125 }
126 }
127}
128
129pub fn intersect_curves_2d(
139 a: &PlanarCurve,
140 b: &PlanarCurve,
141 options: CurveCurveOptions,
142 tol: Tolerances,
143) -> OgeomResult<CurveIntersection<Point2>> {
144 check(options)?;
145 match (a, b) {
146 (PlanarCurve::Line(x), PlanarCurve::Line(y)) => Ok(line_line_2d(x, y, tol)),
147 (PlanarCurve::Line(x), PlanarCurve::Circle(y)) => Ok(line_circle_2d(x, y, false, tol)),
148 (PlanarCurve::Circle(x), PlanarCurve::Line(y)) => Ok(line_circle_2d(y, x, true, tol)),
149 (PlanarCurve::Circle(x), PlanarCurve::Circle(y)) => Ok(circle_circle_2d(x, y, tol)),
150 _ => general_2d(a, b, options, tol),
151 }
152}
153
154pub fn intersect_curves(
160 a: &Curve,
161 b: &Curve,
162 options: CurveCurveOptions,
163 tol: Tolerances,
164) -> OgeomResult<CurveIntersection<Point>> {
165 check(options)?;
166 let (basis_a, window_a) = through_trim(a);
167 let (basis_b, window_b) = through_trim(b);
168 if let Some(found) = same_curve_3d(basis_a, basis_b) {
169 return Ok(clipped_to_windows(
170 found,
171 (basis_a, window_a),
172 (basis_b, window_b),
173 tol,
174 ));
175 }
176 if let Some(found) = analytic_3d(basis_a, basis_b, options, tol) {
177 return Ok(clipped_to_windows(
178 found,
179 (basis_a, window_a),
180 (basis_b, window_b),
181 tol,
182 ));
183 }
184 general_3d(a, b, options, tol)
185}
186
187fn through_trim(curve: &Curve) -> (&Curve, Option<(f64, f64)>) {
195 match curve {
196 Curve::Trimmed(t) if !t.is_reversed() => (t.basis(), Some(Curve3d::domain(&**t))),
197 other => (other, None),
198 }
199}
200
201fn analytic_3d(
204 a: &Curve,
205 b: &Curve,
206 options: CurveCurveOptions,
207 tol: Tolerances,
208) -> Option<CurveIntersection<Point>> {
209 match (a, b) {
210 (Curve::Line(x), Curve::Line(y)) => Some(line_line_3d(x, y, options, tol)),
211 (Curve::Circle(x), Curve::Circle(y)) => {
212 same_circle_3d(x, y, tol).or_else(|| skew_conics_3d(a, b, options, tol))
213 }
214 (Curve::Ellipse(x), Curve::Ellipse(y)) => {
215 same_ellipse_3d(x, y, tol).or_else(|| skew_conics_3d(a, b, options, tol))
216 }
217 (Curve::Circle(_), Curve::Ellipse(_)) | (Curve::Ellipse(_), Curve::Circle(_)) => {
218 skew_conics_3d(a, b, options, tol)
219 }
220 (Curve::Line(x), Curve::Circle(_) | Curve::Ellipse(_)) => line_conic_3d(x, b, options, tol)
221 .map(|mut found| {
222 for c in &mut found.crossings {
223 core::mem::swap(&mut c.on_a, &mut c.on_b);
224 }
225 found.crossings.sort_by(|x, y| x.on_a.total_cmp(&y.on_a));
226 found
227 }),
228 (Curve::Circle(_) | Curve::Ellipse(_), Curve::Line(y)) => line_conic_3d(y, a, options, tol),
229 _ => None,
230 }
231}
232
233fn conic_of(
237 curve: &Curve,
238) -> Option<(
239 Point,
240 ogeom_math::Vector,
241 ogeom_math::Vector,
242 ogeom_math::Vector,
243)> {
244 match curve {
245 Curve::Circle(c) if !c.is_reversed() => {
246 let circle = c.circle();
247 let f = circle.frame();
248 Some((
249 f.origin(),
250 f.x().vector() * circle.radius(),
251 f.y().vector() * circle.radius(),
252 f.z().vector(),
253 ))
254 }
255 Curve::Ellipse(e) if !e.is_reversed() => {
256 let ellipse = e.ellipse();
257 let f = ellipse.frame();
258 Some((
259 f.origin(),
260 f.x().vector() * ellipse.major_radius(),
261 f.y().vector() * ellipse.minor_radius(),
262 f.z().vector(),
263 ))
264 }
265 _ => None,
266 }
267}
268
269fn skew_conics_3d(
275 a: &Curve,
276 b: &Curve,
277 options: CurveCurveOptions,
278 tol: Tolerances,
279) -> Option<CurveIntersection<Point>> {
280 let (ca, ua, va, na) = conic_of(a)?;
281 let (cb, _, _, nb) = conic_of(b)?;
282 if na.cross(nb).magnitude() <= tol.angular() {
283 return ((ca - cb).dot(nb).abs() > options.gap.max(tol.confusion()))
284 .then(CurveIntersection::empty);
285 }
286 let (alpha, beta, gamma) = (nb.dot(ua), nb.dot(va), nb.dot(ca - cb));
287 let size = alpha.hypot(beta);
288 if options.gap.max(tol.confusion()) > size * 1e-3 {
295 return None;
296 }
297 let mut crossings: Vec<Crossing<Point>> = Vec::new();
298 if size > 0.0 && gamma.abs() <= size * (1.0 + 1e-12) {
299 let phase = beta.atan2(alpha);
300 let turn = (-gamma / size).clamp(-1.0, 1.0).acos();
301 let (a_lo, a_hi) = a.domain();
302 let (b_lo, b_hi) = b.domain();
303 let tau = core::f64::consts::TAU;
304 let into = |t: f64, lo: f64| lo + (t - lo).rem_euclid(tau);
305 let roots = if turn <= 1e-12 {
306 vec![phase]
307 } else {
308 vec![phase - turn, phase + turn]
309 };
310 for root in roots {
311 let t = into(root, a_lo);
312 if t > a_hi + tol.parametric() {
313 continue;
314 }
315 let point = a.point_at(t, tol).ok()?;
316 if (point - cb).dot(nb).abs() > tol.confusion() * 10.0 {
320 return None;
321 }
322 let s = match b {
323 Curve::Circle(c) => {
324 ogeom_math::elementary::circle_parameter(&c.circle(), point, tol)
325 }
326 Curve::Ellipse(e) => {
327 ogeom_math::elementary::ellipse_parameter(&e.ellipse(), point, tol)
328 }
329 _ => return None,
330 };
331 let Ok(s) = s else {
332 continue;
333 };
334 let s = into(s, b_lo);
335 if s > b_hi + tol.parametric() {
336 continue;
337 }
338 let gap = b.point_at(s, tol).ok()?.distance(point);
339 if gap > options.gap {
340 continue;
341 }
342 crossings.push(Crossing {
343 on_a: t,
344 on_b: s,
345 point,
346 gap,
347 reach: 0.0,
348 });
349 }
350 }
351 crossings.sort_by(|x, y| x.on_a.total_cmp(&y.on_a));
352 Some(CurveIntersection {
353 crossings,
354 overlaps: Vec::new(),
355 })
356}
357
358fn line_conic_3d(
367 line: &ogeom_geom::LineCurve,
368 conic: &Curve,
369 options: CurveCurveOptions,
370 tol: Tolerances,
371) -> Option<CurveIntersection<Point>> {
372 let (centre, u, v, normal) = conic_of(conic)?;
373 let (origin, along) = (line.axis().location, line.axis().direction.vector());
374 let (a_len, b_len) = (u.magnitude(), v.magnitude());
375 if a_len <= tol.confusion() || b_len <= tol.confusion() {
376 return None;
377 }
378 let (ux, vy) = (u / a_len, v / b_len);
379 let lean = along.dot(normal);
380 let height = (origin - centre).dot(normal);
381 let mut ts: Vec<f64> = Vec::new();
382 if lean.abs() <= tol.angular() {
383 if height.abs() > options.gap.max(tol.confusion()) {
384 return Some(CurveIntersection::empty());
385 }
386 let (x0, y0) = ((origin - centre).dot(ux), (origin - centre).dot(vy));
388 let (dx, dy) = (along.dot(ux), along.dot(vy));
389 let qa = dx * dx / (a_len * a_len) + dy * dy / (b_len * b_len);
390 let qb = 2.0 * (x0 * dx / (a_len * a_len) + y0 * dy / (b_len * b_len));
391 let qc = x0 * x0 / (a_len * a_len) + y0 * y0 / (b_len * b_len) - 1.0;
392 let disc = qb.mul_add(qb, -4.0 * qa * qc);
393 if qa <= 0.0 {
394 return None;
395 }
396 if disc < 0.0 {
397 let t = -qb / (2.0 * qa);
398 let p = origin + along * t;
399 let foot = conic_parameter(conic, p, tol)?;
400 let gap = conic.point_at(foot, tol).ok()?.distance(p);
401 return (gap > options.gap).then(CurveIntersection::empty);
402 }
403 let root = disc.sqrt();
404 ts.push((-qb - root) / (2.0 * qa));
405 ts.push((-qb + root) / (2.0 * qa));
406 } else if lean.abs() >= 0.1 {
407 ts.push(-height / lean);
408 } else {
409 return None;
410 }
411 let (lo, hi) = line.domain();
412 let (c_lo, c_hi) = conic.domain();
413 let tau = core::f64::consts::TAU;
414 let mut crossings: Vec<Crossing<Point>> = Vec::new();
415 for t in ts {
416 if t < lo - tol.parametric() || t > hi + tol.parametric() {
417 continue;
418 }
419 let point = origin + along * t;
420 let s = conic_parameter(conic, point, tol)?;
421 let s = c_lo + (s - c_lo).rem_euclid(tau);
422 if s > c_hi + tol.parametric() {
423 continue;
424 }
425 let on_conic = conic.point_at(s, tol).ok()?;
426 let gap = on_conic.distance(point);
427 if gap > options.gap {
428 continue;
429 }
430 let tangent = conic.d1_at(s, tol).ok()?;
431 if tangent.cross(along).magnitude() <= 1e-3 * tangent.magnitude() {
432 return None;
433 }
434 crossings.push(Crossing {
435 on_a: s,
436 on_b: t,
437 point: on_conic,
438 gap,
439 reach: 0.0,
440 });
441 }
442 crossings.sort_by(|x, y| x.on_a.total_cmp(&y.on_a));
443 crossings.dedup_by(|x, y| (x.on_a - y.on_a).abs() <= tol.parametric());
444 Some(CurveIntersection {
445 crossings,
446 overlaps: Vec::new(),
447 })
448}
449
450fn conic_parameter(conic: &Curve, point: Point, tol: Tolerances) -> Option<f64> {
452 match conic {
453 Curve::Circle(c) => ogeom_math::elementary::circle_parameter(&c.circle(), point, tol).ok(),
454 Curve::Ellipse(e) => {
455 ogeom_math::elementary::ellipse_parameter(&e.ellipse(), point, tol).ok()
456 }
457 _ => None,
458 }
459}
460
461fn clipped_to_windows(
470 found: CurveIntersection<Point>,
471 a: (&Curve, Option<(f64, f64)>),
472 b: (&Curve, Option<(f64, f64)>),
473 tol: Tolerances,
474) -> CurveIntersection<Point> {
475 let (basis_a, window_a) = a;
476 let (basis_b, window_b) = b;
477 if window_a.is_none() && window_b.is_none() {
478 return found;
479 }
480 let period = |curve: &Curve| -> Option<f64> {
481 if curve.is_periodic() {
482 let (lo, hi) = curve.domain();
483 (hi > lo).then_some(hi - lo)
484 } else {
485 None
486 }
487 };
488 let (pa, pb) = (period(basis_a), period(basis_b));
489 let slack = tol.parametric();
490 let placed = |t: f64, window: Option<(f64, f64)>, period: Option<f64>| -> Option<f64> {
491 let Some((lo, hi)) = window else {
492 return Some(t);
493 };
494 for k in [0.0, 1.0, -1.0, 2.0, -2.0] {
495 let shifted = period.map_or(t, |p| p.mul_add(k, t));
496 if shifted >= lo - slack && shifted <= hi + slack {
497 return Some(shifted);
498 }
499 if period.is_none() {
500 break;
501 }
502 }
503 None
504 };
505
506 let mut crossings = Vec::with_capacity(found.crossings.len());
507 for crossing in found.crossings {
508 let (Some(on_a), Some(on_b)) = (
509 placed(crossing.on_a, window_a, pa),
510 placed(crossing.on_b, window_b, pb),
511 ) else {
512 continue;
513 };
514 crossings.push(Crossing {
515 on_a,
516 on_b,
517 ..crossing
518 });
519 }
520
521 let mut overlaps = Vec::with_capacity(found.overlaps.len());
522 for overlap in found.overlaps {
523 let span_a = overlap.on_a.1 - overlap.on_a.0;
524 let span_b = overlap.on_b.1 - overlap.on_b.0;
525 if span_a.abs() <= f64::MIN_POSITIVE || span_b.abs() <= f64::MIN_POSITIVE {
526 continue;
527 }
528 let to_b = |t: f64| overlap.on_b.0 + span_b * (t - overlap.on_a.0) / span_a;
529 let to_a = |t: f64| overlap.on_a.0 + span_a * (t - overlap.on_b.0) / span_b;
530 let ordered = |r: (f64, f64)| if r.0 <= r.1 { r } else { (r.1, r.0) };
531 let mut kept = ordered(overlap.on_a);
532 if let Some(w) = window_b {
535 let (wlo, whi) = ordered((to_a(w.0), to_a(w.1)));
536 let shift = pa.unwrap_or(0.0);
537 let mut best: Option<(f64, f64)> = None;
538 for k in [0.0, 1.0, -1.0, 2.0, -2.0] {
539 let candidate = (
540 kept.0.max(shift.mul_add(k, wlo)),
541 kept.1.min(shift.mul_add(k, whi)),
542 );
543 if candidate.1 - candidate.0 > best.map_or(0.0, |(lo, hi)| hi - lo) {
544 best = Some(candidate);
545 }
546 if shift == 0.0 {
547 break;
548 }
549 }
550 let Some(candidate) = best else { continue };
551 kept = candidate;
552 }
553 if let Some(w) = window_a {
554 let (wlo, whi) = ordered(w);
555 kept = (kept.0.max(wlo), kept.1.min(whi));
556 }
557 if kept.1 - kept.0 <= slack {
558 continue;
559 }
560 let (on_a, on_b) = if span_a >= 0.0 {
561 (kept, (to_b(kept.0), to_b(kept.1)))
562 } else {
563 ((kept.1, kept.0), (to_b(kept.1), to_b(kept.0)))
564 };
565 overlaps.push(Overlap { on_a, on_b });
566 }
567 CurveIntersection {
568 crossings,
569 overlaps,
570 }
571}
572
573fn same_circle_3d(
579 a: &ogeom_geom::CircleCurve,
580 b: &ogeom_geom::CircleCurve,
581 tol: Tolerances,
582) -> Option<CurveIntersection<Point>> {
583 let (ca, cb) = (a.circle(), b.circle());
584 if ca.centre().distance(cb.centre()) > tol.confusion() {
585 return None;
586 }
587 if (ca.radius() - cb.radius()).abs() > tol.confusion() {
588 return None;
589 }
590 let (za, zb) = (ca.frame().z().vector(), cb.frame().z().vector());
593 if za.cross(zb).magnitude() > tol.angular() {
594 return None;
595 }
596 let (lo, hi) = Curve3d::domain(a);
603 let start = a.point_at(lo, tol).ok()?;
604 let local = cb.frame().to_local(start);
605 let angle = local.y.atan2(local.x);
606 let phase = if b.is_reversed() { -angle } else { angle }.rem_euclid(core::f64::consts::TAU);
607 let along_a = a.d1_at(lo, tol).ok()?;
608 let along_b = b.d1_at(phase, tol).ok()?;
609 let winding: f64 = if along_a.dot(along_b) >= 0.0 {
610 1.0
611 } else {
612 -1.0
613 };
614 Some(CurveIntersection {
615 crossings: Vec::new(),
616 overlaps: vec![Overlap {
617 on_a: (lo, hi),
618 on_b: (phase, winding.mul_add(hi - lo, phase)),
619 }],
620 })
621}
622
623fn same_curve_3d(a: &Curve, b: &Curve) -> Option<CurveIntersection<Point>> {
630 let (lo, hi) = Curve3d::domain(a);
631 if a == b {
632 return Some(CurveIntersection {
633 crossings: Vec::new(),
634 overlaps: vec![Overlap {
635 on_a: (lo, hi),
636 on_b: (lo, hi),
637 }],
638 });
639 }
640 use ogeom_geom::Reversible as _;
641 if *a == b.clone().reversed() {
642 let (blo, bhi) = Curve3d::domain(b);
643 return Some(CurveIntersection {
644 crossings: Vec::new(),
645 overlaps: vec![Overlap {
646 on_a: (lo, hi),
647 on_b: (bhi, blo),
648 }],
649 });
650 }
651 None
652}
653
654fn same_ellipse_3d(
661 a: &ogeom_geom::EllipseCurve,
662 b: &ogeom_geom::EllipseCurve,
663 tol: Tolerances,
664) -> Option<CurveIntersection<Point>> {
665 let (ea, eb) = (a.ellipse(), b.ellipse());
666 if ea.frame().origin().distance(eb.frame().origin()) > tol.confusion() {
667 return None;
668 }
669 if (ea.major_radius() - eb.major_radius()).abs() > tol.confusion()
670 || (ea.minor_radius() - eb.minor_radius()).abs() > tol.confusion()
671 {
672 return None;
673 }
674 let (za, zb) = (ea.frame().z().vector(), eb.frame().z().vector());
675 if za.cross(zb).magnitude() > tol.angular() {
676 return None;
677 }
678 let (xa, xb) = (ea.frame().x().vector(), eb.frame().x().vector());
679 if xa.cross(xb).magnitude() > tol.angular() {
680 return None;
681 }
682 let (lo, hi) = Curve3d::domain(a);
686 let start = a.point_at(lo, tol).ok()?;
687 let angle = ogeom_math::elementary::ellipse_parameter(&eb, start, tol).ok()?;
688 let phase = if b.is_reversed() { -angle } else { angle }.rem_euclid(core::f64::consts::TAU);
689 let along_a = a.d1_at(lo, tol).ok()?;
690 let along_b = b.d1_at(phase, tol).ok()?;
691 let winding: f64 = if along_a.dot(along_b) >= 0.0 {
692 1.0
693 } else {
694 -1.0
695 };
696 Some(CurveIntersection {
697 crossings: Vec::new(),
698 overlaps: vec![Overlap {
699 on_a: (lo, hi),
700 on_b: (phase, winding.mul_add(hi - lo, phase)),
701 }],
702 })
703}
704
705fn check(options: CurveCurveOptions) -> OgeomResult<()> {
706 if options.samples < 2 {
707 ogeom_bail!(Construction, "seeding needs at least two segments");
708 }
709 if !options.gap.is_finite() || options.gap <= 0.0 {
710 ogeom_bail!(Construction, "a gap of {} is not a distance", options.gap);
711 }
712 Ok(())
713}
714
715fn line_line_2d(
718 a: &ogeom_geom::Line2d,
719 b: &ogeom_geom::Line2d,
720 tol: Tolerances,
721) -> CurveIntersection<Point2> {
722 let (oa, da) = (a.axis().location, a.axis().direction.vector());
723 let (ob, db) = (b.axis().location, b.axis().direction.vector());
724 let cross = da.cross(db);
725
726 if cross.abs() <= tol.angular() {
727 let between = ob - oa;
729 if between.cross(da).abs() > tol.confusion() {
730 return CurveIntersection::empty();
731 }
732 let (a_lo, a_hi) = a.domain();
734 let (b_lo, b_hi) = b.domain();
735 let project = |p: Point2| (p - oa).dot(da);
737 let (s0, s1) = (project(ob + db * b_lo), project(ob + db * b_hi));
738 let (lo, hi) = (s0.min(s1).max(a_lo), s0.max(s1).min(a_hi));
739 if lo >= hi {
740 return CurveIntersection::empty();
741 }
742 let back = |t: f64| (oa + da * t - ob).dot(db);
744 return CurveIntersection {
745 crossings: Vec::new(),
746 overlaps: vec![Overlap {
747 on_a: (lo, hi),
748 on_b: (back(lo), back(hi)),
754 }],
755 };
756 }
757
758 let between = ob - oa;
759 let t = between.cross(db) / cross;
760 let s = between.cross(da) / cross;
761 let (a_lo, a_hi) = a.domain();
762 let (b_lo, b_hi) = b.domain();
763 if t < a_lo - tol.parametric()
764 || t > a_hi + tol.parametric()
765 || s < b_lo - tol.parametric()
766 || s > b_hi + tol.parametric()
767 {
768 return CurveIntersection::empty();
769 }
770 CurveIntersection {
771 crossings: vec![Crossing {
772 on_a: t,
773 on_b: s,
774 point: oa + da * t,
775 gap: 0.0,
776 reach: 0.0,
777 }],
778 overlaps: Vec::new(),
779 }
780}
781
782fn line_circle_2d(
783 line: &ogeom_geom::Line2d,
784 circle: &ogeom_geom::Circle2d,
785 swapped: bool,
786 tol: Tolerances,
787) -> CurveIntersection<Point2> {
788 let (o, d) = (line.axis().location, line.axis().direction.vector());
789 let c = circle.circle();
790 let centre = c.centre();
791 let radius = c.radius();
792
793 let along = (centre - o).dot(d);
795 let foot = o + d * along;
796 let gap = foot.distance(centre);
797 if gap > radius + tol.confusion() {
798 return CurveIntersection::empty();
799 }
800 let half = radius.mul_add(radius, -(gap * gap)).max(0.0).sqrt();
801 let candidates = if half <= tol.confusion() {
802 vec![along]
803 } else {
804 vec![along - half, along + half]
805 };
806
807 let (l_lo, l_hi) = line.domain();
808 let mut crossings = Vec::new();
809 for t in candidates {
810 if t < l_lo - tol.parametric() || t > l_hi + tol.parametric() {
811 continue;
812 }
813 let p = o + d * t;
814 let Some(s) = circle_parameter(circle, p, tol) else {
815 continue;
816 };
817 let (on_a, on_b) = if swapped { (s, t) } else { (t, s) };
818 crossings.push(Crossing {
819 on_a,
820 on_b,
821 point: p,
822 gap: 0.0,
823 reach: 0.0,
824 });
825 }
826 sort_crossings(&mut crossings);
827 CurveIntersection {
828 crossings,
829 overlaps: Vec::new(),
830 }
831}
832
833fn circle_circle_2d(
834 a: &ogeom_geom::Circle2d,
835 b: &ogeom_geom::Circle2d,
836 tol: Tolerances,
837) -> CurveIntersection<Point2> {
838 let (ca, cb) = (a.circle(), b.circle());
839 let between = cb.centre() - ca.centre();
840 let distance = between.magnitude();
841 let (ra, rb) = (ca.radius(), cb.radius());
842
843 if distance <= tol.confusion() {
844 if (ra - rb).abs() <= tol.confusion() {
845 return CurveIntersection {
847 crossings: Vec::new(),
848 overlaps: vec![Overlap {
849 on_a: a.domain(),
850 on_b: b.domain(),
851 }],
852 };
853 }
854 return CurveIntersection::empty();
855 }
856 if distance > ra + rb + tol.confusion() || distance < (ra - rb).abs() - tol.confusion() {
857 return CurveIntersection::empty();
858 }
859
860 let along = distance.mul_add(distance, ra.mul_add(ra, -(rb * rb))) / (2.0 * distance);
862 let squared = ra.mul_add(ra, -(along * along));
863 let direction = between * (1.0 / distance);
864 let foot = ca.centre() + direction * along;
865 let mut crossings = Vec::new();
866 let mut push = |p: Point2| {
867 if let (Some(s), Some(t)) = (circle_parameter(a, p, tol), circle_parameter(b, p, tol)) {
868 crossings.push(Crossing {
869 on_a: s,
870 on_b: t,
871 point: p,
872 gap: 0.0,
873 reach: 0.0,
874 });
875 }
876 };
877 if squared <= tol.confusion() * tol.confusion() {
878 push(foot);
879 } else {
880 let offset = ogeom_math::Vector2::new(-direction.y, direction.x) * squared.max(0.0).sqrt();
881 push(foot + offset);
882 push(foot - offset);
883 }
884 sort_crossings(&mut crossings);
885 CurveIntersection {
886 crossings,
887 overlaps: Vec::new(),
888 }
889}
890
891fn circle_parameter(curve: &ogeom_geom::Circle2d, p: Point2, tol: Tolerances) -> Option<f64> {
893 let c = curve.circle();
894 let local = p - c.centre();
895 let x = local.dot(c.frame().x().vector());
896 let y = local.dot(c.frame().y().vector());
897 let mut angle = y.atan2(x);
898 if curve.is_reversed() {
899 angle = -angle;
900 }
901 let angle = angle.rem_euclid(core::f64::consts::TAU);
902 let (lo, hi) = curve.domain();
903 if angle >= lo - tol.parametric() && angle <= hi + tol.parametric() {
905 return Some(angle.clamp(lo, hi));
906 }
907 let shifted = angle - core::f64::consts::TAU;
908 if shifted >= lo - tol.parametric() && shifted <= hi + tol.parametric() {
909 return Some(shifted.clamp(lo, hi));
910 }
911 None
912}
913
914fn line_line_3d(
917 a: &ogeom_geom::LineCurve,
918 b: &ogeom_geom::LineCurve,
919 options: CurveCurveOptions,
920 tol: Tolerances,
921) -> CurveIntersection<Point> {
922 let (oa, da) = (a.axis().location, a.axis().direction.vector());
923 let (ob, db) = (b.axis().location, b.axis().direction.vector());
924 let cross = da.cross(db);
925 let denominator = cross.square_magnitude();
926
927 if denominator <= tol.angular() * tol.angular() {
928 let between = ob - oa;
930 if between.cross(da).magnitude() > tol.confusion() {
931 return CurveIntersection::empty();
932 }
933 let (a_lo, a_hi) = a.domain();
934 let (b_lo, b_hi) = b.domain();
935 let project = |p: Point| (p - oa).dot(da);
936 let (s0, s1) = (project(ob + db * b_lo), project(ob + db * b_hi));
937 let (lo, hi) = (s0.min(s1).max(a_lo), s0.max(s1).min(a_hi));
938 if lo >= hi {
939 return CurveIntersection::empty();
940 }
941 let back = |t: f64| (oa + da * t - ob).dot(db);
942 return CurveIntersection {
943 crossings: Vec::new(),
944 overlaps: vec![Overlap {
945 on_a: (lo, hi),
946 on_b: (back(lo), back(hi)),
952 }],
953 };
954 }
955
956 let between = ob - oa;
958 let t = between.cross(db).dot(cross) / denominator;
959 let s = between.cross(da).dot(cross) / denominator;
960 let pa = oa + da * t;
961 let pb = ob + db * s;
962 let gap = pa.distance(pb);
963 let (a_lo, a_hi) = a.domain();
964 let (b_lo, b_hi) = b.domain();
965 if gap > options.gap
966 || t < a_lo - tol.parametric()
967 || t > a_hi + tol.parametric()
968 || s < b_lo - tol.parametric()
969 || s > b_hi + tol.parametric()
970 {
971 return CurveIntersection::empty();
972 }
973 CurveIntersection {
974 crossings: vec![Crossing {
975 on_a: t,
976 on_b: s,
977 point: pa,
978 gap,
979 reach: 0.0,
980 }],
981 overlaps: Vec::new(),
982 }
983}
984
985struct Sampled<P> {
989 points: Vec<P>,
990 parameters: Vec<f64>,
991}
992
993fn sample_2d(curve: &PlanarCurve, n: usize, tol: Tolerances) -> Sampled<Point2> {
994 let (lo, hi) = curve.domain();
995 let mut points = Vec::with_capacity(n + 1);
996 let mut parameters = Vec::with_capacity(n + 1);
997 for i in 0..=n {
998 #[allow(clippy::cast_precision_loss)]
999 let t = lo + (hi - lo) * i as f64 / n as f64;
1000 if let Ok(p) = curve.point_at(t, tol) {
1001 points.push(p);
1002 parameters.push(t);
1003 }
1004 }
1005 Sampled { points, parameters }
1006}
1007
1008fn sample_3d(curve: &Curve, n: usize, tol: Tolerances) -> Sampled<Point> {
1009 let (lo, hi) = curve.domain();
1010 let mut points = Vec::with_capacity(n + 1);
1011 let mut parameters = Vec::with_capacity(n + 1);
1012 for i in 0..=n {
1013 #[allow(clippy::cast_precision_loss)]
1014 let t = lo + (hi - lo) * i as f64 / n as f64;
1015 if let Ok(p) = curve.point_at(t, tol) {
1016 points.push(p);
1017 parameters.push(t);
1018 }
1019 }
1020 Sampled { points, parameters }
1021}
1022
1023fn general_2d(
1024 a: &PlanarCurve,
1025 b: &PlanarCurve,
1026 options: CurveCurveOptions,
1027 tol: Tolerances,
1028) -> OgeomResult<CurveIntersection<Point2>> {
1029 let sa = sample_2d(a, options.samples, tol);
1030 let sb = sample_2d(b, options.samples, tol);
1031
1032 let mut crossings: Vec<Crossing<Point2>> = Vec::new();
1033 for i in 1..sa.points.len() {
1034 for j in 1..sb.points.len() {
1035 let Some((ta, tb)) = segments_cross_2d(
1036 (sa.points[i - 1], sa.points[i]),
1037 (sb.points[j - 1], sb.points[j]),
1038 ) else {
1039 continue;
1040 };
1041 let seed_a = sa.parameters[i - 1] + (sa.parameters[i] - sa.parameters[i - 1]) * ta;
1042 let seed_b = sb.parameters[j - 1] + (sb.parameters[j] - sb.parameters[j - 1]) * tb;
1043 if let Some(found) = polish_2d(a, b, seed_a, seed_b, options, tol) {
1044 push_unique_2d(&mut crossings, found, tol);
1045 }
1046 }
1047 }
1048 sort_crossings(&mut crossings);
1049 Ok(CurveIntersection {
1050 crossings,
1051 overlaps: Vec::new(),
1052 })
1053}
1054
1055fn general_3d(
1056 a: &Curve,
1057 b: &Curve,
1058 options: CurveCurveOptions,
1059 tol: Tolerances,
1060) -> OgeomResult<CurveIntersection<Point>> {
1061 let sa = sample_3d(a, options.samples, tol);
1062 let sb = sample_3d(b, options.samples, tol);
1063
1064 let mut reach = options.gap;
1068 for s in [&sa, &sb] {
1069 let longest = s
1070 .points
1071 .windows(2)
1072 .map(|w| w[0].distance(w[1]))
1073 .fold(0.0_f64, f64::max);
1074 reach += longest;
1075 }
1076
1077 let mut crossings: Vec<Crossing<Point>> = Vec::new();
1078 for i in 1..sa.points.len() {
1079 for j in 1..sb.points.len() {
1080 let (ta, tb, gap) = segments_approach_3d(
1081 (sa.points[i - 1], sa.points[i]),
1082 (sb.points[j - 1], sb.points[j]),
1083 );
1084 if gap > reach {
1085 continue;
1086 }
1087 let seed_a = sa.parameters[i - 1] + (sa.parameters[i] - sa.parameters[i - 1]) * ta;
1088 let seed_b = sb.parameters[j - 1] + (sb.parameters[j] - sb.parameters[j - 1]) * tb;
1089 if let Some(found) = polish_3d(a, b, seed_a, seed_b, options, tol) {
1090 push_unique_3d(&mut crossings, found, tol);
1091 }
1092 }
1093 }
1094 sort_crossings(&mut crossings);
1095
1096 if crossings.len() > 1 {
1105 let mut merged: Vec<Crossing<Point>> = Vec::with_capacity(crossings.len());
1106 let mut run_start: Option<Point> = None;
1107 for c in crossings {
1108 if let Some(last) = merged.last_mut()
1109 && contact_between_3d(a, b, last, &c, options, tol)
1110 {
1111 let start = run_start.get_or_insert(last.point);
1112 let reach = start.distance(c.point).max(last.reach);
1113 if c.gap < last.gap {
1114 *last = c;
1115 }
1116 last.reach = reach;
1117 continue;
1118 }
1119 run_start = None;
1120 merged.push(c);
1121 }
1122 if merged.len() > 1 && a.is_periodic() {
1126 let (lo, hi) = a.domain();
1127 let (first, last) = (merged[0], merged[merged.len() - 1]);
1128 let wrapped = Crossing {
1129 on_a: first.on_a + (hi - lo),
1130 ..first
1131 };
1132 if contact_between_3d(a, b, &last, &wrapped, options, tol) {
1133 let reach = last
1134 .reach
1135 .max(first.reach)
1136 .max(last.point.distance(first.point));
1137 let keep = if first.gap <= last.gap {
1138 0
1139 } else {
1140 merged.len() - 1
1141 };
1142 merged[keep].reach = reach;
1143 if keep == 0 {
1144 merged.pop();
1145 } else {
1146 merged.remove(0);
1147 }
1148 }
1149 }
1150 crossings = merged;
1151 }
1152
1153 let overlaps = shared_support_3d(a, b, &sa, &sb, options, tol);
1163 if !overlaps.is_empty() {
1164 crossings.retain(|c| {
1165 !overlaps.iter().any(|o| {
1166 let (lo, hi) = order(o.on_a.0, o.on_a.1);
1167 c.on_a >= lo - tol.parametric() && c.on_a <= hi + tol.parametric()
1168 })
1169 });
1170 }
1171 let gap = options.gap.max(tol.confusion());
1183 let extent = {
1184 let along = |s: &Sampled<Point>| {
1185 s.points
1186 .windows(2)
1187 .map(|w| w[0].distance(w[1]))
1188 .sum::<f64>()
1189 };
1190 along(&sa).min(along(&sb))
1191 };
1192 let cap = 4.0 * (gap * extent).sqrt();
1193 for c in &mut crossings {
1194 let valley = valley_extent_3d(a, b, &sb, c, gap, tol);
1195 if valley > gap * 8.0 && valley <= cap {
1196 c.reach = c.reach.max(valley);
1197 }
1198 }
1199 Ok(CurveIntersection {
1200 crossings,
1201 overlaps,
1202 })
1203}
1204
1205fn contact_between_3d(
1209 a: &Curve,
1210 b: &Curve,
1211 from: &Crossing<Point>,
1212 to: &Crossing<Point>,
1213 options: CurveCurveOptions,
1214 tol: Tolerances,
1215) -> bool {
1216 if (to.on_a - from.on_a).abs() <= tol.parametric() {
1217 return true;
1218 }
1219 (1..=3).all(|k| {
1220 let f = f64::from(k) / 4.0;
1221 let t = from.on_a + (to.on_a - from.on_a) * f;
1222 let seed = from.on_b + (to.on_b - from.on_b) * f;
1223 a.point_at(t, tol)
1224 .ok()
1225 .and_then(|p| foot_on_3d(b, p, seed, tol))
1226 .is_some_and(|(_, gap)| gap <= options.gap)
1227 })
1228}
1229
1230fn shared_support_3d(
1233 a: &Curve,
1234 b: &Curve,
1235 sa: &Sampled<Point>,
1236 sb: &Sampled<Point>,
1237 options: CurveCurveOptions,
1238 tol: Tolerances,
1239) -> Vec<Overlap> {
1240 let foot = |p: Point| -> Option<(f64, f64)> {
1243 let mut seed = (f64::INFINITY, 0.0);
1244 for j in 1..sb.points.len() {
1245 let (_, tb, gap) = segments_approach_3d((p, p), (sb.points[j - 1], sb.points[j]));
1246 if gap < seed.0 {
1247 seed = (
1248 gap,
1249 sb.parameters[j - 1] + (sb.parameters[j] - sb.parameters[j - 1]) * tb,
1250 );
1251 }
1252 }
1253 if !seed.0.is_finite() {
1254 return None;
1255 }
1256 foot_on_3d(b, p, seed.1, tol)
1257 };
1258 let hugs = |t: f64| -> Option<(f64, f64)> {
1259 let p = a.point_at(t, tol).ok()?;
1260 let (s, gap) = foot(p)?;
1261 (gap <= options.gap).then_some((s, gap))
1262 };
1263 let feet: Vec<Option<(f64, f64)>> = sa.points.iter().map(|p| foot(*p)).collect();
1264 let within = |i: usize| feet[i].is_some_and(|(_, gap)| gap <= options.gap);
1265
1266 let mut overlaps = Vec::new();
1267 let mut i = 0;
1268 while i < sa.points.len() {
1269 if !within(i) {
1270 i += 1;
1271 continue;
1272 }
1273 let start = i;
1274 while i + 1 < sa.points.len() && within(i + 1) {
1275 i += 1;
1276 }
1277 let end = i;
1278 i += 1;
1279 if end == start {
1280 continue;
1281 }
1282 let refine = |inside: usize, outside: Option<usize>| -> (f64, f64) {
1285 let (mut t_in, s_in) = (sa.parameters[inside], feet[inside].map_or(0.0, |f| f.0));
1286 let Some(out) = outside else {
1287 return (t_in, s_in);
1288 };
1289 let mut s_at = s_in;
1290 let mut t_out = sa.parameters[out];
1291 for _ in 0..48 {
1292 if (t_out - t_in).abs() <= tol.parametric() {
1293 break;
1294 }
1295 let mid = f64::midpoint(t_in, t_out);
1296 match hugs(mid) {
1297 Some((s, _)) => {
1298 t_in = mid;
1299 s_at = s;
1300 }
1301 None => t_out = mid,
1302 }
1303 }
1304 (t_in, s_at)
1305 };
1306 let (lo_a, lo_b) = refine(start, start.checked_sub(1));
1307 let (hi_a, hi_b) = refine(end, (end + 1 < sa.points.len()).then_some(end + 1));
1308 if hi_a - lo_a <= tol.parametric() || (hi_b - lo_b).abs() <= tol.parametric() {
1309 continue;
1310 }
1311 overlaps.push(Overlap {
1312 on_a: (lo_a, hi_a),
1313 on_b: (lo_b, hi_b),
1314 });
1315 }
1316 overlaps
1317}
1318
1319fn foot_on_3d(curve: &Curve, p: Point, seed: f64, tol: Tolerances) -> Option<(f64, f64)> {
1322 let mut s = clamp_3d(curve, seed);
1323 let mut best = (s, curve.point_at(s, tol).ok()?.distance(p));
1324 for _ in 0..30 {
1325 let d = curve.derivatives_at(s, 2, tol).ok()?;
1326 let zero = ogeom_math::Vector::ZERO;
1327 let (c, d1, d2) = (
1328 d.first().copied().unwrap_or(zero),
1329 d.get(1).copied().unwrap_or(zero),
1330 d.get(2).copied().unwrap_or(zero),
1331 );
1332 let gap = c - (p - Point::ORIGIN);
1333 let g = gap.dot(d1);
1334 let dg = d1.dot(d1) + gap.dot(d2);
1335 if dg.abs() <= f64::MIN_POSITIVE {
1336 break;
1337 }
1338 let next = clamp_3d(curve, s - g / dg);
1339 let dist = curve.point_at(next, tol).ok()?.distance(p);
1340 let moved = (next - s).abs();
1341 s = next;
1342 if dist < best.1 {
1343 best = (s, dist);
1344 }
1345 if moved <= tol.parametric() {
1346 break;
1347 }
1348 }
1349 Some(best)
1350}
1351
1352fn polish_2d(
1354 a: &PlanarCurve,
1355 b: &PlanarCurve,
1356 seed_a: f64,
1357 seed_b: f64,
1358 options: CurveCurveOptions,
1359 tol: Tolerances,
1360) -> Option<Crossing<Point2>> {
1361 let system = |x: &[f64]| {
1362 let (t, s) = (clamp_2d(a, x[0]), clamp_2d(b, x[1]));
1363 let pa = a.point_at(t, tol).unwrap_or(Point2::ORIGIN);
1364 let pb = b.point_at(s, tol).unwrap_or(Point2::ORIGIN);
1365 let da = a
1366 .d1_at(t, tol)
1367 .unwrap_or(ogeom_math::Vector2::new(0.0, 0.0));
1368 let db = b
1369 .d1_at(s, tol)
1370 .unwrap_or(ogeom_math::Vector2::new(0.0, 0.0));
1371 (
1372 vec![pa.x - pb.x, pa.y - pb.y],
1373 vec![vec![da.x, -db.x], vec![da.y, -db.y]],
1374 )
1375 };
1376 let criteria = solve::Criteria {
1377 residual: tol.confusion() * 0.01,
1378 step: tol.parametric(),
1379 max_iterations: 40,
1380 };
1381 let found = solve::newton_system(system, &[seed_a, seed_b], criteria).ok()?;
1382 let (t, s) = (clamp_2d(a, found.value[0]), clamp_2d(b, found.value[1]));
1383 let pa = a.point_at(t, tol).ok()?;
1384 let pb = b.point_at(s, tol).ok()?;
1385 let gap = pa.distance(pb);
1386 if gap > options.gap {
1387 return None;
1388 }
1389 Some(Crossing {
1390 on_a: t,
1391 on_b: s,
1392 point: pa,
1393 gap,
1394 reach: 0.0,
1395 })
1396}
1397
1398fn polish_3d(
1405 a: &Curve,
1406 b: &Curve,
1407 seed_a: f64,
1408 seed_b: f64,
1409 options: CurveCurveOptions,
1410 tol: Tolerances,
1411) -> Option<Crossing<Point>> {
1412 let system = |x: &[f64]| {
1413 let (t, s) = (clamp_3d(a, x[0]), clamp_3d(b, x[1]));
1414 let pa = a.point_at(t, tol).unwrap_or(Point::ORIGIN);
1415 let pb = b.point_at(s, tol).unwrap_or(Point::ORIGIN);
1416 let da = a.derivatives_at(t, 2, tol).unwrap_or_default();
1417 let db = b.derivatives_at(s, 2, tol).unwrap_or_default();
1418 let zero = ogeom_math::Vector::ZERO;
1419 let (d1a, d2a) = (
1420 da.get(1).copied().unwrap_or(zero),
1421 da.get(2).copied().unwrap_or(zero),
1422 );
1423 let (d1b, d2b) = (
1424 db.get(1).copied().unwrap_or(zero),
1425 db.get(2).copied().unwrap_or(zero),
1426 );
1427 let gap = pa - pb;
1428 (
1429 vec![gap.dot(d1a), -gap.dot(d1b)],
1430 vec![
1431 vec![d1a.dot(d1a) + gap.dot(d2a), -d1a.dot(d1b)],
1432 vec![-d1a.dot(d1b), d1b.dot(d1b) - gap.dot(d2b)],
1433 ],
1434 )
1435 };
1436 let criteria = solve::Criteria {
1437 residual: tol.confusion() * 0.01,
1438 step: tol.parametric(),
1439 max_iterations: 40,
1440 };
1441 let found = solve::newton_system(system, &[seed_a, seed_b], criteria).ok()?;
1442 let (t, s) = (clamp_3d(a, found.value[0]), clamp_3d(b, found.value[1]));
1443 let pa = a.point_at(t, tol).ok()?;
1444 let pb = b.point_at(s, tol).ok()?;
1445 let gap = pa.distance(pb);
1446 if gap > options.gap {
1447 return None;
1448 }
1449 Some(Crossing {
1450 on_a: t,
1451 on_b: s,
1452 point: pa,
1453 gap,
1454 reach: 0.0,
1455 })
1456}
1457
1458fn clamp_2d(curve: &PlanarCurve, t: f64) -> f64 {
1461 let (lo, hi) = curve.domain();
1462 if curve.is_periodic() {
1463 let span = hi - lo;
1464 if span > 0.0 {
1465 return lo + (t - lo).rem_euclid(span);
1466 }
1467 }
1468 t.clamp(lo, hi)
1469}
1470
1471fn clamp_3d(curve: &Curve, t: f64) -> f64 {
1472 let (lo, hi) = curve.domain();
1473 if curve.is_periodic() {
1474 let span = hi - lo;
1475 if span > 0.0 {
1476 return lo + (t - lo).rem_euclid(span);
1477 }
1478 }
1479 t.clamp(lo, hi)
1480}
1481
1482fn segments_cross_2d(a: (Point2, Point2), b: (Point2, Point2)) -> Option<(f64, f64)> {
1484 let da = a.1 - a.0;
1485 let db = b.1 - b.0;
1486 let cross = da.cross(db);
1487 if cross.abs() <= f64::MIN_POSITIVE {
1488 return None;
1489 }
1490 let between = b.0 - a.0;
1491 let t = between.cross(db) / cross;
1492 let s = between.cross(da) / cross;
1493 if !(0.0..=1.0).contains(&t) || !(0.0..=1.0).contains(&s) {
1494 return None;
1495 }
1496 Some((t, s))
1497}
1498
1499fn distance_to_curve_3d(b: &Curve, sb: &Sampled<Point>, p: Point, tol: Tolerances) -> f64 {
1502 let mut best = (0_usize, f64::INFINITY);
1503 for i in 1..sb.points.len() {
1504 let (q0, q1) = (sb.points[i - 1], sb.points[i]);
1505 let d = q1 - q0;
1506 let len2 = d.dot(d);
1507 let f = if len2 <= f64::MIN_POSITIVE {
1508 0.0
1509 } else {
1510 ((p - q0).dot(d) / len2).clamp(0.0, 1.0)
1511 };
1512 let dist = p.distance(q0 + d * f);
1513 if dist < best.1 {
1514 best = (i, dist);
1515 }
1516 }
1517 if best.0 == 0 {
1518 return best.1;
1519 }
1520 let (mut lo, mut hi) = (sb.parameters[best.0 - 1], sb.parameters[best.0]);
1521 let at = |t: f64| -> f64 { b.point_at(t, tol).map_or(f64::INFINITY, |q| q.distance(p)) };
1522 let phi = 0.5 * (3.0 - 5.0_f64.sqrt());
1525 let (mut x1, mut x2) = (lo + phi * (hi - lo), hi - phi * (hi - lo));
1526 let (mut f1, mut f2) = (at(x1), at(x2));
1527 for _ in 0..48 {
1528 if f1 < f2 {
1529 hi = x2;
1530 x2 = x1;
1531 f2 = f1;
1532 x1 = lo + phi * (hi - lo);
1533 f1 = at(x1);
1534 } else {
1535 lo = x1;
1536 x1 = x2;
1537 f1 = f2;
1538 x2 = hi - phi * (hi - lo);
1539 f2 = at(x2);
1540 }
1541 }
1542 f1.min(f2).min(best.1)
1543}
1544
1545fn valley_extent_3d(
1548 a: &Curve,
1549 b: &Curve,
1550 sb: &Sampled<Point>,
1551 crossing: &Crossing<Point>,
1552 gap: f64,
1553 tol: Tolerances,
1554) -> f64 {
1555 let (lo, hi) = a.domain();
1556 let span = hi - lo;
1557 if span <= 0.0 {
1558 return 0.0;
1559 }
1560 let mut extent = 0.0_f64;
1561 for direction in [-1.0, 1.0] {
1562 let inside = |t: f64| -> bool {
1563 if t < lo || t > hi {
1564 return false;
1565 }
1566 a.point_at(t, tol)
1567 .is_ok_and(|q| distance_to_curve_3d(b, sb, q, tol) <= gap)
1568 };
1569 let mut step = span * 1e-6;
1570 let mut last_in = crossing.on_a;
1571 let mut first_out: Option<f64> = None;
1572 while step <= span {
1573 let t = crossing.on_a + direction * step;
1574 if inside(t) {
1575 last_in = t;
1576 step *= 2.0;
1577 } else {
1578 first_out = Some(t);
1579 break;
1580 }
1581 }
1582 let edge = match first_out {
1583 Some(mut out) => {
1584 let mut r#in = last_in;
1585 for _ in 0..30 {
1586 let mid = f64::midpoint(r#in, out);
1587 if inside(mid) {
1588 r#in = mid;
1589 } else {
1590 out = mid;
1591 }
1592 }
1593 r#in
1594 }
1595 None => last_in,
1596 };
1597 if let Ok(q) = a.point_at(edge, tol) {
1598 extent = extent.max(q.distance(crossing.point));
1599 }
1600 }
1601 extent
1602}
1603
1604fn segments_approach_3d(a: (Point, Point), b: (Point, Point)) -> (f64, f64, f64) {
1606 let da = a.1 - a.0;
1607 let db = b.1 - b.0;
1608 let between = a.0 - b.0;
1609 let (aa, bb, ab) = (da.dot(da), db.dot(db), da.dot(db));
1610 let (ad, bd) = (da.dot(between), db.dot(between));
1611 let denominator = ab.mul_add(-ab, aa * bb);
1612
1613 let (mut t, mut s) = if denominator.abs() <= f64::MIN_POSITIVE {
1614 (
1615 0.0,
1616 if bb > 0.0 {
1617 (bd / bb).clamp(0.0, 1.0)
1618 } else {
1619 0.0
1620 },
1621 )
1622 } else {
1623 (
1624 (ab.mul_add(bd, -(bb * ad)) / denominator).clamp(0.0, 1.0),
1625 (aa.mul_add(bd, -(ab * ad)) / denominator).clamp(0.0, 1.0),
1626 )
1627 };
1628 if bb > 0.0 {
1630 s = ((da.dot(between) + t * aa - 0.0).mul_add(0.0, db.dot(between + da * t)) / bb)
1631 .clamp(0.0, 1.0);
1632 }
1633 if aa > 0.0 {
1634 t = (da.dot(db * s - between) / aa).clamp(0.0, 1.0);
1635 }
1636 let pa = a.0 + da * t;
1637 let pb = b.0 + db * s;
1638 (t, s, pa.distance(pb))
1639}
1640
1641fn order(a: f64, b: f64) -> (f64, f64) {
1642 if a <= b { (a, b) } else { (b, a) }
1643}
1644
1645fn sort_crossings<P>(crossings: &mut [Crossing<P>]) {
1646 crossings.sort_by(|x, y| {
1647 x.on_a
1648 .partial_cmp(&y.on_a)
1649 .unwrap_or(core::cmp::Ordering::Equal)
1650 });
1651}
1652
1653fn push_unique_2d(crossings: &mut Vec<Crossing<Point2>>, found: Crossing<Point2>, tol: Tolerances) {
1654 let reach = tol.confusion() * 100.0;
1655 if crossings
1656 .iter()
1657 .any(|c| c.point.distance(found.point) <= reach)
1658 {
1659 return;
1660 }
1661 crossings.push(found);
1662}
1663
1664fn push_unique_3d(crossings: &mut Vec<Crossing<Point>>, found: Crossing<Point>, tol: Tolerances) {
1665 let reach = tol.confusion() * 100.0;
1666 if crossings
1667 .iter()
1668 .any(|c| c.point.distance(found.point) <= reach)
1669 {
1670 return;
1671 }
1672 crossings.push(found);
1673}
1674
1675#[cfg(test)]
1676#[allow(clippy::unwrap_used)]
1677mod tests {
1678 use super::*;
1679 use ogeom_geom::{BSpline2d, Circle2d, CircleCurve, Line2d, LineCurve};
1680 use ogeom_math::{Circle, Circle2, Direction2, Frame, Frame2, KnotVector, Vector2};
1681
1682 const T: Tolerances = Tolerances::millimetres();
1683
1684 fn line2(from: Point2, to: Point2) -> PlanarCurve {
1685 Line2d::segment(from, to, T).unwrap().into()
1686 }
1687
1688 fn circle2(centre: Point2, radius: f64) -> PlanarCurve {
1689 Circle2d::new(
1690 Circle2::new(
1691 Frame2::new(centre, Direction2::new(Vector2::new(1.0, 0.0), T).unwrap()),
1692 radius,
1693 T,
1694 )
1695 .unwrap(),
1696 )
1697 .into()
1698 }
1699
1700 #[test]
1701 fn two_lines_cross_where_algebra_says() {
1702 let a = line2(Point2::new(0.0, 0.0), Point2::new(4.0, 4.0));
1703 let b = line2(Point2::new(0.0, 4.0), Point2::new(4.0, 0.0));
1704 let found = intersect_curves_2d(&a, &b, CurveCurveOptions::default(), T).unwrap();
1705 assert_eq!(found.crossings.len(), 1);
1706 let hit = &found.crossings[0];
1707 assert!(hit.point.is_equal(Point2::new(2.0, 2.0), T));
1708 approx::assert_relative_eq!(hit.on_a, 8.0_f64.sqrt(), epsilon = 1e-9);
1710
1711 let short = line2(Point2::new(0.0, 4.0), Point2::new(1.0, 3.0));
1713 assert!(
1714 intersect_curves_2d(&a, &short, CurveCurveOptions::default(), T)
1715 .unwrap()
1716 .is_empty()
1717 );
1718 }
1719
1720 #[test]
1721 fn collinear_lines_overlap_rather_than_crossing_everywhere() {
1722 let a = line2(Point2::new(0.0, 0.0), Point2::new(10.0, 0.0));
1723 let b = line2(Point2::new(4.0, 0.0), Point2::new(20.0, 0.0));
1724 let found = intersect_curves_2d(&a, &b, CurveCurveOptions::default(), T).unwrap();
1725 assert!(found.crossings.is_empty());
1726 assert_eq!(found.overlaps.len(), 1);
1727 let overlap = &found.overlaps[0];
1728 approx::assert_relative_eq!(overlap.on_a.0, 4.0, epsilon = 1e-9);
1729 approx::assert_relative_eq!(overlap.on_a.1, 10.0, epsilon = 1e-9);
1730 approx::assert_relative_eq!(overlap.on_b.0, 0.0, epsilon = 1e-9);
1731 approx::assert_relative_eq!(overlap.on_b.1, 6.0, epsilon = 1e-9);
1732
1733 let above = line2(Point2::new(0.0, 1.0), Point2::new(10.0, 1.0));
1735 assert!(
1736 intersect_curves_2d(&a, &above, CurveCurveOptions::default(), T)
1737 .unwrap()
1738 .is_empty()
1739 );
1740 }
1741
1742 #[test]
1743 fn a_line_meets_a_circle_in_two_points_one_or_none() {
1744 let circle = circle2(Point2::new(0.0, 0.0), 2.0);
1745 let through = line2(Point2::new(-5.0, 0.0), Point2::new(5.0, 0.0));
1746 let found =
1747 intersect_curves_2d(&through, &circle, CurveCurveOptions::default(), T).unwrap();
1748 assert_eq!(found.crossings.len(), 2);
1749 for hit in &found.crossings {
1750 approx::assert_relative_eq!(
1751 hit.point.distance(Point2::new(0.0, 0.0)),
1752 2.0,
1753 epsilon = 1e-9
1754 );
1755 let PlanarCurve::Circle(_) = &circle else {
1757 unreachable!()
1758 };
1759 let on_circle = circle.point_at(hit.on_b, T).unwrap();
1760 assert!(on_circle.is_equal(hit.point, T));
1761 }
1762
1763 let tangent = line2(Point2::new(-5.0, 2.0), Point2::new(5.0, 2.0));
1764 assert_eq!(
1765 intersect_curves_2d(&tangent, &circle, CurveCurveOptions::default(), T)
1766 .unwrap()
1767 .crossings
1768 .len(),
1769 1
1770 );
1771 let missing = line2(Point2::new(-5.0, 3.0), Point2::new(5.0, 3.0));
1772 assert!(
1773 intersect_curves_2d(&missing, &circle, CurveCurveOptions::default(), T)
1774 .unwrap()
1775 .is_empty()
1776 );
1777 }
1778
1779 #[test]
1780 fn two_circles_cross_touch_coincide_or_miss() {
1781 let a = circle2(Point2::new(0.0, 0.0), 2.0);
1782
1783 let crossing = circle2(Point2::new(3.0, 0.0), 2.0);
1784 let found = intersect_curves_2d(&a, &crossing, CurveCurveOptions::default(), T).unwrap();
1785 assert_eq!(found.crossings.len(), 2);
1786 for hit in &found.crossings {
1787 let on_a = a.point_at(hit.on_a, T).unwrap();
1788 let on_b = crossing.point_at(hit.on_b, T).unwrap();
1789 assert!(on_a.is_equal(hit.point, T));
1790 assert!(on_b.is_equal(hit.point, T));
1791 }
1792
1793 let touching = circle2(Point2::new(4.0, 0.0), 2.0);
1794 assert_eq!(
1795 intersect_curves_2d(&a, &touching, CurveCurveOptions::default(), T)
1796 .unwrap()
1797 .crossings
1798 .len(),
1799 1
1800 );
1801
1802 let same = circle2(Point2::new(0.0, 0.0), 2.0);
1803 let coincident = intersect_curves_2d(&a, &same, CurveCurveOptions::default(), T).unwrap();
1804 assert!(coincident.crossings.is_empty());
1805 assert_eq!(coincident.overlaps.len(), 1);
1806
1807 let apart = circle2(Point2::new(10.0, 0.0), 2.0);
1808 assert!(
1809 intersect_curves_2d(&a, &apart, CurveCurveOptions::default(), T)
1810 .unwrap()
1811 .is_empty()
1812 );
1813 }
1814
1815 #[test]
1816 fn the_general_path_handles_what_has_no_closed_form() {
1817 let wave: PlanarCurve = BSpline2d::new(
1820 KnotVector::new(vec![0.0, 0.0, 0.0, 0.0, 0.5, 1.0, 1.0, 1.0, 1.0], 3).unwrap(),
1821 vec![
1822 Point2::new(0.0, -1.0),
1823 Point2::new(1.0, 3.0),
1824 Point2::new(2.0, -3.0),
1825 Point2::new(3.0, 3.0),
1826 Point2::new(4.0, -1.0),
1827 ],
1828 T,
1829 )
1830 .unwrap()
1831 .into();
1832 let axis = line2(Point2::new(-1.0, 0.0), Point2::new(5.0, 0.0));
1833 let found = intersect_curves_2d(&wave, &axis, CurveCurveOptions::default(), T).unwrap();
1834 assert_eq!(found.crossings.len(), 3, "a wave crosses its axis thrice");
1835 for hit in &found.crossings {
1836 assert!(hit.gap < 1e-9);
1837 assert!(hit.point.y.abs() < 1e-9);
1838 let on_wave = wave.point_at(hit.on_a, T).unwrap();
1839 assert!(on_wave.is_equal(hit.point, T));
1840 }
1841 }
1842
1843 #[test]
1850 fn one_circle_written_twice_states_the_correspondence_between_them() {
1851 use ogeom_math::{Direction, Vector};
1852 let a: Curve = CircleCurve::new(Circle::new(Frame::WORLD, 3.0, T).unwrap()).into();
1853 let third = 2.0 * core::f64::consts::PI / 3.0;
1856 let flipped = Frame::new(
1857 Point::ORIGIN,
1858 -Direction::Z,
1859 Direction::new(Vector::new(third.cos(), third.sin(), 0.0), T).unwrap(),
1860 T,
1861 )
1862 .unwrap();
1863 let b: Curve = CircleCurve::new(Circle::new(flipped, 3.0, T).unwrap()).into();
1864 for pair in [(&a, &b), (&b, &a)] {
1865 let found = intersect_curves(pair.0, pair.1, CurveCurveOptions::default(), T).unwrap();
1866 assert!(
1867 found.crossings.is_empty(),
1868 "every point is a hit, so none is"
1869 );
1870 assert_eq!(found.overlaps.len(), 1);
1871 let overlap = &found.overlaps[0];
1872 let span = overlap.on_a.1 - overlap.on_a.0;
1873 for i in 0..=8 {
1874 let t = f64::from(i) / 8.0;
1875 let ta = span.mul_add(t, overlap.on_a.0);
1876 let tb = (overlap.on_b.1 - overlap.on_b.0).mul_add(t, overlap.on_b.0);
1877 let pa = pair.0.point_at(ta, T).unwrap();
1878 let pb = pair
1879 .1
1880 .point_at(tb.rem_euclid(core::f64::consts::TAU), T)
1881 .unwrap();
1882 assert!(
1883 pa.distance(pb) < 1e-9,
1884 "at {t}: {pa:?} against {pb:?} (on_a {:?} on_b {:?})",
1885 overlap.on_a,
1886 overlap.on_b
1887 );
1888 }
1889 }
1890 }
1891
1892 #[test]
1893 fn space_curves_cross_within_a_gap_and_report_it() {
1894 let a: Curve = CircleCurve::new(Circle::new(Frame::WORLD, 2.0, T).unwrap()).into();
1901 let lifted = Frame::new(
1902 Point::new(3.0, 0.0, 0.001),
1903 ogeom_math::Direction::Z,
1904 ogeom_math::Direction::X,
1905 T,
1906 )
1907 .unwrap();
1908 let b: Curve = CircleCurve::new(Circle::new(lifted, 2.0, T).unwrap()).into();
1909
1910 let options = CurveCurveOptions {
1911 gap: 1e-2,
1912 ..CurveCurveOptions::default()
1913 };
1914 let found = intersect_curves(&a, &b, options, T).unwrap();
1915 assert_eq!(found.crossings.len(), 2, "two near-crossings");
1916 for hit in &found.crossings {
1917 assert!(hit.gap > 1e-4, "the gap is real and must not be zeroed");
1918 assert!(hit.gap < 2e-3, "but small: {}", hit.gap);
1919 }
1920
1921 let strict = CurveCurveOptions {
1923 gap: 1e-5,
1924 ..CurveCurveOptions::default()
1925 };
1926 assert!(intersect_curves(&a, &b, strict, T).unwrap().is_empty());
1927 }
1928
1929 #[test]
1930 fn skew_lines_in_space_miss_and_close_ones_meet() {
1931 let a: Curve = LineCurve::segment(Point::ORIGIN, Point::new(10.0, 0.0, 0.0), T)
1932 .unwrap()
1933 .into();
1934 let skew: Curve =
1935 LineCurve::segment(Point::new(0.0, -5.0, 1.0), Point::new(0.0, 5.0, 1.0), T)
1936 .unwrap()
1937 .into();
1938 assert!(
1939 intersect_curves(&a, &skew, CurveCurveOptions::default(), T)
1940 .unwrap()
1941 .is_empty(),
1942 "a unit apart is not a crossing"
1943 );
1944
1945 let meeting: Curve =
1946 LineCurve::segment(Point::new(5.0, -5.0, 0.0), Point::new(5.0, 5.0, 0.0), T)
1947 .unwrap()
1948 .into();
1949 let found = intersect_curves(&a, &meeting, CurveCurveOptions::default(), T).unwrap();
1950 assert_eq!(found.crossings.len(), 1);
1951 assert!(
1952 found.crossings[0]
1953 .point
1954 .is_equal(Point::new(5.0, 0.0, 0.0), T)
1955 );
1956 assert!(found.crossings[0].gap < 1e-12);
1957
1958 let collinear: Curve =
1960 LineCurve::segment(Point::new(4.0, 0.0, 0.0), Point::new(20.0, 0.0, 0.0), T)
1961 .unwrap()
1962 .into();
1963 let shared = intersect_curves(&a, &collinear, CurveCurveOptions::default(), T).unwrap();
1964 assert_eq!(shared.overlaps.len(), 1);
1965 }
1966
1967 #[test]
1968 fn a_fitted_curve_tracing_an_arc_is_one_overlap_not_a_row_of_crossings() {
1969 let circle: Curve = CircleCurve::new(Circle::new(Frame::WORLD, 4.0, T).unwrap()).into();
1975 let points: Vec<Point> = (0..=40)
1976 .map(|i| {
1977 let a = 0.2 + 1.0 * f64::from(i) / 40.0;
1978 Point::new(4.0 * a.cos(), 4.0 * a.sin(), 0.0)
1979 })
1980 .collect();
1981 let fitted: Curve = ogeom_geom::fit::fit_points(&points, 3, 1e-7, T)
1982 .unwrap()
1983 .curve
1984 .into();
1985 let options = CurveCurveOptions {
1986 gap: 1e-5,
1987 ..CurveCurveOptions::default()
1988 };
1989 let found = intersect_curves(&fitted, &circle, options, T).unwrap();
1990 assert_eq!(found.overlaps.len(), 1, "one shared stretch: {found:?}");
1991 let (lo, hi) = found.overlaps[0].on_a;
1992 let (fa, fb) = fitted.domain();
1993 assert!(
1994 lo - fa < 1e-3 && fb - hi < 1e-3,
1995 "the whole fit runs along the circle"
1996 );
1997 assert!(
1998 found.crossings.is_empty(),
1999 "no crossing survives inside the overlap: {:?}",
2000 found.crossings
2001 );
2002 }
2003
2004 #[test]
2005 fn an_arc_ending_tangent_to_a_line_is_one_crossing_with_its_reach() {
2006 let circle: Curve = CircleCurve::new(Circle::new(Frame::WORLD, 4.0, T).unwrap()).into();
2011 let line: Curve =
2012 LineCurve::segment(Point::new(4.0, -3.0, 0.0), Point::new(4.0, 3.0, 0.0), T)
2013 .unwrap()
2014 .into();
2015 let options = CurveCurveOptions {
2016 gap: 1e-5,
2017 ..CurveCurveOptions::default()
2018 };
2019 let found = intersect_curves(&circle, &line, options, T).unwrap();
2020 assert_eq!(found.crossings.len(), 1, "one touch: {:?}", found.crossings);
2021 let touch = found.crossings[0];
2022 assert!(
2023 touch.point.distance(Point::new(4.0, 0.0, 0.0)) < 2e-2,
2024 "{touch:?}"
2025 );
2026 assert!(touch.reach < 5e-2, "the valley is short: {touch:?}");
2027 assert!(found.overlaps.is_empty());
2028 }
2029
2030 #[test]
2031 fn unusable_options_are_refused() {
2032 let a = line2(Point2::new(0.0, 0.0), Point2::new(1.0, 0.0));
2033 for options in [
2034 CurveCurveOptions {
2035 samples: 1,
2036 ..CurveCurveOptions::default()
2037 },
2038 CurveCurveOptions {
2039 gap: 0.0,
2040 ..CurveCurveOptions::default()
2041 },
2042 CurveCurveOptions {
2043 gap: f64::NAN,
2044 ..CurveCurveOptions::default()
2045 },
2046 ] {
2047 assert!(intersect_curves_2d(&a, &a.clone(), options, T).is_err());
2048 }
2049 }
2050
2051 #[test]
2056 fn circles_and_ellipses_in_different_planes_meet_where_the_planes_do() {
2057 use ogeom_geom::EllipseCurve;
2058 use ogeom_math::{Direction, Ellipse, Vector};
2059 let radius = 2.0;
2060 let tilted = |normal: Vector, major: Vector, lean: f64| -> Curve {
2061 let frame = Frame::new(
2062 Point::ORIGIN,
2063 Direction::new(normal, T).unwrap(),
2064 Direction::new(major, T).unwrap(),
2065 T,
2066 )
2067 .unwrap();
2068 EllipseCurve::new(Ellipse::new(frame, radius / lean.cos(), radius, T).unwrap()).into()
2069 };
2070 let (p, q) = (0.4_f64, 0.7_f64);
2071 let about_x = tilted(
2072 Vector::new(0.0, -p.sin(), p.cos()),
2073 Vector::new(0.0, p.cos(), p.sin()),
2074 p,
2075 );
2076 let about_y = tilted(
2077 Vector::new(-q.sin(), 0.0, q.cos()),
2078 Vector::new(q.cos(), 0.0, q.sin()),
2079 q,
2080 );
2081 let circle: Curve = CircleCurve::new(Circle::new(Frame::WORLD, radius, T).unwrap()).into();
2082 for (a, b) in [
2083 (&about_x, &about_y),
2084 (&about_y, &about_x),
2085 (&circle, &about_x),
2086 (&about_y, &circle),
2087 ] {
2088 let found = intersect_curves(a, b, CurveCurveOptions::default(), T).unwrap();
2089 assert_eq!(found.crossings.len(), 2, "{found:?}");
2090 for hit in &found.crossings {
2091 let on_a = a.point_at(hit.on_a, T).unwrap();
2092 let on_b = b.point_at(hit.on_b, T).unwrap();
2093 assert!(on_a.distance(on_b) < 1e-9, "{on_a:?} against {on_b:?}");
2094 assert!((on_a.x.hypot(on_a.y) - radius).abs() < 1e-9);
2095 }
2096 }
2097 let lifted = Frame::new(Point::new(0.0, 0.0, 1.0), Direction::Z, Direction::X, T).unwrap();
2098 let above: Curve = CircleCurve::new(Circle::new(lifted, radius, T).unwrap()).into();
2099 assert!(
2100 intersect_curves(&circle, &above, CurveCurveOptions::default(), T)
2101 .unwrap()
2102 .is_empty()
2103 );
2104 }
2105
2106 #[test]
2111 fn circles_in_all_but_one_plane_meet_where_they_pass() {
2112 use ogeom_math::{Direction, Vector};
2113 let flat: Curve = CircleCurve::new(Circle::new(Frame::WORLD, 2.0, T).unwrap()).into();
2114 let lean = 1e-6;
2115 let normal = Direction::new(Vector::new(lean, 0.0, 1.0), T).unwrap();
2116 let centre = Point::new(1.0, 0.0, lean.mul_add(-0.5, 5e-8));
2117 let tilted_frame = Frame::new(
2118 centre,
2119 normal,
2120 Direction::new(Vector::new(1.0, 0.0, -lean), T).unwrap(),
2121 T,
2122 )
2123 .unwrap();
2124 let tilted: Curve = CircleCurve::new(Circle::new(tilted_frame, 2.0, T).unwrap()).into();
2125 let found = intersect_curves(&flat, &tilted, CurveCurveOptions::default(), T).unwrap();
2126 assert_eq!(found.crossings.len(), 2, "{found:?}");
2127 for hit in &found.crossings {
2128 assert!(hit.gap < 1e-7, "{hit:?}");
2129 let p = flat.point_at(hit.on_a, T).unwrap();
2130 assert!((p.x - 0.5).abs() < 1e-4, "{p:?}");
2131 }
2132 }
2133
2134 #[test]
2139 fn lines_meet_circles_and_ellipses_in_closed_form() {
2140 use ogeom_geom::EllipseCurve;
2141 use ogeom_math::Ellipse;
2142 let ellipse: Curve =
2143 EllipseCurve::new(Ellipse::new(Frame::WORLD, 3.0, 2.0, T).unwrap()).into();
2144 let across: Curve =
2145 LineCurve::segment(Point::new(-5.0, 1.0, 0.0), Point::new(5.0, 1.0, 0.0), T)
2146 .unwrap()
2147 .into();
2148 for (a, b, line_first) in [(&ellipse, &across, false), (&across, &ellipse, true)] {
2149 let found = intersect_curves(a, b, CurveCurveOptions::default(), T).unwrap();
2150 assert_eq!(found.crossings.len(), 2, "{found:?}");
2151 for hit in &found.crossings {
2152 let p = a.point_at(hit.on_a, T).unwrap();
2153 let q = b.point_at(hit.on_b, T).unwrap();
2154 assert!(p.distance(q) < 1e-9, "{p:?} against {q:?}");
2155 let on_line = if line_first { p } else { q };
2156 assert!((on_line.y - 1.0).abs() < 1e-12);
2157 }
2158 }
2159 let circle: Curve = CircleCurve::new(Circle::new(Frame::WORLD, 2.0, T).unwrap()).into();
2160 let through: Curve =
2161 LineCurve::segment(Point::new(2.0, 0.0, -1.0), Point::new(2.0, 0.0, 1.0), T)
2162 .unwrap()
2163 .into();
2164 let found = intersect_curves(&circle, &through, CurveCurveOptions::default(), T).unwrap();
2165 assert_eq!(found.crossings.len(), 1);
2166 assert!(found.crossings[0].on_a.abs() < 1e-9);
2167 assert!((found.crossings[0].on_b - 1.0).abs() < 1e-9);
2168 let inside: Curve =
2169 LineCurve::segment(Point::new(1.0, 0.0, -1.0), Point::new(1.0, 0.0, 1.0), T)
2170 .unwrap()
2171 .into();
2172 assert!(
2173 intersect_curves(&circle, &inside, CurveCurveOptions::default(), T)
2174 .unwrap()
2175 .is_empty()
2176 );
2177 }
2178}