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 let (basis_a, window_a) = through_trim_2d(a);
148 let (basis_b, window_b) = through_trim_2d(b);
149 let found = match (basis_a, basis_b) {
150 (PlanarCurve::Line(x), PlanarCurve::Line(y)) => line_line_2d(x, y, tol),
151 (PlanarCurve::Line(x), PlanarCurve::Circle(y)) => line_circle_2d(x, y, false, tol),
152 (PlanarCurve::Circle(x), PlanarCurve::Line(y)) => line_circle_2d(y, x, true, tol),
153 (PlanarCurve::Circle(x), PlanarCurve::Circle(y)) => circle_circle_2d(x, y, tol),
154 _ => return general_2d(a, b, options, tol),
155 };
156 let side = |curve: &PlanarCurve, window: Option<(f64, f64)>| Side {
157 period: {
158 let (lo, hi) = curve.domain();
159 (curve.is_periodic() && hi > lo).then_some(hi - lo)
160 },
161 domain: curve.domain(),
162 window,
163 };
164 Ok(clipped_to_windows(
165 found,
166 side(basis_a, window_a),
167 side(basis_b, window_b),
168 tol,
169 ))
170}
171
172fn through_trim_2d(curve: &PlanarCurve) -> (&PlanarCurve, Option<(f64, f64)>) {
175 match curve {
176 PlanarCurve::Trimmed(t) if !t.is_reversed() => (t.basis(), Some(t.domain())),
177 other => (other, None),
178 }
179}
180
181#[derive(Debug, Clone, Copy)]
184struct Side {
185 period: Option<f64>,
186 domain: (f64, f64),
187 window: Option<(f64, f64)>,
188}
189
190impl Side {
191 fn of(curve: &Curve, window: Option<(f64, f64)>) -> Self {
192 let (lo, hi) = curve.domain();
193 Self {
194 period: (curve.is_periodic() && hi > lo).then_some(hi - lo),
195 domain: (lo, hi),
196 window,
197 }
198 }
199}
200
201pub fn intersect_curves(
207 a: &Curve,
208 b: &Curve,
209 options: CurveCurveOptions,
210 tol: Tolerances,
211) -> OgeomResult<CurveIntersection<Point>> {
212 check(options)?;
213 let (basis_a, window_a) = through_trim(a);
214 let (basis_b, window_b) = through_trim(b);
215 if let Some(found) = same_curve_3d(basis_a, basis_b) {
216 return Ok(clipped_to_windows(
217 found,
218 Side::of(basis_a, window_a),
219 Side::of(basis_b, window_b),
220 tol,
221 ));
222 }
223 if let Some(found) = analytic_3d(basis_a, basis_b, options, tol) {
224 return Ok(clipped_to_windows(
225 found,
226 Side::of(basis_a, window_a),
227 Side::of(basis_b, window_b),
228 tol,
229 ));
230 }
231 if let Some(found) = tangent_line_conic_3d(a, b, options, tol) {
232 return Ok(found);
233 }
234 general_3d(a, b, options, tol)
235}
236
237fn through_trim(curve: &Curve) -> (&Curve, Option<(f64, f64)>) {
245 match curve {
246 Curve::Trimmed(t) if !t.is_reversed() => (t.basis(), Some(Curve3d::domain(&**t))),
247 other => (other, None),
248 }
249}
250
251fn analytic_3d(
254 a: &Curve,
255 b: &Curve,
256 options: CurveCurveOptions,
257 tol: Tolerances,
258) -> Option<CurveIntersection<Point>> {
259 match (a, b) {
260 (Curve::Line(x), Curve::Line(y)) => Some(line_line_3d(x, y, options, tol)),
261 (Curve::Circle(x), Curve::Circle(y)) => same_circle_3d(x, y, tol)
262 .or_else(|| coplanar_circles_3d(x, y, options, tol))
263 .or_else(|| skew_conics_3d(a, b, options, tol)),
264 (Curve::Ellipse(x), Curve::Ellipse(y)) => {
265 same_ellipse_3d(x, y, tol).or_else(|| skew_conics_3d(a, b, options, tol))
266 }
267 (Curve::Circle(_), Curve::Ellipse(_)) | (Curve::Ellipse(_), Curve::Circle(_)) => {
268 skew_conics_3d(a, b, options, tol)
269 }
270 (Curve::Line(x), Curve::Circle(_) | Curve::Ellipse(_)) => line_conic_3d(x, b, options, tol)
271 .map(|mut found| {
272 for c in &mut found.crossings {
273 core::mem::swap(&mut c.on_a, &mut c.on_b);
274 }
275 found.crossings.sort_by(|x, y| x.on_a.total_cmp(&y.on_a));
276 found
277 }),
278 (Curve::Circle(_) | Curve::Ellipse(_), Curve::Line(y)) => line_conic_3d(y, a, options, tol),
279 _ => None,
280 }
281}
282
283fn conic_of(
287 curve: &Curve,
288) -> Option<(
289 Point,
290 ogeom_math::Vector,
291 ogeom_math::Vector,
292 ogeom_math::Vector,
293)> {
294 let (centre, [x, y, z], [ra, rb]) = conic_axes(curve)?;
295 Some((centre, x * ra, y * rb, z))
296}
297
298fn conic_axes(curve: &Curve) -> Option<(Point, [ogeom_math::Vector; 3], [f64; 2])> {
301 let (frame, radii) = match curve {
302 Curve::Circle(c) if !c.is_reversed() => {
303 let circle = c.circle();
304 (circle.frame(), [circle.radius(); 2])
305 }
306 Curve::Ellipse(e) if !e.is_reversed() => {
307 let ellipse = e.ellipse();
308 (
309 ellipse.frame(),
310 [ellipse.major_radius(), ellipse.minor_radius()],
311 )
312 }
313 _ => return None,
314 };
315 Some((
316 frame.origin(),
317 [frame.x().vector(), frame.y().vector(), frame.z().vector()],
318 radii,
319 ))
320}
321
322fn skew_conics_3d(
328 a: &Curve,
329 b: &Curve,
330 options: CurveCurveOptions,
331 tol: Tolerances,
332) -> Option<CurveIntersection<Point>> {
333 let (ca, ua, va, na) = conic_of(a)?;
334 let (cb, _, _, nb) = conic_of(b)?;
335 if na.cross(nb).magnitude() <= tol.angular() {
336 return ((ca - cb).dot(nb).abs() > options.gap.max(tol.confusion()))
337 .then(CurveIntersection::empty);
338 }
339 let (alpha, beta, gamma) = (nb.dot(ua), nb.dot(va), nb.dot(ca - cb));
340 let size = alpha.hypot(beta);
341 if options.gap.max(tol.confusion()) > size * 1e-3 {
348 return None;
349 }
350 let mut crossings: Vec<Crossing<Point>> = Vec::new();
351 if size > 0.0 && gamma.abs() <= size * (1.0 + 1e-12) {
352 let phase = beta.atan2(alpha);
353 let turn = (-gamma / size).clamp(-1.0, 1.0).acos();
354 let (a_lo, a_hi) = a.domain();
355 let (b_lo, b_hi) = b.domain();
356 let tau = core::f64::consts::TAU;
357 let into = |t: f64, lo: f64| lo + (t - lo).rem_euclid(tau);
358 let roots = if turn <= 1e-12 {
359 vec![phase]
360 } else {
361 vec![phase - turn, phase + turn]
362 };
363 for root in roots {
364 let t = into(root, a_lo);
365 if t > a_hi + tol.parametric() {
366 continue;
367 }
368 let point = a.point_at(t, tol).ok()?;
369 if (point - cb).dot(nb).abs() > tol.confusion() * 10.0 {
373 return None;
374 }
375 let s = match b {
376 Curve::Circle(c) => {
377 ogeom_math::elementary::circle_parameter(&c.circle(), point, tol)
378 }
379 Curve::Ellipse(e) => {
380 ogeom_math::elementary::ellipse_parameter(&e.ellipse(), point, tol)
381 }
382 _ => return None,
383 };
384 let Ok(s) = s else {
385 continue;
386 };
387 let s = into(s, b_lo);
388 if s > b_hi + tol.parametric() {
389 continue;
390 }
391 let gap = b.point_at(s, tol).ok()?.distance(point);
392 if gap > options.gap {
393 continue;
394 }
395 crossings.push(Crossing {
396 on_a: t,
397 on_b: s,
398 point,
399 gap,
400 reach: 0.0,
401 });
402 }
403 }
404 crossings.sort_by(|x, y| x.on_a.total_cmp(&y.on_a));
405 Some(CurveIntersection {
406 crossings,
407 overlaps: Vec::new(),
408 })
409}
410
411fn line_conic_3d(
423 line: &ogeom_geom::LineCurve,
424 conic: &Curve,
425 options: CurveCurveOptions,
426 tol: Tolerances,
427) -> Option<CurveIntersection<Point>> {
428 let (centre, u, v, normal) = conic_of(conic)?;
429 let (origin, along) = (line.axis().location, line.axis().direction.vector());
430 let (a_len, b_len) = (u.magnitude(), v.magnitude());
431 if a_len <= tol.confusion() || b_len <= tol.confusion() {
432 return None;
433 }
434 let (ux, vy) = (u / a_len, v / b_len);
435 let lean = along.dot(normal);
436 let height = (origin - centre).dot(normal);
437 let mut ts: Vec<f64> = Vec::new();
438 let mut deep = false;
439 if lean.abs() <= tol.angular() {
440 if height.abs() > options.gap.max(tol.confusion()) {
441 return Some(CurveIntersection::empty());
442 }
443 let (x0, y0) = ((origin - centre).dot(ux), (origin - centre).dot(vy));
445 let (dx, dy) = (along.dot(ux), along.dot(vy));
446 let qa = dx * dx / (a_len * a_len) + dy * dy / (b_len * b_len);
447 let qb = 2.0 * (x0 * dx / (a_len * a_len) + y0 * dy / (b_len * b_len));
448 let qc = x0 * x0 / (a_len * a_len) + y0 * y0 / (b_len * b_len) - 1.0;
449 let disc = qb.mul_add(qb, -4.0 * qa * qc);
450 if qa <= 0.0 {
451 return None;
452 }
453 if disc < 0.0 {
454 let t = -qb / (2.0 * qa);
455 let p = origin + along * t;
456 let foot = conic_parameter(conic, p, tol)?;
457 let gap = conic.point_at(foot, tol).ok()?.distance(p);
458 return (gap > options.gap).then(CurveIntersection::empty);
459 }
460 let middle = origin + along * (-qb / (2.0 * qa));
461 let foot = conic_parameter(conic, middle, tol)?;
462 deep = conic.point_at(foot, tol).ok()?.distance(middle) > options.gap;
463 let root = disc.sqrt();
464 ts.push((-qb - root) / (2.0 * qa));
465 ts.push((-qb + root) / (2.0 * qa));
466 } else if lean.abs() >= 0.1 {
467 ts.push(-height / lean);
468 } else {
469 return None;
470 }
471 let (lo, hi) = line.domain();
472 let (c_lo, c_hi) = conic.domain();
473 let tau = core::f64::consts::TAU;
474 let mut crossings: Vec<Crossing<Point>> = Vec::new();
475 for t in ts {
476 if t < lo - tol.parametric() || t > hi + tol.parametric() {
477 continue;
478 }
479 let point = origin + along * t;
480 let s = conic_parameter(conic, point, tol)?;
481 let s = c_lo + (s - c_lo).rem_euclid(tau);
482 if s > c_hi + tol.parametric() {
483 continue;
484 }
485 let on_conic = conic.point_at(s, tol).ok()?;
486 let gap = on_conic.distance(point);
487 if gap > options.gap {
488 continue;
489 }
490 let tangent = conic.d1_at(s, tol).ok()?;
491 if !deep && tangent.cross(along).magnitude() <= 1e-3 * tangent.magnitude() {
492 return None;
493 }
494 crossings.push(Crossing {
495 on_a: s,
496 on_b: t,
497 point: on_conic,
498 gap,
499 reach: 0.0,
500 });
501 }
502 crossings.sort_by(|x, y| x.on_a.total_cmp(&y.on_a));
503 crossings.dedup_by(|x, y| (x.on_a - y.on_a).abs() <= tol.parametric());
504 Some(CurveIntersection {
505 crossings,
506 overlaps: Vec::new(),
507 })
508}
509
510fn conic_parameter(conic: &Curve, point: Point, tol: Tolerances) -> Option<f64> {
512 match conic {
513 Curve::Circle(c) => ogeom_math::elementary::circle_parameter(&c.circle(), point, tol).ok(),
514 Curve::Ellipse(e) => {
515 ogeom_math::elementary::ellipse_parameter(&e.ellipse(), point, tol).ok()
516 }
517 _ => None,
518 }
519}
520
521fn tangent_line_conic_3d(
562 a: &Curve,
563 b: &Curve,
564 options: CurveCurveOptions,
565 tol: Tolerances,
566) -> Option<CurveIntersection<Point>> {
567 let (basis_a, window_a) = through_trim(a);
568 let (basis_b, window_b) = through_trim(b);
569 let (line, conic, line_first) = match (basis_a, basis_b) {
570 (Curve::Line(l), c) => (l, c, true),
571 (c, Curve::Line(l)) => (l, c, false),
572 _ => return None,
573 };
574 let (line_side, conic_side) = if line_first { (a, b) } else { (b, a) };
575 let (centre, [x_axis, y_axis, normal], [ra, rb]) = conic_axes(conic)?;
576 let reach_gap = options.gap.max(tol.confusion());
577 if reach_gap >= ra.min(rb) {
578 return None;
579 }
580 let (origin, along) = (line.axis().location, line.axis().direction.vector());
581 let offset = origin - centre;
582 if along.dot(normal).abs() > tol.angular() || offset.dot(normal).abs() > reach_gap {
583 return None;
584 }
585 let scaled = ScaledConic {
586 centre,
587 axes: [x_axis, y_axis],
588 radii: [ra, rb],
589 origin,
590 along,
591 };
592 let [dx, dy] = scaled.slope();
593 let qa = dx.mul_add(dx, dy * dy);
594 let t = -(offset.dot(x_axis) / ra).mul_add(dx, offset.dot(y_axis) / rb * dy) / qa;
595 let vertex = scaled.at(t);
596 let slope_error = scaled.slope_error();
597 let qa_low =
598 qa - slope_error * (2.0 * (dx.abs() + dy.abs()) + slope_error) - 3.0 * f64::EPSILON * qa;
599 if !(qa_low > 0.0 && vertex.f.is_finite() && vertex.g.is_finite()) {
600 return None;
601 }
602 let slope = vertex.g.abs() + vertex.g_error;
603 let below = slope * slope / (4.0 * qa_low);
604 if vertex.f + vertex.f_error < 0.0 || vertex.f - vertex.f_error - below > 0.0 {
605 return None;
606 }
607 let spread =
608 slope / (2.0 * qa_low) + ((vertex.f_error + below - vertex.f).max(0.0) / qa_low).sqrt();
609
610 let p = origin + along * t;
611 let (c_lo, c_hi) = conic.domain();
612 let s = conic_parameter(conic, p, tol)?;
613 let s = c_lo + (s - c_lo).rem_euclid(core::f64::consts::TAU);
614 let on_conic = conic.point_at(s, tol).ok()?;
615 let gap = on_conic.distance(p);
616 let (lo, hi) = line.domain();
617 let on_basis =
618 t >= lo - tol.parametric() && t <= hi + tol.parametric() && s <= c_hi + tol.parametric();
619 let touch = if line_first {
620 Crossing {
621 on_a: t,
622 on_b: s,
623 point: p,
624 gap,
625 reach: 0.0,
626 }
627 } else {
628 Crossing {
629 on_a: s,
630 on_b: t,
631 point: on_conic,
632 gap,
633 reach: 0.0,
634 }
635 };
636 let kept = on_basis
637 .then(|| {
638 clipped_to_windows(
639 CurveIntersection {
640 crossings: vec![touch],
641 overlaps: Vec::new(),
642 },
643 Side::of(basis_a, window_a),
644 Side::of(basis_b, window_b),
645 tol,
646 )
647 .crossings
648 .first()
649 .copied()
650 })
651 .flatten();
652 let Some(mut touch) = kept else {
653 let line_apart = {
654 let (lo, hi) = line_side.domain();
655 if t + spread < lo || t - spread > hi {
656 let end = scaled.at(if t < lo { lo } else { hi });
657 let radius = (1.0 + end.f - end.f_error).max(0.0).sqrt();
658 ra.min(rb) * (radius - 1.0)
659 } else {
660 0.0
661 }
662 };
663 let conic_apart = {
664 let shadow = along - normal * along.dot(normal);
665 let across = normal.cross(shadow) / shadow.magnitude();
666 let (sa, sb, sc) = (
667 ra * across.dot(x_axis),
668 rb * across.dot(y_axis),
669 across.dot(centre - origin),
670 );
671 let at = |u: f64| sa.mul_add(u.cos(), sb.mul_add(u.sin(), sc));
672 let (lo, hi) = conic_side.domain();
673 let pi = core::f64::consts::PI;
676 let mut values = vec![at(lo), at(hi)];
677 let mut u = lo + (sb.atan2(sa) - lo).rem_euclid(pi);
678 while u < hi {
679 values.push(at(u));
680 u += pi;
681 }
682 let least = values.iter().copied().fold(f64::INFINITY, f64::min);
683 let most = values.iter().copied().fold(f64::NEG_INFINITY, f64::max);
684 let rounding = 16.0 * f64::EPSILON * (sa.abs() + sb.abs() + sc.abs());
685 if least > 0.0 {
686 least - rounding
687 } else if most < 0.0 {
688 -most - rounding
689 } else {
690 0.0
691 }
692 };
693 return (line_apart.max(conic_apart) > options.gap).then(CurveIntersection::empty);
694 };
695 if gap > options.gap {
696 return None;
697 }
698
699 let distance = |q: Point| -> f64 {
700 if line_first {
701 distance_to_conic_3d(b, conic, touch.on_b, q, tol)
702 } else {
703 let (lo, hi) = b.domain();
704 let foot = (q - origin).dot(along).clamp(lo, hi);
705 q.distance(origin + along * foot)
706 }
707 };
708 let ends = valley_ends_3d(a, touch.on_a, reach_gap, tol, &distance);
709 let valley = valley_span_3d(a, &touch, &ends, tol);
710 let extent =
711 sampled_length_3d(a, options.samples, tol).min(sampled_length_3d(b, options.samples, tol));
712 let cap = 4.0 * (reach_gap * extent).sqrt();
713 if valley > cap || spread > valley {
714 return None;
715 }
716 let owned = valley > reach_gap * 8.0;
717 if !owned && 2.0 * valley > one_crossing_radius(tol) {
718 return None;
719 }
720 let (a_lo, a_hi) = a.domain();
723 let n = options.samples;
724 let (from, to) = order(
725 ends[0].1.unwrap_or(ends[0].0),
726 ends[1].1.unwrap_or(ends[1].0),
727 );
728 #[allow(clippy::cast_precision_loss)]
729 let held = (0..=n)
730 .map(|i| a_lo + (a_hi - a_lo) * i as f64 / n as f64)
731 .filter(|&u| u >= from && u <= to)
732 .count();
733 if held >= 2 {
734 return None;
735 }
736 if owned {
737 touch.reach = valley;
738 }
739 Some(CurveIntersection {
740 crossings: vec![touch],
741 overlaps: Vec::new(),
742 })
743}
744
745struct ScaledConic {
748 centre: Point,
749 axes: [ogeom_math::Vector; 2],
750 radii: [f64; 2],
751 origin: Point,
752 along: ogeom_math::Vector,
753}
754
755struct ScaledValue {
758 f: f64,
759 f_error: f64,
760 g: f64,
761 g_error: f64,
762}
763
764impl ScaledConic {
765 fn slope(&self) -> [f64; 2] {
767 [
768 self.along.dot(self.axes[0]) / self.radii[0],
769 self.along.dot(self.axes[1]) / self.radii[1],
770 ]
771 }
772
773 fn slope_error(&self) -> f64 {
777 8.0 * f64::EPSILON * self.along.magnitude() / self.radii[0].min(self.radii[1])
778 }
779
780 fn at(&self, t: f64) -> ScaledValue {
788 let gamma = |n: f64| n * f64::EPSILON;
789 let magnitude = |q: Point| (q - Point::ORIGIN).magnitude();
790 let smaller = self.radii[0].min(self.radii[1]);
791 let w = (self.origin + self.along * t) - self.centre;
792 let (x, y) = (
793 w.dot(self.axes[0]) / self.radii[0],
794 w.dot(self.axes[1]) / self.radii[1],
795 );
796 let [dx, dy] = self.slope();
797 let e = gamma(3.0)
798 * (magnitude(self.origin) + self.along.magnitude() * t.abs() + magnitude(self.centre))
799 / smaller
800 + gamma(8.0) * w.magnitude() / smaller;
801 let e_d = self.slope_error();
802 let (sum_xy, sum_d) = (x.abs() + y.abs(), dx.abs() + dy.abs());
803 ScaledValue {
804 f: x.mul_add(x, y * y) - 1.0,
805 f_error: 2.0 * e * (sum_xy + e) + gamma(3.0) * x.mul_add(x, y.mul_add(y, 1.0)),
806 g: 2.0 * x.mul_add(dx, y * dy),
807 g_error: 2.0 * (e * sum_d + e_d * sum_xy + 2.0 * e * e_d)
808 + gamma(8.0) * ((x * dx).abs() + (y * dy).abs()),
809 }
810 }
811}
812
813fn distance_to_conic_3d(curve: &Curve, basis: &Curve, seed: f64, q: Point, tol: Tolerances) -> f64 {
817 let (lo, hi) = curve.domain();
818 let at = |u: f64| {
819 curve
820 .point_at(u, tol)
821 .map_or(f64::INFINITY, |c| c.distance(q))
822 };
823 let ends = at(lo).min(at(hi));
824 match basis {
825 Curve::Circle(c) => {
826 let Ok(u) = ogeom_math::elementary::circle_parameter(&c.circle(), q, tol) else {
827 return ends;
828 };
829 let tau = core::f64::consts::TAU;
830 let u = lo + (u - lo).rem_euclid(tau);
831 if u <= hi { at(u) } else { ends }
832 }
833 _ => foot_on_3d(curve, q, seed, tol).map_or(ends, |(_, d)| d.min(ends)),
834 }
835}
836
837fn sampled_length_3d(curve: &Curve, n: usize, tol: Tolerances) -> f64 {
840 let (lo, hi) = curve.domain();
841 #[allow(clippy::cast_precision_loss)]
842 let segments = n as f64;
843 match through_trim(curve).0 {
844 Curve::Line(l) => l.axis().direction.vector().magnitude() * (hi - lo),
845 Curve::Circle(c) => {
846 segments * 2.0 * c.circle().radius() * ((hi - lo) / (2.0 * segments)).sin()
847 }
848 _ => sample_3d(curve, n, tol)
849 .points
850 .windows(2)
851 .map(|w| w[0].distance(w[1]))
852 .sum(),
853 }
854}
855
856fn clipped_to_windows<P>(
865 found: CurveIntersection<P>,
866 a: Side,
867 b: Side,
868 tol: Tolerances,
869) -> CurveIntersection<P> {
870 let (window_a, window_b) = (a.window, b.window);
871 if window_a.is_none() && window_b.is_none() {
872 return found;
873 }
874 let (pa, pb) = (a.period, b.period);
875 let slack = tol.parametric();
876 let placed = |t: f64, window: Option<(f64, f64)>, period: Option<f64>| -> Option<f64> {
877 let Some((lo, hi)) = window else {
878 return Some(t);
879 };
880 for k in [0.0, 1.0, -1.0, 2.0, -2.0] {
881 let shifted = period.map_or(t, |p| p.mul_add(k, t));
882 if shifted >= lo - slack && shifted <= hi + slack {
883 return Some(shifted);
884 }
885 if period.is_none() {
886 break;
887 }
888 }
889 None
890 };
891
892 let mut crossings = Vec::with_capacity(found.crossings.len());
893 for crossing in found.crossings {
894 let (Some(on_a), Some(on_b)) = (
895 placed(crossing.on_a, window_a, pa),
896 placed(crossing.on_b, window_b, pb),
897 ) else {
898 continue;
899 };
900 crossings.push(Crossing {
901 on_a,
902 on_b,
903 ..crossing
904 });
905 }
906
907 let mut overlaps: Vec<Overlap> = Vec::with_capacity(found.overlaps.len());
908 let ordered = |r: (f64, f64)| if r.0 <= r.1 { r } else { (r.1, r.0) };
909 let meet = |x: (f64, f64), y: (f64, f64)| -> Option<(f64, f64)> {
910 let both = (x.0.max(y.0), x.1.min(y.1));
911 (both.1 - both.0 > slack).then_some(both)
912 };
913 let (domain_a, domain_b) = (a.domain, b.domain);
914 let shifts = |period: Option<f64>| -> Vec<f64> {
915 period.map_or_else(
916 || vec![0.0],
917 |p| [0.0, 1.0, -1.0, 2.0, -2.0].iter().map(|k| k * p).collect(),
918 )
919 };
920 for overlap in found.overlaps {
921 let span_a = overlap.on_a.1 - overlap.on_a.0;
922 let span_b = overlap.on_b.1 - overlap.on_b.0;
923 if span_a.abs() <= f64::MIN_POSITIVE || span_b.abs() <= f64::MIN_POSITIVE {
924 continue;
925 }
926 let rate = span_b / span_a;
927 let to_b = |t: f64| overlap.on_b.0 + rate * (t - overlap.on_a.0);
928 let to_a = |t: f64| overlap.on_a.0 + (t - overlap.on_b.0) / rate;
929 let whole = pa.is_some_and(|p| span_a.abs() >= p - slack)
934 && pb.is_some_and(|p| span_b.abs() >= p - slack);
935 let window_a = window_a.map_or(domain_a, ordered);
936 let window_b = window_b.map_or(domain_b, ordered);
937 let mut pieces_a: Vec<(f64, f64, f64)> = Vec::new();
938 if whole {
939 pieces_a.push((window_a.0, window_a.1, 0.0));
940 } else {
941 for shift in shifts(pa) {
942 let stretch = ordered(overlap.on_a);
943 if let Some(piece) = meet((stretch.0 + shift, stretch.1 + shift), window_a) {
944 pieces_a.push((piece.0, piece.1, shift));
945 }
946 }
947 }
948 for (lo, hi, shift_a) in pieces_a {
949 let image = ordered((to_b(lo - shift_a), to_b(hi - shift_a)));
951 for shift_b in shifts(pb) {
952 let Some(on_b) = meet((image.0 + shift_b, image.1 + shift_b), window_b) else {
953 continue;
954 };
955 let back = |t: f64| to_a(t - shift_b) + shift_a;
956 let on_a = ordered((back(on_b.0), back(on_b.1)));
957 if on_a.1 - on_a.0 <= slack {
958 continue;
959 }
960 let forward = |t: f64| to_b(t - shift_a) + shift_b;
962 let (on_a, on_b) = if span_a >= 0.0 {
963 (on_a, (forward(on_a.0), forward(on_a.1)))
964 } else {
965 ((on_a.1, on_a.0), (forward(on_a.1), forward(on_a.0)))
966 };
967 let repeated = overlaps.iter().any(|o| {
968 (o.on_a.0 - on_a.0).abs() <= slack && (o.on_a.1 - on_a.1).abs() <= slack
969 });
970 if !repeated {
971 overlaps.push(Overlap { on_a, on_b });
972 }
973 }
974 }
975 }
976 CurveIntersection {
977 crossings,
978 overlaps,
979 }
980}
981
982fn coplanar_circles_3d(
990 a: &ogeom_geom::CircleCurve,
991 b: &ogeom_geom::CircleCurve,
992 options: CurveCurveOptions,
993 tol: Tolerances,
994) -> Option<CurveIntersection<Point>> {
995 let (ca, cb) = (a.circle(), b.circle());
996 let normal = ca.frame().z().vector();
997 if normal.cross(cb.frame().z().vector()).magnitude() > tol.angular() {
998 return None;
999 }
1000 let between = cb.centre() - ca.centre();
1001 if between.dot(normal).abs() > tol.confusion() {
1002 return None;
1003 }
1004 let in_plane = between - normal * between.dot(normal);
1005 let distance = in_plane.magnitude();
1006 let (ra, rb) = (ca.radius(), cb.radius());
1007 if distance <= tol.confusion() {
1008 return None;
1009 }
1010 if distance > ra + rb + options.gap || distance < (ra - rb).abs() - options.gap {
1011 return Some(CurveIntersection::empty());
1012 }
1013 let weld = tol.confusion() * 50.0;
1017 if (distance - (ra + rb)).abs() <= weld || (distance - (ra - rb).abs()).abs() <= weld {
1018 return None;
1019 }
1020 let along = distance.mul_add(distance, ra.mul_add(ra, -(rb * rb))) / (2.0 * distance);
1021 let squared = ra.mul_add(ra, -(along * along));
1022 if squared <= tol.confusion() * tol.confusion() {
1023 return None;
1024 }
1025 let half = squared.sqrt();
1026 let ux = in_plane / distance;
1027 let uy = normal.cross(ux);
1028 let parameter = |curve: &ogeom_geom::CircleCurve, p: Point| -> f64 {
1029 let local = curve.circle().frame().to_local(p);
1030 let angle = local.y.atan2(local.x);
1031 let angle = if curve.is_reversed() { -angle } else { angle };
1032 let (lo, _) = Curve3d::domain(curve);
1033 lo + (angle - lo).rem_euclid(core::f64::consts::TAU)
1034 };
1035 let mut crossings: Vec<Crossing<Point>> = [half, -half]
1036 .into_iter()
1037 .map(|h| {
1038 let point = ca.centre() + ux * along + uy * h;
1039 Crossing {
1040 on_a: parameter(a, point),
1041 on_b: parameter(b, point),
1042 point,
1043 gap: 0.0,
1044 reach: 0.0,
1045 }
1046 })
1047 .collect();
1048 sort_crossings(&mut crossings);
1049 Some(CurveIntersection {
1050 crossings,
1051 overlaps: Vec::new(),
1052 })
1053}
1054
1055fn same_circle_3d(
1061 a: &ogeom_geom::CircleCurve,
1062 b: &ogeom_geom::CircleCurve,
1063 tol: Tolerances,
1064) -> Option<CurveIntersection<Point>> {
1065 let (ca, cb) = (a.circle(), b.circle());
1066 if ca.centre().distance(cb.centre()) > tol.confusion() {
1067 return None;
1068 }
1069 if (ca.radius() - cb.radius()).abs() > tol.confusion() {
1070 return None;
1071 }
1072 let (za, zb) = (ca.frame().z().vector(), cb.frame().z().vector());
1075 if za.cross(zb).magnitude() > tol.angular() {
1076 return None;
1077 }
1078 let (lo, hi) = Curve3d::domain(a);
1085 let start = a.point_at(lo, tol).ok()?;
1086 let local = cb.frame().to_local(start);
1087 let angle = local.y.atan2(local.x);
1088 let phase = if b.is_reversed() { -angle } else { angle }.rem_euclid(core::f64::consts::TAU);
1089 let along_a = a.d1_at(lo, tol).ok()?;
1090 let along_b = b.d1_at(phase, tol).ok()?;
1091 let winding: f64 = if along_a.dot(along_b) >= 0.0 {
1092 1.0
1093 } else {
1094 -1.0
1095 };
1096 Some(CurveIntersection {
1097 crossings: Vec::new(),
1098 overlaps: vec![Overlap {
1099 on_a: (lo, hi),
1100 on_b: (phase, winding.mul_add(hi - lo, phase)),
1101 }],
1102 })
1103}
1104
1105fn same_curve_3d(a: &Curve, b: &Curve) -> Option<CurveIntersection<Point>> {
1112 let (lo, hi) = Curve3d::domain(a);
1113 if a == b {
1114 return Some(CurveIntersection {
1115 crossings: Vec::new(),
1116 overlaps: vec![Overlap {
1117 on_a: (lo, hi),
1118 on_b: (lo, hi),
1119 }],
1120 });
1121 }
1122 use ogeom_geom::Reversible as _;
1123 if *a == b.clone().reversed() {
1124 let (blo, bhi) = Curve3d::domain(b);
1125 return Some(CurveIntersection {
1126 crossings: Vec::new(),
1127 overlaps: vec![Overlap {
1128 on_a: (lo, hi),
1129 on_b: (bhi, blo),
1130 }],
1131 });
1132 }
1133 None
1134}
1135
1136fn same_ellipse_3d(
1143 a: &ogeom_geom::EllipseCurve,
1144 b: &ogeom_geom::EllipseCurve,
1145 tol: Tolerances,
1146) -> Option<CurveIntersection<Point>> {
1147 let (ea, eb) = (a.ellipse(), b.ellipse());
1148 if ea.frame().origin().distance(eb.frame().origin()) > tol.confusion() {
1149 return None;
1150 }
1151 if (ea.major_radius() - eb.major_radius()).abs() > tol.confusion()
1152 || (ea.minor_radius() - eb.minor_radius()).abs() > tol.confusion()
1153 {
1154 return None;
1155 }
1156 let (za, zb) = (ea.frame().z().vector(), eb.frame().z().vector());
1157 if za.cross(zb).magnitude() > tol.angular() {
1158 return None;
1159 }
1160 let (xa, xb) = (ea.frame().x().vector(), eb.frame().x().vector());
1161 if xa.cross(xb).magnitude() > tol.angular() {
1162 return None;
1163 }
1164 let (lo, hi) = Curve3d::domain(a);
1168 let start = a.point_at(lo, tol).ok()?;
1169 let angle = ogeom_math::elementary::ellipse_parameter(&eb, start, tol).ok()?;
1170 let phase = if b.is_reversed() { -angle } else { angle }.rem_euclid(core::f64::consts::TAU);
1171 let along_a = a.d1_at(lo, tol).ok()?;
1172 let along_b = b.d1_at(phase, tol).ok()?;
1173 let winding: f64 = if along_a.dot(along_b) >= 0.0 {
1174 1.0
1175 } else {
1176 -1.0
1177 };
1178 Some(CurveIntersection {
1179 crossings: Vec::new(),
1180 overlaps: vec![Overlap {
1181 on_a: (lo, hi),
1182 on_b: (phase, winding.mul_add(hi - lo, phase)),
1183 }],
1184 })
1185}
1186
1187fn check(options: CurveCurveOptions) -> OgeomResult<()> {
1188 if options.samples < 2 {
1189 ogeom_bail!(Construction, "seeding needs at least two segments");
1190 }
1191 if !options.gap.is_finite() || options.gap <= 0.0 {
1192 ogeom_bail!(Construction, "a gap of {} is not a distance", options.gap);
1193 }
1194 Ok(())
1195}
1196
1197fn line_line_2d(
1200 a: &ogeom_geom::Line2d,
1201 b: &ogeom_geom::Line2d,
1202 tol: Tolerances,
1203) -> CurveIntersection<Point2> {
1204 let (oa, da) = (a.axis().location, a.axis().direction.vector());
1205 let (ob, db) = (b.axis().location, b.axis().direction.vector());
1206 let cross = da.cross(db);
1207
1208 if cross.abs() <= tol.angular() {
1209 let between = ob - oa;
1211 if between.cross(da).abs() > tol.confusion() {
1212 return CurveIntersection::empty();
1213 }
1214 let (a_lo, a_hi) = a.domain();
1216 let (b_lo, b_hi) = b.domain();
1217 let project = |p: Point2| (p - oa).dot(da);
1219 let (s0, s1) = (project(ob + db * b_lo), project(ob + db * b_hi));
1220 let (lo, hi) = (s0.min(s1).max(a_lo), s0.max(s1).min(a_hi));
1221 let back = |t: f64| (oa + da * t - ob).dot(db);
1223 if hi - lo <= tol.confusion() {
1224 if lo - hi > tol.confusion() {
1226 return CurveIntersection::empty();
1227 }
1228 let t = f64::midpoint(lo, hi).clamp(a_lo, a_hi);
1229 return CurveIntersection {
1230 crossings: vec![Crossing {
1231 on_a: t,
1232 on_b: back(t).clamp(b_lo, b_hi),
1233 point: oa + da * t,
1234 gap: 0.0,
1235 reach: 0.0,
1236 }],
1237 overlaps: Vec::new(),
1238 };
1239 }
1240 return CurveIntersection {
1241 crossings: Vec::new(),
1242 overlaps: vec![Overlap {
1243 on_a: (lo, hi),
1244 on_b: (back(lo), back(hi)),
1250 }],
1251 };
1252 }
1253
1254 let between = ob - oa;
1255 let t = between.cross(db) / cross;
1256 let s = between.cross(da) / cross;
1257 let (a_lo, a_hi) = a.domain();
1258 let (b_lo, b_hi) = b.domain();
1259 if t < a_lo - tol.parametric()
1260 || t > a_hi + tol.parametric()
1261 || s < b_lo - tol.parametric()
1262 || s > b_hi + tol.parametric()
1263 {
1264 return CurveIntersection::empty();
1265 }
1266 CurveIntersection {
1267 crossings: vec![Crossing {
1268 on_a: t,
1269 on_b: s,
1270 point: oa + da * t,
1271 gap: 0.0,
1272 reach: 0.0,
1273 }],
1274 overlaps: Vec::new(),
1275 }
1276}
1277
1278fn line_circle_2d(
1279 line: &ogeom_geom::Line2d,
1280 circle: &ogeom_geom::Circle2d,
1281 swapped: bool,
1282 tol: Tolerances,
1283) -> CurveIntersection<Point2> {
1284 let (o, d) = (line.axis().location, line.axis().direction.vector());
1285 let c = circle.circle();
1286 let centre = c.centre();
1287 let radius = c.radius();
1288
1289 let along = (centre - o).dot(d);
1291 let foot = o + d * along;
1292 let gap = foot.distance(centre);
1293 if gap > radius + tol.confusion() {
1294 return CurveIntersection::empty();
1295 }
1296 let half = radius.mul_add(radius, -(gap * gap)).max(0.0).sqrt();
1297 let candidates = if half <= tol.confusion() {
1298 vec![along]
1299 } else {
1300 vec![along - half, along + half]
1301 };
1302
1303 let (l_lo, l_hi) = line.domain();
1304 let mut crossings = Vec::new();
1305 for t in candidates {
1306 if t < l_lo - tol.parametric() || t > l_hi + tol.parametric() {
1307 continue;
1308 }
1309 let p = o + d * t;
1310 let Some(s) = circle_parameter(circle, p, tol) else {
1311 continue;
1312 };
1313 let (on_a, on_b) = if swapped { (s, t) } else { (t, s) };
1314 crossings.push(Crossing {
1315 on_a,
1316 on_b,
1317 point: p,
1318 gap: 0.0,
1319 reach: 0.0,
1320 });
1321 }
1322 sort_crossings(&mut crossings);
1323 CurveIntersection {
1324 crossings,
1325 overlaps: Vec::new(),
1326 }
1327}
1328
1329fn circle_circle_2d(
1330 a: &ogeom_geom::Circle2d,
1331 b: &ogeom_geom::Circle2d,
1332 tol: Tolerances,
1333) -> CurveIntersection<Point2> {
1334 let (ca, cb) = (a.circle(), b.circle());
1335 let between = cb.centre() - ca.centre();
1336 let distance = between.magnitude();
1337 let (ra, rb) = (ca.radius(), cb.radius());
1338
1339 if distance <= tol.confusion() {
1340 if (ra - rb).abs() <= tol.confusion() {
1341 use ogeom_geom::Curve2d as _;
1346 let (lo, hi) = a.domain();
1347 let correspondence = (|| {
1348 let start = a.point_at(lo, tol).ok()?;
1349 let phase = circle_parameter(b, start, tol)?;
1350 let along_a = a.d1_at(lo, tol).ok()?;
1351 let along_b = b.d1_at(phase, tol).ok()?;
1352 let winding: f64 = if along_a.dot(along_b) >= 0.0 {
1353 1.0
1354 } else {
1355 -1.0
1356 };
1357 Some((phase, winding.mul_add(hi - lo, phase)))
1358 })();
1359 return CurveIntersection {
1360 crossings: Vec::new(),
1361 overlaps: vec![Overlap {
1362 on_a: (lo, hi),
1363 on_b: correspondence.unwrap_or_else(|| b.domain()),
1364 }],
1365 };
1366 }
1367 return CurveIntersection::empty();
1368 }
1369 if distance > ra + rb + tol.confusion() || distance < (ra - rb).abs() - tol.confusion() {
1370 return CurveIntersection::empty();
1371 }
1372
1373 let along = distance.mul_add(distance, ra.mul_add(ra, -(rb * rb))) / (2.0 * distance);
1375 let squared = ra.mul_add(ra, -(along * along));
1376 let direction = between * (1.0 / distance);
1377 let foot = ca.centre() + direction * along;
1378 let mut crossings = Vec::new();
1379 let mut push = |p: Point2| {
1380 if let (Some(s), Some(t)) = (circle_parameter(a, p, tol), circle_parameter(b, p, tol)) {
1381 crossings.push(Crossing {
1382 on_a: s,
1383 on_b: t,
1384 point: p,
1385 gap: 0.0,
1386 reach: 0.0,
1387 });
1388 }
1389 };
1390 if squared <= tol.confusion() * tol.confusion() {
1391 push(foot);
1392 } else {
1393 let offset = ogeom_math::Vector2::new(-direction.y, direction.x) * squared.max(0.0).sqrt();
1394 push(foot + offset);
1395 push(foot - offset);
1396 }
1397 sort_crossings(&mut crossings);
1398 CurveIntersection {
1399 crossings,
1400 overlaps: Vec::new(),
1401 }
1402}
1403
1404fn circle_parameter(curve: &ogeom_geom::Circle2d, p: Point2, tol: Tolerances) -> Option<f64> {
1406 let c = curve.circle();
1407 let local = p - c.centre();
1408 let x = local.dot(c.frame().x().vector());
1409 let y = local.dot(c.frame().y().vector());
1410 let mut angle = y.atan2(x);
1411 if curve.is_reversed() {
1412 angle = -angle;
1413 }
1414 let angle = angle.rem_euclid(core::f64::consts::TAU);
1415 let (lo, hi) = curve.domain();
1416 if angle >= lo - tol.parametric() && angle <= hi + tol.parametric() {
1418 return Some(angle.clamp(lo, hi));
1419 }
1420 let shifted = angle - core::f64::consts::TAU;
1421 if shifted >= lo - tol.parametric() && shifted <= hi + tol.parametric() {
1422 return Some(shifted.clamp(lo, hi));
1423 }
1424 None
1425}
1426
1427fn line_line_3d(
1430 a: &ogeom_geom::LineCurve,
1431 b: &ogeom_geom::LineCurve,
1432 options: CurveCurveOptions,
1433 tol: Tolerances,
1434) -> CurveIntersection<Point> {
1435 let (oa, da) = (a.axis().location, a.axis().direction.vector());
1436 let (ob, db) = (b.axis().location, b.axis().direction.vector());
1437 let cross = da.cross(db);
1438 let denominator = cross.square_magnitude();
1439
1440 if denominator <= tol.angular() * tol.angular() {
1441 let between = ob - oa;
1443 if between.cross(da).magnitude() > tol.confusion() {
1444 return CurveIntersection::empty();
1445 }
1446 let (a_lo, a_hi) = a.domain();
1447 let (b_lo, b_hi) = b.domain();
1448 let project = |p: Point| (p - oa).dot(da);
1449 let (s0, s1) = (project(ob + db * b_lo), project(ob + db * b_hi));
1450 let (lo, hi) = (s0.min(s1).max(a_lo), s0.max(s1).min(a_hi));
1451 let back = |t: f64| (oa + da * t - ob).dot(db);
1452 if hi - lo <= tol.confusion() {
1453 if lo - hi > tol.confusion() {
1455 return CurveIntersection::empty();
1456 }
1457 let t = f64::midpoint(lo, hi).clamp(a_lo, a_hi);
1458 let s = back(t).clamp(b_lo, b_hi);
1459 let point = oa + da * t;
1460 return CurveIntersection {
1461 crossings: vec![Crossing {
1462 on_a: t,
1463 on_b: s,
1464 point,
1465 gap: point.distance(ob + db * s),
1466 reach: 0.0,
1467 }],
1468 overlaps: Vec::new(),
1469 };
1470 }
1471 return CurveIntersection {
1472 crossings: Vec::new(),
1473 overlaps: vec![Overlap {
1474 on_a: (lo, hi),
1475 on_b: (back(lo), back(hi)),
1481 }],
1482 };
1483 }
1484
1485 let between = ob - oa;
1487 let t = between.cross(db).dot(cross) / denominator;
1488 let s = between.cross(da).dot(cross) / denominator;
1489 let pa = oa + da * t;
1490 let pb = ob + db * s;
1491 let gap = pa.distance(pb);
1492 let (a_lo, a_hi) = a.domain();
1493 let (b_lo, b_hi) = b.domain();
1494 if gap > options.gap
1495 || t < a_lo - tol.parametric()
1496 || t > a_hi + tol.parametric()
1497 || s < b_lo - tol.parametric()
1498 || s > b_hi + tol.parametric()
1499 {
1500 return CurveIntersection::empty();
1501 }
1502 CurveIntersection {
1503 crossings: vec![Crossing {
1504 on_a: t,
1505 on_b: s,
1506 point: pa,
1507 gap,
1508 reach: 0.0,
1509 }],
1510 overlaps: Vec::new(),
1511 }
1512}
1513
1514struct Sampled<P> {
1518 points: Vec<P>,
1519 parameters: Vec<f64>,
1520}
1521
1522fn sample_2d(curve: &PlanarCurve, n: usize, tol: Tolerances) -> Sampled<Point2> {
1523 let (lo, hi) = curve.domain();
1524 let mut points = Vec::with_capacity(n + 1);
1525 let mut parameters = Vec::with_capacity(n + 1);
1526 for i in 0..=n {
1527 #[allow(clippy::cast_precision_loss)]
1528 let t = lo + (hi - lo) * i as f64 / n as f64;
1529 if let Ok(p) = curve.point_at(t, tol) {
1530 points.push(p);
1531 parameters.push(t);
1532 }
1533 }
1534 Sampled { points, parameters }
1535}
1536
1537fn sample_3d(curve: &Curve, n: usize, tol: Tolerances) -> Sampled<Point> {
1538 let (lo, hi) = curve.domain();
1539 let mut points = Vec::with_capacity(n + 1);
1540 let mut parameters = Vec::with_capacity(n + 1);
1541 for i in 0..=n {
1542 #[allow(clippy::cast_precision_loss)]
1543 let t = lo + (hi - lo) * i as f64 / n as f64;
1544 if let Ok(p) = curve.point_at(t, tol) {
1545 points.push(p);
1546 parameters.push(t);
1547 }
1548 }
1549 Sampled { points, parameters }
1550}
1551
1552fn general_2d(
1553 a: &PlanarCurve,
1554 b: &PlanarCurve,
1555 options: CurveCurveOptions,
1556 tol: Tolerances,
1557) -> OgeomResult<CurveIntersection<Point2>> {
1558 let sa = sample_2d(a, options.samples, tol);
1559 let sb = sample_2d(b, options.samples, tol);
1560
1561 let mut crossings: Vec<Crossing<Point2>> = Vec::new();
1562 for i in 1..sa.points.len() {
1563 for j in 1..sb.points.len() {
1564 let Some((ta, tb)) = segments_cross_2d(
1565 (sa.points[i - 1], sa.points[i]),
1566 (sb.points[j - 1], sb.points[j]),
1567 ) else {
1568 continue;
1569 };
1570 let seed_a = sa.parameters[i - 1] + (sa.parameters[i] - sa.parameters[i - 1]) * ta;
1571 let seed_b = sb.parameters[j - 1] + (sb.parameters[j] - sb.parameters[j - 1]) * tb;
1572 if let Some(found) = polish_2d(a, b, seed_a, seed_b, options, tol) {
1573 push_unique_2d(&mut crossings, found, tol);
1574 }
1575 }
1576 }
1577 sort_crossings(&mut crossings);
1578 Ok(CurveIntersection {
1579 crossings,
1580 overlaps: Vec::new(),
1581 })
1582}
1583
1584fn general_3d(
1585 a: &Curve,
1586 b: &Curve,
1587 options: CurveCurveOptions,
1588 tol: Tolerances,
1589) -> OgeomResult<CurveIntersection<Point>> {
1590 let sa = sample_3d(a, options.samples, tol);
1591 let sb = sample_3d(b, options.samples, tol);
1592
1593 let mut reach = options.gap;
1597 for s in [&sa, &sb] {
1598 let longest = s
1599 .points
1600 .windows(2)
1601 .map(|w| w[0].distance(w[1]))
1602 .fold(0.0_f64, f64::max);
1603 reach += longest;
1604 }
1605
1606 let segments_b = Segments::of(&sb);
1614 let feet: Vec<Option<(f64, f64)>> = sa
1615 .points
1616 .iter()
1617 .map(|p| foot_via_samples(b, &sb, &segments_b, *p, tol))
1618 .collect();
1619 let overlaps = if hugging_runs(&feet, options.gap).contains(&true) {
1620 shared_support_3d(a, b, &sa, (&sb, &segments_b), &feet, options, tol)
1621 } else {
1622 Vec::new()
1623 };
1624 let shared = |lo: f64, hi: f64| {
1625 overlaps.iter().any(|o| {
1626 let (from, to) = order(o.on_a.0, o.on_a.1);
1627 lo >= from && hi <= to
1628 })
1629 };
1630
1631 let (na, nb) = (sa.points.len(), sb.points.len());
1638 let boxes = |s: &Sampled<Point>| -> Vec<(Point, Point)> {
1643 s.points
1644 .windows(2)
1645 .map(|w| {
1646 (
1647 Point::new(w[0].x.min(w[1].x), w[0].y.min(w[1].y), w[0].z.min(w[1].z)),
1648 Point::new(w[0].x.max(w[1].x), w[0].y.max(w[1].y), w[0].z.max(w[1].z)),
1649 )
1650 })
1651 .collect()
1652 };
1653 let (boxes_a, boxes_b) = (boxes(&sa), &segments_b.boxes);
1654 let apart = |x: &(Point, Point), y: &(Point, Point)| {
1655 x.0.x - y.1.x > reach
1656 || y.0.x - x.1.x > reach
1657 || x.0.y - y.1.y > reach
1658 || y.0.y - x.1.y > reach
1659 || x.0.z - y.1.z > reach
1660 || y.0.z - x.1.z > reach
1661 };
1662 let mut approach: Vec<(f64, f64, f64)> =
1663 Vec::with_capacity(na.saturating_sub(1) * nb.saturating_sub(1));
1664 for i in 1..na {
1665 for j in 1..nb {
1666 approach.push(if apart(&boxes_a[i - 1], &boxes_b[j - 1]) {
1667 (0.0, 0.0, f64::INFINITY)
1668 } else {
1669 segments_approach_3d(
1670 (sa.points[i - 1], sa.points[i]),
1671 (sb.points[j - 1], sb.points[j]),
1672 )
1673 });
1674 }
1675 }
1676 let cols = nb.saturating_sub(1);
1677 let gap_at = |i: usize, j: usize| approach[(i - 1) * cols + (j - 1)].2;
1678 let basin = |i: usize, j: usize| {
1681 let here = gap_at(i, j);
1682 let along_a = (i.saturating_sub(1).max(1)..=(i + 1).min(na - 1))
1683 .all(|p| p == i || here <= gap_at(p, j));
1684 let along_b = (j.saturating_sub(1).max(1)..=(j + 1).min(nb - 1))
1685 .all(|q| q == j || here <= gap_at(i, q));
1686 along_a || along_b
1687 };
1688 let mut crossings: Vec<Crossing<Point>> = Vec::new();
1689 for i in 1..na {
1690 let (lo, hi) = order(sa.parameters[i - 1], sa.parameters[i]);
1691 if shared(lo, hi) {
1692 continue;
1693 }
1694 for j in 1..nb {
1695 let (ta, tb, gap) = approach[(i - 1) * cols + (j - 1)];
1696 if gap > reach || !basin(i, j) {
1697 continue;
1698 }
1699 let seed_a = sa.parameters[i - 1] + (sa.parameters[i] - sa.parameters[i - 1]) * ta;
1700 let seed_b = sb.parameters[j - 1] + (sb.parameters[j] - sb.parameters[j - 1]) * tb;
1701 if let Some(found) = polish_3d(a, b, seed_a, seed_b, options, tol) {
1702 push_unique_3d(a, &mut crossings, found, tol);
1703 }
1704 }
1705 }
1706 sort_crossings(&mut crossings);
1707
1708 if crossings.len() > 1 {
1717 let mut merged: Vec<Crossing<Point>> = Vec::with_capacity(crossings.len());
1718 let mut run_start: Option<Point> = None;
1719 for c in crossings {
1720 if let Some(last) = merged.last_mut()
1721 && contact_between_3d(a, b, last, &c, options, tol)
1722 {
1723 let start = run_start.get_or_insert(last.point);
1724 let reach = start.distance(c.point).max(last.reach);
1725 if c.gap < last.gap {
1726 *last = c;
1727 }
1728 last.reach = reach;
1729 continue;
1730 }
1731 run_start = None;
1732 merged.push(c);
1733 }
1734 if merged.len() > 1 && a.is_periodic() {
1738 let (lo, hi) = a.domain();
1739 let (first, last) = (merged[0], merged[merged.len() - 1]);
1740 let wrapped = Crossing {
1741 on_a: first.on_a + (hi - lo),
1742 ..first
1743 };
1744 if contact_between_3d(a, b, &last, &wrapped, options, tol) {
1745 let reach = last
1746 .reach
1747 .max(first.reach)
1748 .max(last.point.distance(first.point));
1749 let keep = if first.gap <= last.gap {
1750 0
1751 } else {
1752 merged.len() - 1
1753 };
1754 merged[keep].reach = reach;
1755 if keep == 0 {
1756 merged.pop();
1757 } else {
1758 merged.remove(0);
1759 }
1760 }
1761 }
1762 crossings = merged;
1763 }
1764
1765 if !overlaps.is_empty() {
1775 crossings.retain(|c| {
1776 !overlaps.iter().any(|o| {
1777 let (lo, hi) = order(o.on_a.0, o.on_a.1);
1778 c.on_a >= lo - tol.parametric() && c.on_a <= hi + tol.parametric()
1779 })
1780 });
1781 }
1782 let gap = options.gap.max(tol.confusion());
1794 let extent = {
1795 let along = |s: &Sampled<Point>| {
1796 s.points
1797 .windows(2)
1798 .map(|w| w[0].distance(w[1]))
1799 .sum::<f64>()
1800 };
1801 along(&sa).min(along(&sb))
1802 };
1803 let cap = 4.0 * (gap * extent).sqrt();
1804 for c in &mut crossings {
1805 let ends = valley_ends_3d(a, c.on_a, gap, tol, &|q| {
1806 distance_to_curve_3d(b, &sb, &segments_b, q, tol)
1807 });
1808 let valley = valley_span_3d(a, c, &ends, tol);
1809 if valley > gap * 8.0 && valley <= cap {
1810 c.reach = c.reach.max(valley);
1811 }
1812 }
1813 Ok(CurveIntersection {
1814 crossings,
1815 overlaps,
1816 })
1817}
1818
1819fn contact_between_3d(
1823 a: &Curve,
1824 b: &Curve,
1825 from: &Crossing<Point>,
1826 to: &Crossing<Point>,
1827 options: CurveCurveOptions,
1828 tol: Tolerances,
1829) -> bool {
1830 if (to.on_a - from.on_a).abs() <= tol.parametric() {
1831 return true;
1832 }
1833 (1..=3).all(|k| {
1834 let f = f64::from(k) / 4.0;
1835 let t = from.on_a + (to.on_a - from.on_a) * f;
1836 let seed = from.on_b + (to.on_b - from.on_b) * f;
1837 a.point_at(t, tol)
1838 .ok()
1839 .and_then(|p| foot_on_3d(b, p, seed, tol))
1840 .is_some_and(|(_, gap)| gap <= options.gap)
1841 })
1842}
1843
1844struct Segments {
1849 boxes: Vec<(Point, Point)>,
1850 size: f64,
1852 last: core::cell::Cell<usize>,
1854}
1855
1856impl Segments {
1857 fn of(s: &Sampled<Point>) -> Self {
1858 let boxes = s
1859 .points
1860 .windows(2)
1861 .map(|w| {
1862 (
1863 Point::new(w[0].x.min(w[1].x), w[0].y.min(w[1].y), w[0].z.min(w[1].z)),
1864 Point::new(w[0].x.max(w[1].x), w[0].y.max(w[1].y), w[0].z.max(w[1].z)),
1865 )
1866 })
1867 .collect();
1868 let size = s.points.iter().fold(0.0_f64, |m, p| {
1869 m.max(p.x.abs()).max(p.y.abs()).max(p.z.abs())
1870 });
1871 Self {
1872 boxes,
1873 size,
1874 last: core::cell::Cell::new(0),
1875 }
1876 }
1877
1878 fn beyond(&self, j: usize, p: Point, nearest: f64) -> bool {
1884 const ROUNDING: f64 = 64.0 * f64::EPSILON;
1885 let (lo, hi) = self.boxes[j - 1];
1886 let out = |x: f64, l: f64, h: f64| (l - x).max(x - h).max(0.0);
1887 let off = ogeom_math::Vector::new(
1888 out(p.x, lo.x, hi.x),
1889 out(p.y, lo.y, hi.y),
1890 out(p.z, lo.z, hi.z),
1891 );
1892 let size = self.size.max(p.x.abs()).max(p.y.abs()).max(p.z.abs());
1893 off.magnitude() > ROUNDING.mul_add(size, nearest)
1894 }
1895}
1896
1897fn foot_via_samples(
1900 b: &Curve,
1901 sb: &Sampled<Point>,
1902 segments: &Segments,
1903 p: Point,
1904 tol: Tolerances,
1905) -> Option<(f64, f64)> {
1906 let approach = |j: usize| segments_approach_3d((p, p), (sb.points[j - 1], sb.points[j]));
1907 let hint = segments.last.get();
1910 let mut bound = if hint >= 1 && hint < sb.points.len() {
1911 approach(hint).2
1912 } else {
1913 f64::INFINITY
1914 };
1915 let mut seed = (f64::INFINITY, 0.0);
1916 for j in 1..sb.points.len() {
1917 if segments.beyond(j, p, bound) {
1918 continue;
1919 }
1920 let (_, tb, gap) = approach(j);
1921 if gap < seed.0 {
1922 seed = (
1923 gap,
1924 sb.parameters[j - 1] + (sb.parameters[j] - sb.parameters[j - 1]) * tb,
1925 );
1926 bound = bound.min(gap);
1927 segments.last.set(j);
1928 }
1929 }
1930 if cfg!(debug_assertions) {
1931 let mut every = (f64::INFINITY, 0.0);
1932 for j in 1..sb.points.len() {
1933 let (_, tb, gap) = segments_approach_3d((p, p), (sb.points[j - 1], sb.points[j]));
1934 if gap < every.0 {
1935 every = (
1936 gap,
1937 sb.parameters[j - 1] + (sb.parameters[j] - sb.parameters[j - 1]) * tb,
1938 );
1939 }
1940 }
1941 assert!(
1942 every.0.to_bits() == seed.0.to_bits() && every.1.to_bits() == seed.1.to_bits(),
1943 "boxed scan found {seed:?}, every segment {every:?}"
1944 );
1945 }
1946 if !seed.0.is_finite() {
1947 return None;
1948 }
1949 foot_on_3d(b, p, seed.1, tol)
1950}
1951
1952fn hugging_runs(feet: &[Option<(f64, f64)>], gap: f64) -> Vec<bool> {
1956 let within = |i: usize| feet[i].is_some_and(|(_, g)| g <= gap);
1957 let mut out = vec![false; feet.len()];
1958 let mut i = 0;
1959 while i < feet.len() {
1960 if !within(i) {
1961 i += 1;
1962 continue;
1963 }
1964 let start = i;
1965 while i + 1 < feet.len() && within(i + 1) {
1966 i += 1;
1967 }
1968 if i > start {
1969 out[start..=i].fill(true);
1970 }
1971 i += 1;
1972 }
1973 out
1974}
1975
1976fn shared_support_3d(
1979 a: &Curve,
1980 b: &Curve,
1981 sa: &Sampled<Point>,
1982 (sb, segments_b): (&Sampled<Point>, &Segments),
1983 feet: &[Option<(f64, f64)>],
1984 options: CurveCurveOptions,
1985 tol: Tolerances,
1986) -> Vec<Overlap> {
1987 let hugs = |t: f64| -> Option<(f64, f64)> {
1988 let p = a.point_at(t, tol).ok()?;
1989 let (s, gap) = foot_via_samples(b, sb, segments_b, p, tol)?;
1990 (gap <= options.gap).then_some((s, gap))
1991 };
1992 let within = |i: usize| feet[i].is_some_and(|(_, gap)| gap <= options.gap);
1993
1994 let mut overlaps = Vec::new();
1995 let mut i = 0;
1996 while i < sa.points.len() {
1997 if !within(i) {
1998 i += 1;
1999 continue;
2000 }
2001 let start = i;
2002 while i + 1 < sa.points.len() && within(i + 1) {
2003 i += 1;
2004 }
2005 let end = i;
2006 i += 1;
2007 if end == start {
2008 continue;
2009 }
2010 let refine = |inside: usize, outside: Option<usize>| -> (f64, f64) {
2013 let (mut t_in, s_in) = (sa.parameters[inside], feet[inside].map_or(0.0, |f| f.0));
2014 let Some(out) = outside else {
2015 return (t_in, s_in);
2016 };
2017 let mut s_at = s_in;
2018 let mut t_out = sa.parameters[out];
2019 for _ in 0..48 {
2020 if (t_out - t_in).abs() <= tol.parametric() {
2021 break;
2022 }
2023 let mid = f64::midpoint(t_in, t_out);
2024 match hugs(mid) {
2025 Some((s, _)) => {
2026 t_in = mid;
2027 s_at = s;
2028 }
2029 None => t_out = mid,
2030 }
2031 }
2032 (t_in, s_at)
2033 };
2034 let (lo_a, lo_b) = refine(start, start.checked_sub(1));
2035 let (hi_a, mut hi_b) = refine(end, (end + 1 < sa.points.len()).then_some(end + 1));
2036 if b.is_periodic() && !matches!(b, Curve::Trimmed(_)) {
2042 let (from, to) = b.domain();
2043 let period = to - from;
2044 let mut last = lo_b;
2045 for foot in feet[start..=end].iter().flatten() {
2046 last = foot.0 + period * ((last - foot.0) / period).round();
2047 }
2048 hi_b += period * ((last - hi_b) / period).round();
2049 }
2050 if hi_a - lo_a <= tol.parametric() || (hi_b - lo_b).abs() <= tol.parametric() {
2051 continue;
2052 }
2053 overlaps.push(Overlap {
2054 on_a: (lo_a, hi_a),
2055 on_b: (lo_b, hi_b),
2056 });
2057 }
2058 overlaps
2059}
2060
2061fn foot_on_3d(curve: &Curve, p: Point, seed: f64, tol: Tolerances) -> Option<(f64, f64)> {
2064 let mut s = clamp_3d(curve, seed);
2065 let mut best = (s, curve.point_at(s, tol).ok()?.distance(p));
2066 for _ in 0..30 {
2067 let d = curve.derivatives_at(s, 2, tol).ok()?;
2068 let zero = ogeom_math::Vector::ZERO;
2069 let (c, d1, d2) = (
2070 d.first().copied().unwrap_or(zero),
2071 d.get(1).copied().unwrap_or(zero),
2072 d.get(2).copied().unwrap_or(zero),
2073 );
2074 let gap = c - (p - Point::ORIGIN);
2075 let g = gap.dot(d1);
2076 let dg = d1.dot(d1) + gap.dot(d2);
2077 if dg.abs() <= f64::MIN_POSITIVE {
2078 break;
2079 }
2080 let next = clamp_3d(curve, s - g / dg);
2081 let dist = curve.point_at(next, tol).ok()?.distance(p);
2082 let moved = (next - s).abs();
2083 s = next;
2084 if dist < best.1 {
2085 best = (s, dist);
2086 }
2087 if moved <= tol.parametric() {
2088 break;
2089 }
2090 }
2091 Some(best)
2092}
2093
2094fn polish_2d(
2096 a: &PlanarCurve,
2097 b: &PlanarCurve,
2098 seed_a: f64,
2099 seed_b: f64,
2100 options: CurveCurveOptions,
2101 tol: Tolerances,
2102) -> Option<Crossing<Point2>> {
2103 let system = |x: &[f64; 2]| {
2104 let (t, s) = (clamp_2d(a, x[0]), clamp_2d(b, x[1]));
2105 let pa = a.point_at(t, tol).unwrap_or(Point2::ORIGIN);
2106 let pb = b.point_at(s, tol).unwrap_or(Point2::ORIGIN);
2107 let da = a
2108 .d1_at(t, tol)
2109 .unwrap_or(ogeom_math::Vector2::new(0.0, 0.0));
2110 let db = b
2111 .d1_at(s, tol)
2112 .unwrap_or(ogeom_math::Vector2::new(0.0, 0.0));
2113 ([pa.x - pb.x, pa.y - pb.y], [[da.x, -db.x], [da.y, -db.y]])
2114 };
2115 let criteria = solve::Criteria {
2116 residual: tol.confusion() * 0.01,
2117 step: tol.parametric(),
2118 max_iterations: 40,
2119 };
2120 let found = solve::newton_system_fixed(system, [seed_a, seed_b], criteria).ok()?;
2121 let (t, s) = (clamp_2d(a, found.0[0]), clamp_2d(b, found.0[1]));
2122 let pa = a.point_at(t, tol).ok()?;
2123 let pb = b.point_at(s, tol).ok()?;
2124 let gap = pa.distance(pb);
2125 if gap > options.gap {
2126 return None;
2127 }
2128 Some(Crossing {
2129 on_a: t,
2130 on_b: s,
2131 point: pa,
2132 gap,
2133 reach: 0.0,
2134 })
2135}
2136
2137fn polish_3d(
2152 a: &Curve,
2153 b: &Curve,
2154 seed_a: f64,
2155 seed_b: f64,
2156 options: CurveCurveOptions,
2157 tol: Tolerances,
2158) -> Option<Crossing<Point>> {
2159 let (t, s, converged) = stationary_3d(a, b, seed_a, seed_b, tol)?;
2160 let mut found = crossing_at_3d(a, b, t, s, tol)?;
2161 if found.gap > options.gap && !converged {
2162 let (t, s) = alternating_feet_3d(a, b, t, s, found.gap, tol)?;
2163 let walked = crossing_at_3d(a, b, t, s, tol)?;
2164 let polished = stationary_3d(a, b, t, s, tol)
2165 .and_then(|(t, s, _)| crossing_at_3d(a, b, t, s, tol))
2166 .filter(|p| p.gap < walked.gap);
2167 found = polished.unwrap_or(walked);
2168 }
2169 (found.gap <= options.gap).then_some(found)
2170}
2171
2172fn crossing_at_3d(
2174 a: &Curve,
2175 b: &Curve,
2176 t: f64,
2177 s: f64,
2178 tol: Tolerances,
2179) -> Option<Crossing<Point>> {
2180 let pa = a.point_at(t, tol).ok()?;
2181 let pb = b.point_at(s, tol).ok()?;
2182 Some(Crossing {
2183 on_a: t,
2184 on_b: s,
2185 point: pa,
2186 gap: pa.distance(pb),
2187 reach: 0.0,
2188 })
2189}
2190
2191fn alternating_feet_3d(
2197 a: &Curve,
2198 b: &Curve,
2199 mut t: f64,
2200 mut s: f64,
2201 mut gap: f64,
2202 tol: Tolerances,
2203) -> Option<(f64, f64)> {
2204 for _ in 0..64 {
2205 let before = gap;
2206 let pa = a.point_at(t, tol).ok()?;
2207 if let Some((next, d)) = foot_on_3d(b, pa, s, tol)
2208 && d < gap
2209 {
2210 (s, gap) = (next, d);
2211 }
2212 let pb = b.point_at(s, tol).ok()?;
2213 if let Some((next, d)) = foot_on_3d(a, pb, t, tol)
2214 && d < gap
2215 {
2216 (t, gap) = (next, d);
2217 }
2218 if before - gap <= tol.confusion() * 0.01 {
2219 break;
2220 }
2221 }
2222 Some((t, s))
2223}
2224
2225fn stationary_3d(
2229 a: &Curve,
2230 b: &Curve,
2231 seed_a: f64,
2232 seed_b: f64,
2233 tol: Tolerances,
2234) -> Option<(f64, f64, bool)> {
2235 let system = |x: [f64; 2]| {
2238 let (t, s) = (clamp_3d(a, x[0]), clamp_3d(b, x[1]));
2239 let (Ok(da), Ok(db)) = (a.derivatives_at(t, 2, tol), b.derivatives_at(s, 2, tol)) else {
2240 return ([f64::INFINITY; 2], [[0.0; 2]; 2]);
2243 };
2244 let zero = ogeom_math::Vector::ZERO;
2245 let at = |d: &[ogeom_math::Vector], k: usize| d.get(k).copied().unwrap_or(zero);
2246 let (pa, d1a, d2a) = (at(&da, 0), at(&da, 1), at(&da, 2));
2247 let (pb, d1b, d2b) = (at(&db, 0), at(&db, 1), at(&db, 2));
2248 let gap = pa - pb;
2249 (
2250 [gap.dot(d1a), -gap.dot(d1b)],
2251 [
2252 [d1a.dot(d1a) + gap.dot(d2a), -d1a.dot(d1b)],
2253 [-d1a.dot(d1b), d1b.dot(d1b) - gap.dot(d2b)],
2254 ],
2255 )
2256 };
2257 let criteria = solve::Criteria {
2258 residual: tol.confusion() * 0.01,
2259 step: tol.parametric(),
2260 max_iterations: 40,
2261 };
2262 let ([t, s], _, convergence, _) =
2263 solve::newton_system_2(system, [seed_a, seed_b], criteria).ok()?;
2264 Some((
2265 clamp_3d(a, t),
2266 clamp_3d(b, s),
2267 convergence == solve::Convergence::Residual,
2268 ))
2269}
2270
2271fn clamp_2d(curve: &PlanarCurve, t: f64) -> f64 {
2274 let (lo, hi) = curve.domain();
2275 if curve.is_periodic() {
2276 let span = hi - lo;
2277 if span > 0.0 {
2278 return lo + (t - lo).rem_euclid(span);
2279 }
2280 }
2281 t.clamp(lo, hi)
2282}
2283
2284fn clamp_3d(curve: &Curve, t: f64) -> f64 {
2285 let (lo, hi) = curve.domain();
2286 if curve.is_periodic() {
2287 let span = hi - lo;
2288 if span > 0.0 {
2289 return lo + (t - lo).rem_euclid(span);
2290 }
2291 }
2292 t.clamp(lo, hi)
2293}
2294
2295fn segments_cross_2d(a: (Point2, Point2), b: (Point2, Point2)) -> Option<(f64, f64)> {
2297 let da = a.1 - a.0;
2298 let db = b.1 - b.0;
2299 let cross = da.cross(db);
2300 if cross.abs() <= f64::MIN_POSITIVE {
2301 return None;
2302 }
2303 let between = b.0 - a.0;
2304 let t = between.cross(db) / cross;
2305 let s = between.cross(da) / cross;
2306 if !(0.0..=1.0).contains(&t) || !(0.0..=1.0).contains(&s) {
2307 return None;
2308 }
2309 Some((t, s))
2310}
2311
2312fn distance_to_curve_3d(
2315 b: &Curve,
2316 sb: &Sampled<Point>,
2317 segments: &Segments,
2318 p: Point,
2319 tol: Tolerances,
2320) -> f64 {
2321 let mut best = (0_usize, f64::INFINITY);
2322 for i in 1..sb.points.len() {
2323 if segments.beyond(i, p, best.1) {
2324 continue;
2325 }
2326 let (q0, q1) = (sb.points[i - 1], sb.points[i]);
2327 let d = q1 - q0;
2328 let len2 = d.dot(d);
2329 let f = if len2 <= f64::MIN_POSITIVE {
2330 0.0
2331 } else {
2332 ((p - q0).dot(d) / len2).clamp(0.0, 1.0)
2333 };
2334 let dist = p.distance(q0 + d * f);
2335 if dist < best.1 {
2336 best = (i, dist);
2337 }
2338 }
2339 if best.0 == 0 {
2340 return best.1;
2341 }
2342 let (mut lo, mut hi) = (sb.parameters[best.0 - 1], sb.parameters[best.0]);
2343 let at = |t: f64| -> f64 { b.point_at(t, tol).map_or(f64::INFINITY, |q| q.distance(p)) };
2344 let phi = 0.5 * (3.0 - 5.0_f64.sqrt());
2347 let (mut x1, mut x2) = (lo + phi * (hi - lo), hi - phi * (hi - lo));
2348 let (mut f1, mut f2) = (at(x1), at(x2));
2349 for _ in 0..48 {
2350 if f1 < f2 {
2351 hi = x2;
2352 x2 = x1;
2353 f2 = f1;
2354 x1 = lo + phi * (hi - lo);
2355 f1 = at(x1);
2356 } else {
2357 lo = x1;
2358 x1 = x2;
2359 f1 = f2;
2360 x2 = hi - phi * (hi - lo);
2361 f2 = at(x2);
2362 }
2363 }
2364 f1.min(f2).min(best.1)
2365}
2366
2367fn valley_ends_3d(
2372 a: &Curve,
2373 on_a: f64,
2374 gap: f64,
2375 tol: Tolerances,
2376 distance: &dyn Fn(Point) -> f64,
2377) -> [(f64, Option<f64>); 2] {
2378 let (lo, hi) = a.domain();
2379 let span = hi - lo;
2380 let mut ends = [(on_a, None); 2];
2381 if span <= 0.0 {
2382 return ends;
2383 }
2384 let inside = |t: f64| -> bool {
2385 if t < lo || t > hi {
2386 return false;
2387 }
2388 a.point_at(t, tol).is_ok_and(|q| distance(q) <= gap)
2389 };
2390 for (end, direction) in ends.iter_mut().zip([-1.0, 1.0]) {
2391 let mut step = span * 1e-6;
2392 let mut last_in = on_a;
2393 let mut first_out: Option<f64> = None;
2394 while step <= span {
2395 let t = on_a + direction * step;
2396 if inside(t) {
2397 last_in = t;
2398 step *= 2.0;
2399 } else {
2400 first_out = Some(t);
2401 break;
2402 }
2403 }
2404 *end = match first_out {
2405 Some(mut out) => {
2406 let mut r#in = last_in;
2407 for _ in 0..30 {
2408 let mid = f64::midpoint(r#in, out);
2409 if inside(mid) {
2410 r#in = mid;
2411 } else {
2412 out = mid;
2413 }
2414 }
2415 (r#in, Some(out))
2416 }
2417 None => (last_in, None),
2418 };
2419 }
2420 ends
2421}
2422
2423fn valley_span_3d(
2426 a: &Curve,
2427 crossing: &Crossing<Point>,
2428 ends: &[(f64, Option<f64>); 2],
2429 tol: Tolerances,
2430) -> f64 {
2431 let (lo, hi) = a.domain();
2432 if hi - lo <= 0.0 {
2433 return 0.0;
2434 }
2435 ends.iter()
2436 .filter_map(|(edge, _)| a.point_at(*edge, tol).ok())
2437 .map(|q| q.distance(crossing.point))
2438 .fold(0.0, f64::max)
2439}
2440
2441fn segments_approach_3d(a: (Point, Point), b: (Point, Point)) -> (f64, f64, f64) {
2443 let da = a.1 - a.0;
2444 let db = b.1 - b.0;
2445 let between = a.0 - b.0;
2446 let (aa, bb, ab) = (da.dot(da), db.dot(db), da.dot(db));
2447 let (ad, bd) = (da.dot(between), db.dot(between));
2448 let denominator = ab.mul_add(-ab, aa * bb);
2449
2450 let (mut t, mut s) = if denominator.abs() <= f64::MIN_POSITIVE {
2451 (
2452 0.0,
2453 if bb > 0.0 {
2454 (bd / bb).clamp(0.0, 1.0)
2455 } else {
2456 0.0
2457 },
2458 )
2459 } else {
2460 (
2461 (ab.mul_add(bd, -(bb * ad)) / denominator).clamp(0.0, 1.0),
2462 (aa.mul_add(bd, -(ab * ad)) / denominator).clamp(0.0, 1.0),
2463 )
2464 };
2465 if bb > 0.0 {
2467 s = ((da.dot(between) + t * aa - 0.0).mul_add(0.0, db.dot(between + da * t)) / bb)
2468 .clamp(0.0, 1.0);
2469 }
2470 if aa > 0.0 {
2471 t = (da.dot(db * s - between) / aa).clamp(0.0, 1.0);
2472 }
2473 let pa = a.0 + da * t;
2474 let pb = b.0 + db * s;
2475 (t, s, pa.distance(pb))
2476}
2477
2478fn order(a: f64, b: f64) -> (f64, f64) {
2479 if a <= b { (a, b) } else { (b, a) }
2480}
2481
2482fn sort_crossings<P>(crossings: &mut [Crossing<P>]) {
2483 crossings.sort_by(|x, y| {
2484 x.on_a
2485 .partial_cmp(&y.on_a)
2486 .unwrap_or(core::cmp::Ordering::Equal)
2487 });
2488}
2489
2490fn push_unique_2d(crossings: &mut Vec<Crossing<Point2>>, found: Crossing<Point2>, tol: Tolerances) {
2491 let reach = tol.confusion() * 100.0;
2492 if crossings
2493 .iter()
2494 .any(|c| c.point.distance(found.point) <= reach)
2495 {
2496 return;
2497 }
2498 crossings.push(found);
2499}
2500
2501fn one_crossing_radius(tol: Tolerances) -> f64 {
2504 tol.confusion() * 100.0
2505}
2506
2507fn push_unique_3d(
2514 a: &Curve,
2515 crossings: &mut Vec<Crossing<Point>>,
2516 found: Crossing<Point>,
2517 tol: Tolerances,
2518) {
2519 let reach = one_crossing_radius(tol);
2520 let (lo, hi) = a.domain();
2521 let period = hi - lo;
2522 let closed = a.is_periodic()
2523 || (period.is_finite()
2524 && a.point_at(lo, tol)
2525 .ok()
2526 .zip(a.point_at(hi, tol).ok())
2527 .is_some_and(|(p, q)| p.distance(q) <= reach));
2528 let same = |c: &Crossing<Point>| {
2529 if c.point.distance(found.point) > reach {
2530 return false;
2531 }
2532 let mut mid = f64::midpoint(c.on_a, found.on_a);
2533 if closed && (c.on_a - found.on_a).abs() > period * 0.5 {
2534 mid += period * 0.5;
2535 if mid > hi {
2536 mid -= period;
2537 }
2538 }
2539 a.point_at(mid, tol)
2540 .is_ok_and(|p| p.distance(found.point) <= reach)
2541 };
2542 if crossings.iter().any(same) {
2543 return;
2544 }
2545 crossings.push(found);
2546}
2547
2548#[cfg(test)]
2549#[allow(clippy::unwrap_used)]
2550mod tests {
2551 use super::*;
2552 use ogeom_geom::{BSpline2d, Circle2d, CircleCurve, Line2d, LineCurve};
2553 use ogeom_math::{Circle, Circle2, Direction2, Frame, Frame2, KnotVector, Vector2};
2554
2555 const T: Tolerances = Tolerances::millimetres();
2556
2557 fn line2(from: Point2, to: Point2) -> PlanarCurve {
2558 Line2d::segment(from, to, T).unwrap().into()
2559 }
2560
2561 fn circle2(centre: Point2, radius: f64) -> PlanarCurve {
2562 Circle2d::new(
2563 Circle2::new(
2564 Frame2::new(centre, Direction2::new(Vector2::new(1.0, 0.0), T).unwrap()),
2565 radius,
2566 T,
2567 )
2568 .unwrap(),
2569 )
2570 .into()
2571 }
2572
2573 #[test]
2574 fn two_lines_cross_where_algebra_says() {
2575 let a = line2(Point2::new(0.0, 0.0), Point2::new(4.0, 4.0));
2576 let b = line2(Point2::new(0.0, 4.0), Point2::new(4.0, 0.0));
2577 let found = intersect_curves_2d(&a, &b, CurveCurveOptions::default(), T).unwrap();
2578 assert_eq!(found.crossings.len(), 1);
2579 let hit = &found.crossings[0];
2580 assert!(hit.point.is_equal(Point2::new(2.0, 2.0), T));
2581 approx::assert_relative_eq!(hit.on_a, 8.0_f64.sqrt(), epsilon = 1e-9);
2583
2584 let short = line2(Point2::new(0.0, 4.0), Point2::new(1.0, 3.0));
2586 assert!(
2587 intersect_curves_2d(&a, &short, CurveCurveOptions::default(), T)
2588 .unwrap()
2589 .is_empty()
2590 );
2591 }
2592
2593 #[test]
2594 fn collinear_lines_overlap_rather_than_crossing_everywhere() {
2595 let a = line2(Point2::new(0.0, 0.0), Point2::new(10.0, 0.0));
2596 let b = line2(Point2::new(4.0, 0.0), Point2::new(20.0, 0.0));
2597 let found = intersect_curves_2d(&a, &b, CurveCurveOptions::default(), T).unwrap();
2598 assert!(found.crossings.is_empty());
2599 assert_eq!(found.overlaps.len(), 1);
2600 let overlap = &found.overlaps[0];
2601 approx::assert_relative_eq!(overlap.on_a.0, 4.0, epsilon = 1e-9);
2602 approx::assert_relative_eq!(overlap.on_a.1, 10.0, epsilon = 1e-9);
2603 approx::assert_relative_eq!(overlap.on_b.0, 0.0, epsilon = 1e-9);
2604 approx::assert_relative_eq!(overlap.on_b.1, 6.0, epsilon = 1e-9);
2605
2606 let above = line2(Point2::new(0.0, 1.0), Point2::new(10.0, 1.0));
2608 assert!(
2609 intersect_curves_2d(&a, &above, CurveCurveOptions::default(), T)
2610 .unwrap()
2611 .is_empty()
2612 );
2613 }
2614
2615 #[test]
2616 fn a_line_meets_a_circle_in_two_points_one_or_none() {
2617 let circle = circle2(Point2::new(0.0, 0.0), 2.0);
2618 let through = line2(Point2::new(-5.0, 0.0), Point2::new(5.0, 0.0));
2619 let found =
2620 intersect_curves_2d(&through, &circle, CurveCurveOptions::default(), T).unwrap();
2621 assert_eq!(found.crossings.len(), 2);
2622 for hit in &found.crossings {
2623 approx::assert_relative_eq!(
2624 hit.point.distance(Point2::new(0.0, 0.0)),
2625 2.0,
2626 epsilon = 1e-9
2627 );
2628 let PlanarCurve::Circle(_) = &circle else {
2630 unreachable!()
2631 };
2632 let on_circle = circle.point_at(hit.on_b, T).unwrap();
2633 assert!(on_circle.is_equal(hit.point, T));
2634 }
2635
2636 let tangent = line2(Point2::new(-5.0, 2.0), Point2::new(5.0, 2.0));
2637 assert_eq!(
2638 intersect_curves_2d(&tangent, &circle, CurveCurveOptions::default(), T)
2639 .unwrap()
2640 .crossings
2641 .len(),
2642 1
2643 );
2644 let missing = line2(Point2::new(-5.0, 3.0), Point2::new(5.0, 3.0));
2645 assert!(
2646 intersect_curves_2d(&missing, &circle, CurveCurveOptions::default(), T)
2647 .unwrap()
2648 .is_empty()
2649 );
2650 }
2651
2652 #[test]
2653 fn two_circles_cross_touch_coincide_or_miss() {
2654 let a = circle2(Point2::new(0.0, 0.0), 2.0);
2655
2656 let crossing = circle2(Point2::new(3.0, 0.0), 2.0);
2657 let found = intersect_curves_2d(&a, &crossing, CurveCurveOptions::default(), T).unwrap();
2658 assert_eq!(found.crossings.len(), 2);
2659 for hit in &found.crossings {
2660 let on_a = a.point_at(hit.on_a, T).unwrap();
2661 let on_b = crossing.point_at(hit.on_b, T).unwrap();
2662 assert!(on_a.is_equal(hit.point, T));
2663 assert!(on_b.is_equal(hit.point, T));
2664 }
2665
2666 let touching = circle2(Point2::new(4.0, 0.0), 2.0);
2667 assert_eq!(
2668 intersect_curves_2d(&a, &touching, CurveCurveOptions::default(), T)
2669 .unwrap()
2670 .crossings
2671 .len(),
2672 1
2673 );
2674
2675 let same = circle2(Point2::new(0.0, 0.0), 2.0);
2676 let coincident = intersect_curves_2d(&a, &same, CurveCurveOptions::default(), T).unwrap();
2677 assert!(coincident.crossings.is_empty());
2678 assert_eq!(coincident.overlaps.len(), 1);
2679
2680 let apart = circle2(Point2::new(10.0, 0.0), 2.0);
2681 assert!(
2682 intersect_curves_2d(&a, &apart, CurveCurveOptions::default(), T)
2683 .unwrap()
2684 .is_empty()
2685 );
2686 }
2687
2688 #[test]
2689 fn the_general_path_handles_what_has_no_closed_form() {
2690 let wave: PlanarCurve = BSpline2d::new(
2693 KnotVector::new(vec![0.0, 0.0, 0.0, 0.0, 0.5, 1.0, 1.0, 1.0, 1.0], 3).unwrap(),
2694 vec![
2695 Point2::new(0.0, -1.0),
2696 Point2::new(1.0, 3.0),
2697 Point2::new(2.0, -3.0),
2698 Point2::new(3.0, 3.0),
2699 Point2::new(4.0, -1.0),
2700 ],
2701 T,
2702 )
2703 .unwrap()
2704 .into();
2705 let axis = line2(Point2::new(-1.0, 0.0), Point2::new(5.0, 0.0));
2706 let found = intersect_curves_2d(&wave, &axis, CurveCurveOptions::default(), T).unwrap();
2707 assert_eq!(found.crossings.len(), 3, "a wave crosses its axis thrice");
2708 for hit in &found.crossings {
2709 assert!(hit.gap < 1e-9);
2710 assert!(hit.point.y.abs() < 1e-9);
2711 let on_wave = wave.point_at(hit.on_a, T).unwrap();
2712 assert!(on_wave.is_equal(hit.point, T));
2713 }
2714 }
2715
2716 #[test]
2719 fn one_planar_circle_written_twice_states_the_correspondence() {
2720 let a = circle2(Point2::new(1.0, 2.0), 3.0);
2721 let quarter = Frame2::new(
2722 Point2::new(1.0, 2.0),
2723 Direction2::new(Vector2::new(0.0, 1.0), T).unwrap(),
2724 );
2725 let b: PlanarCurve = Circle2d::new(Circle2::new(quarter, 3.0, T).unwrap()).into();
2726 let flipped: PlanarCurve = ogeom_geom::Reversible::reversed(&b);
2727 for other in [b, flipped] {
2728 let found = intersect_curves_2d(&a, &other, CurveCurveOptions::default(), T).unwrap();
2729 assert_eq!(found.overlaps.len(), 1);
2730 let overlap = found.overlaps[0];
2731 for i in 0..=8 {
2732 let t = f64::from(i) / 8.0;
2733 let ta = (overlap.on_a.1 - overlap.on_a.0).mul_add(t, overlap.on_a.0);
2734 let tb = (overlap.on_b.1 - overlap.on_b.0).mul_add(t, overlap.on_b.0);
2735 let pa = a.point_at(ta, T).unwrap();
2736 let pb = other
2737 .point_at(tb.rem_euclid(core::f64::consts::TAU), T)
2738 .unwrap();
2739 assert!(pa.distance(pb) < 1e-9, "at {t}: {pa:?} against {pb:?}");
2740 }
2741 }
2742 }
2743
2744 #[test]
2747 fn collinear_segments_meeting_end_to_end_share_a_point() {
2748 let a = line2(Point2::new(0.0, 0.0), Point2::new(5.0, 0.0));
2749 let b = line2(Point2::new(5.0, 0.0), Point2::new(9.0, 0.0));
2750 let found = intersect_curves_2d(&a, &b, CurveCurveOptions::default(), T).unwrap();
2751 assert!(found.overlaps.is_empty());
2752 assert_eq!(found.crossings.len(), 1);
2753 assert!(found.crossings[0].point.distance(Point2::new(5.0, 0.0)) < 1e-12);
2754
2755 let a: Curve = LineCurve::segment(Point::ORIGIN, Point::new(0.0, 0.0, 5.0), T)
2756 .unwrap()
2757 .into();
2758 let b: Curve = LineCurve::segment(Point::new(0.0, 0.0, 5.0), Point::new(0.0, 0.0, 7.0), T)
2759 .unwrap()
2760 .into();
2761 let found = intersect_curves(&a, &b, CurveCurveOptions::default(), T).unwrap();
2762 assert!(found.overlaps.is_empty());
2763 assert_eq!(found.crossings.len(), 1);
2764 assert!(found.crossings[0].point.distance(Point::new(0.0, 0.0, 5.0)) < 1e-12);
2765 }
2766
2767 #[test]
2774 fn one_circle_written_twice_states_the_correspondence_between_them() {
2775 use ogeom_math::{Direction, Vector};
2776 let a: Curve = CircleCurve::new(Circle::new(Frame::WORLD, 3.0, T).unwrap()).into();
2777 let third = 2.0 * core::f64::consts::PI / 3.0;
2780 let flipped = Frame::new(
2781 Point::ORIGIN,
2782 -Direction::Z,
2783 Direction::new(Vector::new(third.cos(), third.sin(), 0.0), T).unwrap(),
2784 T,
2785 )
2786 .unwrap();
2787 let b: Curve = CircleCurve::new(Circle::new(flipped, 3.0, T).unwrap()).into();
2788 for pair in [(&a, &b), (&b, &a)] {
2789 let found = intersect_curves(pair.0, pair.1, CurveCurveOptions::default(), T).unwrap();
2790 assert!(
2791 found.crossings.is_empty(),
2792 "every point is a hit, so none is"
2793 );
2794 assert_eq!(found.overlaps.len(), 1);
2795 let overlap = &found.overlaps[0];
2796 let span = overlap.on_a.1 - overlap.on_a.0;
2797 for i in 0..=8 {
2798 let t = f64::from(i) / 8.0;
2799 let ta = span.mul_add(t, overlap.on_a.0);
2800 let tb = (overlap.on_b.1 - overlap.on_b.0).mul_add(t, overlap.on_b.0);
2801 let pa = pair.0.point_at(ta, T).unwrap();
2802 let pb = pair
2803 .1
2804 .point_at(tb.rem_euclid(core::f64::consts::TAU), T)
2805 .unwrap();
2806 assert!(
2807 pa.distance(pb) < 1e-9,
2808 "at {t}: {pa:?} against {pb:?} (on_a {:?} on_b {:?})",
2809 overlap.on_a,
2810 overlap.on_b
2811 );
2812 }
2813 }
2814 }
2815
2816 #[test]
2817 fn space_curves_cross_within_a_gap_and_report_it() {
2818 let a: Curve = CircleCurve::new(Circle::new(Frame::WORLD, 2.0, T).unwrap()).into();
2825 let lifted = Frame::new(
2826 Point::new(3.0, 0.0, 0.001),
2827 ogeom_math::Direction::Z,
2828 ogeom_math::Direction::X,
2829 T,
2830 )
2831 .unwrap();
2832 let b: Curve = CircleCurve::new(Circle::new(lifted, 2.0, T).unwrap()).into();
2833
2834 let options = CurveCurveOptions {
2835 gap: 1e-2,
2836 ..CurveCurveOptions::default()
2837 };
2838 let found = intersect_curves(&a, &b, options, T).unwrap();
2839 assert_eq!(found.crossings.len(), 2, "two near-crossings");
2840 for hit in &found.crossings {
2841 assert!(hit.gap > 1e-4, "the gap is real and must not be zeroed");
2842 assert!(hit.gap < 2e-3, "but small: {}", hit.gap);
2843 }
2844
2845 let strict = CurveCurveOptions {
2847 gap: 1e-5,
2848 ..CurveCurveOptions::default()
2849 };
2850 assert!(intersect_curves(&a, &b, strict, T).unwrap().is_empty());
2851 }
2852
2853 #[test]
2859 fn a_line_through_a_figure_eight_s_double_point_is_crossed_twice() {
2860 let ring = [
2861 Point::new(2.0, 0.0, 0.0),
2862 Point::new(1.0, 1.0, 0.0),
2863 Point::new(-1.0, -1.0, 0.0),
2864 Point::new(-2.0, 0.0, 0.0),
2865 Point::new(-1.0, 1.0, 0.0),
2866 Point::new(1.0, -1.0, 0.0),
2867 ];
2868 let a: Curve = ogeom_geom::BSplineCurve::periodic(&ring, 3, T)
2869 .unwrap()
2870 .into();
2871 let b: Curve = LineCurve::segment(Point::new(0.0, 0.0, -1.0), Point::new(0.0, 0.0, 1.0), T)
2872 .unwrap()
2873 .into();
2874 let found = intersect_curves(&a, &b, CurveCurveOptions::default(), T).unwrap();
2875 assert_eq!(found.crossings.len(), 2, "{:?}", found.crossings);
2876 let (lo, hi) = a.domain();
2877 let apart = (found.crossings[0].on_a - found.crossings[1].on_a).abs();
2878 assert!(
2879 (apart - (hi - lo) / 2.0).abs() < 1e-6,
2880 "half a turn apart: {apart}"
2881 );
2882 for hit in &found.crossings {
2883 assert!(hit.point.distance(Point::ORIGIN) < 1e-9, "{:?}", hit.point);
2884 }
2885 }
2886
2887 #[test]
2888 fn skew_lines_in_space_miss_and_close_ones_meet() {
2889 let a: Curve = LineCurve::segment(Point::ORIGIN, Point::new(10.0, 0.0, 0.0), T)
2890 .unwrap()
2891 .into();
2892 let skew: Curve =
2893 LineCurve::segment(Point::new(0.0, -5.0, 1.0), Point::new(0.0, 5.0, 1.0), T)
2894 .unwrap()
2895 .into();
2896 assert!(
2897 intersect_curves(&a, &skew, CurveCurveOptions::default(), T)
2898 .unwrap()
2899 .is_empty(),
2900 "a unit apart is not a crossing"
2901 );
2902
2903 let meeting: Curve =
2904 LineCurve::segment(Point::new(5.0, -5.0, 0.0), Point::new(5.0, 5.0, 0.0), T)
2905 .unwrap()
2906 .into();
2907 let found = intersect_curves(&a, &meeting, CurveCurveOptions::default(), T).unwrap();
2908 assert_eq!(found.crossings.len(), 1);
2909 assert!(
2910 found.crossings[0]
2911 .point
2912 .is_equal(Point::new(5.0, 0.0, 0.0), T)
2913 );
2914 assert!(found.crossings[0].gap < 1e-12);
2915
2916 let collinear: Curve =
2918 LineCurve::segment(Point::new(4.0, 0.0, 0.0), Point::new(20.0, 0.0, 0.0), T)
2919 .unwrap()
2920 .into();
2921 let shared = intersect_curves(&a, &collinear, CurveCurveOptions::default(), T).unwrap();
2922 assert_eq!(shared.overlaps.len(), 1);
2923 }
2924
2925 #[test]
2926 fn a_fitted_curve_tracing_an_arc_is_one_overlap_not_a_row_of_crossings() {
2927 let circle: Curve = CircleCurve::new(Circle::new(Frame::WORLD, 4.0, T).unwrap()).into();
2933 let points: Vec<Point> = (0..=40)
2934 .map(|i| {
2935 let a = 0.2 + 1.0 * f64::from(i) / 40.0;
2936 Point::new(4.0 * a.cos(), 4.0 * a.sin(), 0.0)
2937 })
2938 .collect();
2939 let fitted: Curve = ogeom_geom::fit::fit_points(&points, 3, 1e-7, T)
2940 .unwrap()
2941 .curve
2942 .into();
2943 let options = CurveCurveOptions {
2944 gap: 1e-5,
2945 ..CurveCurveOptions::default()
2946 };
2947 let found = intersect_curves(&fitted, &circle, options, T).unwrap();
2948 assert_eq!(found.overlaps.len(), 1, "one shared stretch: {found:?}");
2949 let (lo, hi) = found.overlaps[0].on_a;
2950 let (fa, fb) = fitted.domain();
2951 assert!(
2952 lo - fa < 1e-3 && fb - hi < 1e-3,
2953 "the whole fit runs along the circle"
2954 );
2955 assert!(
2956 found.crossings.is_empty(),
2957 "no crossing survives inside the overlap: {:?}",
2958 found.crossings
2959 );
2960 }
2961
2962 #[test]
2968 fn an_arc_across_a_periodic_curve_s_seam_overlaps_it_the_way_it_goes() {
2969 let ellipse: Curve = ogeom_geom::EllipseCurve::new(
2970 ogeom_math::Ellipse::new(Frame::WORLD, 0.5025, 0.5, T).unwrap(),
2971 )
2972 .into();
2973 let arc: Curve = ellipse.to_bspline_over((5.5, 7.0), T).unwrap().into();
2974 let found = intersect_curves(&arc, &ellipse, CurveCurveOptions::default(), T).unwrap();
2975 let [overlap] = found.overlaps.as_slice() else {
2976 panic!("one shared stretch: {found:?}");
2977 };
2978 let (lo, hi) = overlap.on_a;
2979 assert!(lo.abs() < 1e-9 && (hi - 1.0).abs() < 1e-9);
2980 let (from, to) = overlap.on_b;
2981 assert!(
2982 (from - 5.5).abs() < 1e-9 && (to - 7.0).abs() < 1e-9,
2983 "{overlap:?}"
2984 );
2985 }
2986
2987 #[test]
2988 fn an_arc_ending_tangent_to_a_line_is_one_crossing_with_its_reach() {
2989 let circle: Curve = CircleCurve::new(Circle::new(Frame::WORLD, 4.0, T).unwrap()).into();
2994 let line: Curve =
2995 LineCurve::segment(Point::new(4.0, -3.0, 0.0), Point::new(4.0, 3.0, 0.0), T)
2996 .unwrap()
2997 .into();
2998 let options = CurveCurveOptions {
2999 gap: 1e-5,
3000 ..CurveCurveOptions::default()
3001 };
3002 let found = intersect_curves(&circle, &line, options, T).unwrap();
3003 assert_eq!(found.crossings.len(), 1, "one touch: {:?}", found.crossings);
3004 let touch = found.crossings[0];
3005 assert!(
3006 touch.point.distance(Point::new(4.0, 0.0, 0.0)) < 2e-2,
3007 "{touch:?}"
3008 );
3009 assert!(touch.reach < 5e-2, "the valley is short: {touch:?}");
3010 assert!(found.overlaps.is_empty());
3011 }
3012
3013 #[test]
3020 fn a_tangent_line_meets_a_conic_as_the_sampling_path_says() {
3021 use ogeom_geom::{EllipseCurve, TrimmedCurve};
3022 use ogeom_math::{Direction, Ellipse, Vector};
3023 use std::f64::consts::{FRAC_PI_2, PI};
3024 let tilted = Frame::new(
3025 Point::new(3.0, -2.0, 7.0),
3026 Direction::new(Vector::new(1.0, 2.0, 2.0), T).unwrap(),
3027 Direction::new(Vector::new(2.0, -1.0, 0.0), T).unwrap(),
3028 T,
3029 )
3030 .unwrap();
3031 let circle: Curve = CircleCurve::new(Circle::new(Frame::WORLD, 4.0, T).unwrap()).into();
3032 let tilted_circle: Curve = CircleCurve::new(Circle::new(tilted, 1.0, T).unwrap()).into();
3033 let ellipse: Curve =
3034 EllipseCurve::new(Ellipse::new(Frame::WORLD, 3.0, 2.0, T).unwrap()).into();
3035 let arc = |from: f64, to: f64| -> Curve {
3036 TrimmedCurve::new(circle.clone(), from, to, T)
3037 .unwrap()
3038 .into()
3039 };
3040 let segment =
3041 |from: Point, to: Point| -> Curve { LineCurve::segment(from, to, T).unwrap().into() };
3042 let (ox, oy) = (tilted.x().vector(), tilted.y().vector());
3043 let (eu, ev) = (3.0 * 2.5_f64.cos(), 2.0 * 2.5_f64.sin());
3044 let ellipse_tangent = Vector::new(-3.0 * 2.5_f64.sin(), 2.0 * 2.5_f64.cos(), 0.0);
3045 let along_top = segment(Point::new(-5.0, 4.0, 0.0), Point::new(1.0, 4.0, 0.0));
3046 let cases: Vec<(&str, Curve, Curve, bool)> = vec![
3047 ("circle", circle.clone(), along_top.clone(), true),
3048 (
3049 "tilted",
3050 tilted_circle,
3051 segment(
3052 tilted.origin() + oy - ox * 2.0,
3053 tilted.origin() + oy + ox * 5.0,
3054 ),
3055 true,
3056 ),
3057 (
3058 "ellipse's end",
3059 ellipse.clone(),
3060 segment(Point::new(-3.0, -1.0, 0.0), Point::new(-3.0, 4.0, 0.0)),
3061 true,
3062 ),
3063 (
3064 "ellipse's side",
3065 ellipse.clone(),
3066 segment(Point::new(-6.0, -2.0, 0.0), Point::new(2.0, -2.0, 0.0)),
3067 true,
3068 ),
3069 (
3070 "ellipse aslant",
3071 ellipse,
3072 segment(
3073 Point::new(eu, ev, 0.0) - ellipse_tangent,
3074 Point::new(eu, ev, 0.0) + ellipse_tangent * 2.0,
3075 ),
3076 true,
3077 ),
3078 (
3079 "arc ending at the touch",
3080 arc(0.0, FRAC_PI_2),
3081 along_top.clone(),
3082 true,
3083 ),
3084 (
3085 "arc starting at the touch",
3086 arc(FRAC_PI_2, PI),
3087 along_top.clone(),
3088 true,
3089 ),
3090 (
3091 "line ending at the touch",
3092 circle.clone(),
3093 segment(Point::new(-3.0, 4.0, 0.0), Point::new(0.0, 4.0, 0.0)),
3094 true,
3095 ),
3096 (
3097 "lifted",
3098 circle.clone(),
3099 segment(Point::new(-5.0, 4.0, 2e-8), Point::new(1.0, 4.0, 2e-8)),
3100 true,
3101 ),
3102 (
3103 "arc away from the touch",
3104 arc(PI, 2.0 * PI),
3105 along_top,
3106 true,
3107 ),
3108 (
3109 "line short of the touch",
3110 circle.clone(),
3111 segment(Point::new(1.0, 4.0, 0.0), Point::new(3.0, 4.0, 0.0)),
3112 true,
3113 ),
3114 (
3115 "missing by a hair",
3116 circle.clone(),
3117 segment(
3118 Point::new(-5.0, 4.0 + 1e-9, 0.0),
3119 Point::new(1.0, 4.0 + 1e-9, 0.0),
3120 ),
3121 false,
3122 ),
3123 (
3124 "crossing a hair inside",
3125 circle,
3126 segment(
3127 Point::new(-5.0, 4.0 - 1e-9, 0.0),
3128 Point::new(1.0, 4.0 - 1e-9, 0.0),
3129 ),
3130 false,
3131 ),
3132 ];
3133 for gap in [1e-7, 1e-5] {
3134 let options = CurveCurveOptions {
3135 gap,
3136 ..CurveCurveOptions::default()
3137 };
3138 for (name, conic, line, touches) in &cases {
3139 for (a, b) in [(conic, line), (line, conic)] {
3140 let closed = tangent_line_conic_3d(a, b, options, T);
3141 let Some(closed) = closed else {
3142 assert!(!touches, "{name}, gap {gap}: no closed form");
3143 continue;
3144 };
3145 assert!(touches, "{name}, gap {gap}: {closed:?}");
3146 let sampled = general_3d(a, b, options, T).unwrap();
3147 let context = format!("{name}, gap {gap}: {closed:?} against {sampled:?}");
3148 assert!(closed.overlaps.is_empty() && sampled.overlaps.is_empty());
3149 assert_eq!(closed.crossings.len(), sampled.crossings.len(), "{context}");
3150 for (x, y) in closed.crossings.iter().zip(&sampled.crossings) {
3155 let apart = x.point.distance(y.point);
3156 let (p, q) = (
3157 b.point_at(x.on_b, T).unwrap(),
3158 a.point_at(x.on_a, T).unwrap(),
3159 );
3160 assert!(p.distance(q) <= x.gap + 1e-15, "{context}");
3161 assert!(apart <= y.reach.max(gap), "{context}");
3162 assert!(x.gap <= y.gap + 1e-15, "{context}");
3163 assert_eq!(x.reach > 0.0, y.reach > 0.0, "{context}");
3164 assert!(
3165 (x.reach - y.reach).abs() <= apart + 1e-6 * y.reach,
3166 "{context}"
3167 );
3168 }
3169 }
3170 }
3171 }
3172 }
3173
3174 #[test]
3175 fn unusable_options_are_refused() {
3176 let a = line2(Point2::new(0.0, 0.0), Point2::new(1.0, 0.0));
3177 for options in [
3178 CurveCurveOptions {
3179 samples: 1,
3180 ..CurveCurveOptions::default()
3181 },
3182 CurveCurveOptions {
3183 gap: 0.0,
3184 ..CurveCurveOptions::default()
3185 },
3186 CurveCurveOptions {
3187 gap: f64::NAN,
3188 ..CurveCurveOptions::default()
3189 },
3190 ] {
3191 assert!(intersect_curves_2d(&a, &a.clone(), options, T).is_err());
3192 }
3193 }
3194
3195 #[test]
3200 fn circles_and_ellipses_in_different_planes_meet_where_the_planes_do() {
3201 use ogeom_geom::EllipseCurve;
3202 use ogeom_math::{Direction, Ellipse, Vector};
3203 let radius = 2.0;
3204 let tilted = |normal: Vector, major: Vector, lean: f64| -> Curve {
3205 let frame = Frame::new(
3206 Point::ORIGIN,
3207 Direction::new(normal, T).unwrap(),
3208 Direction::new(major, T).unwrap(),
3209 T,
3210 )
3211 .unwrap();
3212 EllipseCurve::new(Ellipse::new(frame, radius / lean.cos(), radius, T).unwrap()).into()
3213 };
3214 let (p, q) = (0.4_f64, 0.7_f64);
3215 let about_x = tilted(
3216 Vector::new(0.0, -p.sin(), p.cos()),
3217 Vector::new(0.0, p.cos(), p.sin()),
3218 p,
3219 );
3220 let about_y = tilted(
3221 Vector::new(-q.sin(), 0.0, q.cos()),
3222 Vector::new(q.cos(), 0.0, q.sin()),
3223 q,
3224 );
3225 let circle: Curve = CircleCurve::new(Circle::new(Frame::WORLD, radius, T).unwrap()).into();
3226 for (a, b) in [
3227 (&about_x, &about_y),
3228 (&about_y, &about_x),
3229 (&circle, &about_x),
3230 (&about_y, &circle),
3231 ] {
3232 let found = intersect_curves(a, b, CurveCurveOptions::default(), T).unwrap();
3233 assert_eq!(found.crossings.len(), 2, "{found:?}");
3234 for hit in &found.crossings {
3235 let on_a = a.point_at(hit.on_a, T).unwrap();
3236 let on_b = b.point_at(hit.on_b, T).unwrap();
3237 assert!(on_a.distance(on_b) < 1e-9, "{on_a:?} against {on_b:?}");
3238 assert!((on_a.x.hypot(on_a.y) - radius).abs() < 1e-9);
3239 }
3240 }
3241 let lifted = Frame::new(Point::new(0.0, 0.0, 1.0), Direction::Z, Direction::X, T).unwrap();
3242 let above: Curve = CircleCurve::new(Circle::new(lifted, radius, T).unwrap()).into();
3243 assert!(
3244 intersect_curves(&circle, &above, CurveCurveOptions::default(), T)
3245 .unwrap()
3246 .is_empty()
3247 );
3248 }
3249
3250 #[test]
3255 fn circles_in_all_but_one_plane_meet_where_they_pass() {
3256 use ogeom_math::{Direction, Vector};
3257 let flat: Curve = CircleCurve::new(Circle::new(Frame::WORLD, 2.0, T).unwrap()).into();
3258 let lean = 1e-6;
3259 let normal = Direction::new(Vector::new(lean, 0.0, 1.0), T).unwrap();
3260 let centre = Point::new(1.0, 0.0, lean.mul_add(-0.5, 5e-8));
3261 let tilted_frame = Frame::new(
3262 centre,
3263 normal,
3264 Direction::new(Vector::new(1.0, 0.0, -lean), T).unwrap(),
3265 T,
3266 )
3267 .unwrap();
3268 let tilted: Curve = CircleCurve::new(Circle::new(tilted_frame, 2.0, T).unwrap()).into();
3269 let found = intersect_curves(&flat, &tilted, CurveCurveOptions::default(), T).unwrap();
3270 assert_eq!(found.crossings.len(), 2, "{found:?}");
3271 for hit in &found.crossings {
3272 assert!(hit.gap < 1e-7, "{hit:?}");
3273 let p = flat.point_at(hit.on_a, T).unwrap();
3274 assert!((p.x - 0.5).abs() < 1e-4, "{p:?}");
3275 }
3276 }
3277
3278 #[test]
3283 fn lines_meet_circles_and_ellipses_in_closed_form() {
3284 use ogeom_geom::EllipseCurve;
3285 use ogeom_math::Ellipse;
3286 let ellipse: Curve =
3287 EllipseCurve::new(Ellipse::new(Frame::WORLD, 3.0, 2.0, T).unwrap()).into();
3288 let across: Curve =
3289 LineCurve::segment(Point::new(-5.0, 1.0, 0.0), Point::new(5.0, 1.0, 0.0), T)
3290 .unwrap()
3291 .into();
3292 for (a, b, line_first) in [(&ellipse, &across, false), (&across, &ellipse, true)] {
3293 let found = intersect_curves(a, b, CurveCurveOptions::default(), T).unwrap();
3294 assert_eq!(found.crossings.len(), 2, "{found:?}");
3295 for hit in &found.crossings {
3296 let p = a.point_at(hit.on_a, T).unwrap();
3297 let q = b.point_at(hit.on_b, T).unwrap();
3298 assert!(p.distance(q) < 1e-9, "{p:?} against {q:?}");
3299 let on_line = if line_first { p } else { q };
3300 assert!((on_line.y - 1.0).abs() < 1e-12);
3301 }
3302 }
3303 let circle: Curve = CircleCurve::new(Circle::new(Frame::WORLD, 2.0, T).unwrap()).into();
3304 let through: Curve =
3305 LineCurve::segment(Point::new(2.0, 0.0, -1.0), Point::new(2.0, 0.0, 1.0), T)
3306 .unwrap()
3307 .into();
3308 let found = intersect_curves(&circle, &through, CurveCurveOptions::default(), T).unwrap();
3309 assert_eq!(found.crossings.len(), 1);
3310 assert!(found.crossings[0].on_a.abs() < 1e-9);
3311 assert!((found.crossings[0].on_b - 1.0).abs() < 1e-9);
3312 let inside: Curve =
3313 LineCurve::segment(Point::new(1.0, 0.0, -1.0), Point::new(1.0, 0.0, 1.0), T)
3314 .unwrap()
3315 .into();
3316 assert!(
3317 intersect_curves(&circle, &inside, CurveCurveOptions::default(), T)
3318 .unwrap()
3319 .is_empty()
3320 );
3321 }
3322
3323 #[test]
3330 fn a_chord_deeper_than_the_gap_crosses_twice_however_shallowly() {
3331 use ogeom_math::{Direction, Vector};
3332 let line: Curve =
3333 LineCurve::segment(Point::new(0.0, 0.0, 10.0), Point::new(0.0, 14.0, 10.0), T)
3334 .unwrap()
3335 .into();
3336 for depth in [3e-7, 5e-7, 1e-6] {
3337 for seam in [Vector::new(0.0, 0.0, 1.0), Vector::new(0.0, 1.0, 0.0)] {
3338 let frame = Frame::new(
3339 Point::new(0.0, 7.0, 8.0 + depth),
3340 Direction::X,
3341 Direction::new(seam, T).unwrap(),
3342 T,
3343 )
3344 .unwrap();
3345 let circle: Curve = CircleCurve::new(Circle::new(frame, 2.0, T).unwrap()).into();
3346 let stored = (8.0 + depth) - 8.0;
3350 let half = (stored * (4.0 - stored)).sqrt();
3351 let along = 16.0 * f64::EPSILON * 10.0 * 2.0 / half;
3352 for (a, b, line_first) in [(&line, &circle, true), (&circle, &line, false)] {
3353 let found = intersect_curves(a, b, CurveCurveOptions::default(), T).unwrap();
3354 assert_eq!(found.crossings.len(), 2, "depth {depth}: {found:?}");
3355 let mut ys: Vec<f64> = found.crossings.iter().map(|c| c.point.y).collect();
3356 ys.sort_by(f64::total_cmp);
3357 for (y, want) in ys.iter().zip([7.0 - half, 7.0 + half]) {
3358 assert!(
3359 (y - want).abs() < along,
3360 "depth {depth}: {y} against {want}"
3361 );
3362 }
3363 for hit in &found.crossings {
3364 let (on_line, on_circle) = if line_first {
3365 (hit.on_a, hit.on_b)
3366 } else {
3367 (hit.on_b, hit.on_a)
3368 };
3369 let p = line.point_at(on_line, T).unwrap();
3370 let q = circle.point_at(on_circle, T).unwrap();
3371 assert!(p.distance(q) < 1e-12, "{p:?} against {q:?}");
3372 }
3373 }
3374 }
3375 }
3376 }
3377
3378 #[test]
3383 fn arcs_of_one_circle_overlap_across_the_seam() {
3384 use ogeom_geom::{Curve3d as _, TrimmedCurve};
3385 let circle = |x: ogeom_math::Direction, z: ogeom_math::Direction| -> Curve {
3386 let frame = Frame::new(Point::new(10.0, 20.0, 30.0), z, x, T).unwrap();
3387 CircleCurve::new(Circle::new(frame, 50.0, T).unwrap()).into()
3388 };
3389 let trim = |c: &Curve, a: f64, b: f64| -> Curve {
3390 TrimmedCurve::new(c.clone(), a, b, T).unwrap().into()
3391 };
3392 let (x, y) = (ogeom_math::Direction::X, ogeom_math::Direction::Y);
3393 let (up, down) = (ogeom_math::Direction::Z, -ogeom_math::Direction::Z);
3394 let plain = circle(x, up);
3395 let turned = circle(y, up);
3396 let backwards = circle(x, down);
3397 for (a, b, share) in [
3399 (trim(&plain, 5.5, 7.0), trim(&plain, 0.2, 1.0), 0.517 / 1.5),
3400 (trim(&plain, 0.2, 1.0), trim(&plain, 5.5, 7.0), 0.517 / 0.8),
3401 (
3402 trim(&plain, 5.8, 6.8),
3403 trim(&backwards, 5.0, 6.2),
3404 0.4336 / 1.0,
3405 ),
3406 (trim(&plain, 5.0, 7.5), backwards.clone(), 1.0),
3407 (trim(&plain, 0.5, 6.0), trim(&turned, 4.0, 6.0), 1.218 / 5.5),
3408 ] {
3409 let found = intersect_curves(&a, &b, CurveCurveOptions::default(), T).unwrap();
3410 let (wa, wb) = (a.domain(), b.domain());
3411 let mut covered = 0.0;
3412 for o in &found.overlaps {
3413 for (t, w) in [
3414 (o.on_a.0, wa),
3415 (o.on_a.1, wa),
3416 (o.on_b.0, wb),
3417 (o.on_b.1, wb),
3418 ] {
3419 assert!(t >= w.0 - 1e-9 && t <= w.1 + 1e-9, "{t} outside {w:?}");
3420 }
3421 for (ta, tb) in [(o.on_a.0, o.on_b.0), (o.on_a.1, o.on_b.1)] {
3422 let gap = a
3423 .point_at(ta, T)
3424 .unwrap()
3425 .distance(b.point_at(tb, T).unwrap());
3426 assert!(gap < 1e-9, "ends {gap} apart");
3427 }
3428 covered += (o.on_a.1 - o.on_a.0).abs();
3429 }
3430 let want = share * (wa.1 - wa.0);
3431 assert!((covered - want).abs() < 2e-3, "{covered} against {want}");
3432 }
3433 }
3434
3435 #[test]
3439 fn a_trimmed_arc_meets_tangents_as_its_circle_does() {
3440 use ogeom_geom::Trimmed2d;
3441 let options = CurveCurveOptions::default();
3442 let circle = circle2(Point2::new(0.0, 0.0), 10.0);
3443 let arc: PlanarCurve = Trimmed2d::new(circle, 0.3, 2.9, T).unwrap().into();
3444 let tangent = line2(Point2::new(-20.0, 10.0), Point2::new(20.0, 10.0));
3445 let found = intersect_curves_2d(&arc, &tangent, options, T).unwrap();
3446 assert_eq!(found.crossings.len(), 1);
3447 assert!(found.crossings[0].point.is_equal(Point2::new(0.0, 10.0), T));
3448 let grazing = line2(
3449 Point2::new(-20.0, 10.0 - 1e-4),
3450 Point2::new(20.0, 10.0 - 1e-4),
3451 );
3452 let found = intersect_curves_2d(&arc, &grazing, options, T).unwrap();
3453 assert_eq!(found.crossings.len(), 2);
3454 let beside: PlanarCurve =
3455 Trimmed2d::new(circle2(Point2::new(20.0, 0.0), 10.0), 2.0, 4.5, T)
3456 .unwrap()
3457 .into();
3458 let whole = circle2(Point2::new(0.0, 0.0), 10.0);
3459 let found = intersect_curves_2d(&whole, &beside, options, T).unwrap();
3460 assert_eq!(found.crossings.len(), 1);
3461 assert!(found.crossings[0].point.is_equal(Point2::new(10.0, 0.0), T));
3462 let below = line2(Point2::new(-20.0, -10.0), Point2::new(20.0, -10.0));
3464 assert!(
3465 intersect_curves_2d(&arc, &below, options, T)
3466 .unwrap()
3467 .is_empty()
3468 );
3469 }
3470
3471 #[test]
3476 fn the_boxed_nearest_segment_is_the_one_every_segment_finds() {
3477 let frame = Frame::new(
3478 Point::new(120.0, -40.0, 7.5),
3479 ogeom_math::Direction::new(ogeom_math::Vector::new(0.2, -0.3, 1.0), T).unwrap(),
3480 ogeom_math::Direction::new(ogeom_math::Vector::new(1.0, 0.0, 0.2), T).unwrap(),
3481 T,
3482 )
3483 .unwrap();
3484 let curve: Curve = CircleCurve::new(Circle::new(frame, 6.0, T).unwrap()).into();
3485 let sampled = sample_3d(&curve, 64, T);
3486 let segments = Segments::of(&sampled);
3487 let every = |p: Point| {
3488 let mut seed = (f64::INFINITY, 0.0);
3489 for j in 1..sampled.points.len() {
3490 let (_, tb, gap) =
3491 segments_approach_3d((p, p), (sampled.points[j - 1], sampled.points[j]));
3492 if gap < seed.0 {
3493 let (a, b) = (sampled.parameters[j - 1], sampled.parameters[j]);
3494 seed = (gap, a + (b - a) * tb);
3495 }
3496 }
3497 seed
3498 };
3499 for k in 0..97 {
3500 let t = f64::from(k * 37 % 97) / 97.0 * core::f64::consts::TAU;
3501 let on = curve.point_at(t, T).unwrap();
3502 for p in [
3503 on,
3504 on + ogeom_math::Vector::new(0.0, 0.0, 1e-9),
3505 frame.origin(),
3506 on + ogeom_math::Vector::new(50.0, 3.0, -8.0),
3507 ] {
3508 let found = foot_via_samples(&curve, &sampled, &segments, p, T);
3509 let seed = every(p);
3510 let expected = foot_on_3d(&curve, p, seed.1, T);
3511 assert_eq!(
3512 found.map(|(s, g)| (s.to_bits(), g.to_bits())),
3513 expected.map(|(s, g)| (s.to_bits(), g.to_bits())),
3514 "at {p:?}"
3515 );
3516 }
3517 }
3518 }
3519}