diff --git a/crates/bone-kernel/src/intersect3.rs b/crates/bone-kernel/src/intersect3.rs new file mode 100644 index 0000000..60c3230 --- /dev/null +++ b/crates/bone-kernel/src/intersect3.rs @@ -0,0 +1,290 @@ +use bone_types::{Angle, Parameter, Plane3, Point2, Point3, Tolerance, UnitVec3, Vec3}; +use uom::si::angle::radian; + +use crate::arc2::Arc2; +use crate::arc3::Arc3; +use crate::circle2::Circle2; +use crate::circle3::Circle3; +use crate::circular3; +use crate::curve2::Curve2; +use crate::curve3::{Curve3, Curve3Kind}; +use crate::intersect::{IntersectionSet, IntersectionSet2, intersect_curves}; +use crate::line2::Line2; +use crate::line3::Line3; + +pub type IntersectionSet3 = IntersectionSet; + +#[must_use] +pub fn intersect_curves_3( + a: &Curve3Kind, + b: &Curve3Kind, + tolerance: Tolerance, +) -> IntersectionSet3 { + use Curve3Kind::{Arc, Circle, Line}; + match (a, b) { + (Line(l), Line(m)) => line_line(l, m, tolerance), + (Line(l), Circle(c)) | (Circle(c), Line(l)) => line_circle(l, c, tolerance), + (Line(l), Arc(a)) | (Arc(a), Line(l)) => line_arc(l, a, tolerance), + (Circle(c), Circle(d)) => circle_circle(c, d, tolerance), + (Circle(c), Arc(a)) | (Arc(a), Circle(c)) => circle_arc(c, a, tolerance), + (Arc(a), Arc(b)) => arc_arc(a, b, tolerance), + } +} + +#[must_use] +fn unit_to_vec(u: UnitVec3) -> Vec3 { + let (x, y, z) = u.components(); + Vec3::from_mm(x, y, z) +} + +#[must_use] +fn project(plane: Plane3, p: Point3) -> Point2 { + let (px, py, _) = circular3::local_coords(plane, p); + Point2::from_mm(px, py) +} + +#[must_use] +fn lift(plane: Plane3, p: Point2) -> Point3 { + let (px, py) = p.coords_mm(); + let (ox, oy, oz) = plane.origin().coords_mm(); + let (xx, xy, xz) = plane.x_axis().components(); + let (yx, yy, yz) = plane.y_axis().components(); + Point3::from_mm( + ox + px * xx + py * yx, + oy + px * xy + py * yy, + oz + px * xz + py * yz, + ) +} + +#[must_use] +fn lift_set(plane: Plane3, set: IntersectionSet2) -> IntersectionSet3 { + match set { + IntersectionSet::Empty => IntersectionSet3::Empty, + IntersectionSet::One(p) => IntersectionSet3::One(lift(plane, p)), + IntersectionSet::Two(a, b) => IntersectionSet3::Two(lift(plane, a), lift(plane, b)), + IntersectionSet::Coincident => IntersectionSet3::Coincident, + } +} + +#[must_use] +fn keep_points(set: IntersectionSet3, keep: impl Fn(Point3) -> bool) -> IntersectionSet3 { + match set { + IntersectionSet3::Empty => IntersectionSet3::Empty, + IntersectionSet3::Coincident => IntersectionSet3::Coincident, + IntersectionSet3::One(p) => { + if keep(p) { + IntersectionSet3::One(p) + } else { + IntersectionSet3::Empty + } + } + IntersectionSet3::Two(a, b) => match (keep(a), keep(b)) { + (true, true) => IntersectionSet3::Two(a, b), + (true, false) => IntersectionSet3::One(a), + (false, true) => IntersectionSet3::One(b), + (false, false) => IntersectionSet3::Empty, + }, + } +} + +fn line_line(a: &Line3, b: &Line3, tolerance: Tolerance) -> IntersectionSet3 { + let (pa, pb) = (a.start(), b.start()); + let dir_a = a.end() - a.start(); + let dir_b = b.end() - b.start(); + let eps = tolerance.value(); + let (la, lb) = (a.length_mm(), b.length_mm()); + let aa = dir_a.dot_mm2(dir_a); + let ab = dir_a.dot_mm2(dir_b); + let bb = dir_b.dot_mm2(dir_b); + let offset = pa - pb; + let dot_a = dir_a.dot_mm2(offset); + let dot_b = dir_b.dot_mm2(offset); + let denom = aa * bb - ab * ab; + + if dir_a.cross(dir_b).norm_mm() <= eps * la.min(lb) { + let between = pb - pa; + let along = between.dot_mm2(dir_a) / aa; + if (between - dir_a * along).norm_mm() > eps { + return IntersectionSet3::Empty; + } + let other_end = (b.end() - pa).dot_mm2(dir_a) / aa; + let (lo, hi) = if along <= other_end { + (along, other_end) + } else { + (other_end, along) + }; + let overlap_lo = lo.max(0.0); + let overlap_hi = hi.min(1.0); + let eps_param = eps / la; + if overlap_hi < overlap_lo - eps_param { + return IntersectionSet3::Empty; + } + if overlap_hi <= overlap_lo + eps_param { + return IntersectionSet3::One(pa + dir_a * f64::midpoint(overlap_lo, overlap_hi)); + } + return IntersectionSet3::Coincident; + } + + let param_a = (ab * dot_b - bb * dot_a) / denom; + let param_b = (aa * dot_b - ab * dot_a) / denom; + let (eps_a, eps_b) = (eps / la, eps / lb); + if param_a < -eps_a || param_a > 1.0 + eps_a || param_b < -eps_b || param_b > 1.0 + eps_b { + return IntersectionSet3::Empty; + } + let on_a = pa + dir_a * param_a; + let on_b = pb + dir_b * param_b; + if (on_b - on_a).norm_mm() <= eps { + IntersectionSet3::One(on_a + (on_b - on_a) * 0.5) + } else { + IntersectionSet3::Empty + } +} + +fn line_circle(line: &Line3, circle: &Circle3, tolerance: Tolerance) -> IntersectionSet3 { + let plane = circle.plane(); + let normal = unit_to_vec(plane.normal()); + let u = line.end() - line.start(); + let eps = tolerance.value(); + let r = circle.radius_mm(); + let un = u.dot_mm2(normal); + let dist_start = (line.start() - plane.origin()).dot_mm2(normal); + + if un.abs() <= eps { + if dist_start.abs() > eps { + return IntersectionSet3::Empty; + } + let (Ok(l2), Ok(c2)) = ( + Line2::new( + project(plane, line.start()), + project(plane, line.end()), + tolerance, + ), + Circle2::new(Point2::origin(), circle.radius(), tolerance), + ) else { + return IntersectionSet3::Empty; + }; + return lift_set( + plane, + intersect_curves(&l2.as_kind(), &c2.as_kind(), tolerance), + ); + } + + let s = -dist_start / un; + let eps_s = eps / line.length_mm(); + if s < -eps_s || s > 1.0 + eps_s { + return IntersectionSet3::Empty; + } + let x = line.start() + u * s; + let radial = (x - plane.origin()).norm_mm(); + if (radial - r).abs() <= eps { + IntersectionSet3::One(x) + } else { + IntersectionSet3::Empty + } +} + +fn line_arc(line: &Line3, arc: &Arc3, tolerance: Tolerance) -> IntersectionSet3 { + let set = line_circle(line, &arc.as_full_circle(), tolerance); + keep_points(set, |p| arc.contains_point(p, tolerance)) +} + +fn circle_circle(a: &Circle3, b: &Circle3, tolerance: Tolerance) -> IntersectionSet3 { + let (pa, pb) = (a.plane(), b.plane()); + let na = unit_to_vec(pa.normal()); + let nb = unit_to_vec(pb.normal()); + let eps = tolerance.value(); + let cross = na.cross(nb); + + if cross.norm_mm() <= eps { + if (pb.origin() - pa.origin()).dot_mm2(na).abs() > eps { + return IntersectionSet3::Empty; + } + let (Ok(a2), Ok(b2)) = ( + Circle2::new(Point2::origin(), a.radius(), tolerance), + Circle2::new(project(pa, pb.origin()), b.radius(), tolerance), + ) else { + return IntersectionSet3::Empty; + }; + return lift_set( + pa, + intersect_curves(&a2.as_kind(), &b2.as_kind(), tolerance), + ); + } + + let axis_u = unit_to_vec(pa.x_axis()); + let axis_v = unit_to_vec(pa.y_axis()); + let ra = a.radius_mm(); + let coef_cos = ra * nb.dot_mm2(axis_u); + let coef_sin = ra * nb.dot_mm2(axis_v); + let rhs = nb.dot_mm2(b.center() - a.center()); + let amp = (coef_cos * coef_cos + coef_sin * coef_sin).sqrt(); + if rhs.abs() > amp + eps { + return IntersectionSet3::Empty; + } + let phi = coef_sin.atan2(coef_cos); + let delta = (rhs / amp).clamp(-1.0, 1.0).acos(); + let on_b = + |candidate: Point3| ((candidate - b.center()).norm_mm() - b.radius_mm()).abs() <= eps; + let point_at = |t: f64| a.center() + axis_u * (ra * t.cos()) + axis_v * (ra * t.sin()); + let mut hits = [phi - delta, phi + delta] + .into_iter() + .map(point_at) + .filter(|&candidate| on_b(candidate)); + match (hits.next(), hits.next()) { + (None, _) => IntersectionSet3::Empty, + (Some(p), None) => IntersectionSet3::One(p), + (Some(p), Some(q)) => { + if (q - p).norm_mm() <= eps { + IntersectionSet3::One(p) + } else { + IntersectionSet3::Two(p, q) + } + } + } +} + +fn circle_arc(circle: &Circle3, arc: &Arc3, tolerance: Tolerance) -> IntersectionSet3 { + let set = circle_circle(circle, &arc.as_full_circle(), tolerance); + keep_points(set, |p| arc.contains_point(p, tolerance)) +} + +fn arc_arc(a: &Arc3, b: &Arc3, tolerance: Tolerance) -> IntersectionSet3 { + match circle_circle(&a.as_full_circle(), &b.as_full_circle(), tolerance) { + IntersectionSet3::Coincident => coincident_arc_arc(a, b, tolerance), + other => keep_points(other, |p| { + a.contains_point(p, tolerance) && b.contains_point(p, tolerance) + }), + } +} + +fn coincident_arc_arc(a: &Arc3, b: &Arc3, tolerance: Tolerance) -> IntersectionSet3 { + let plane = a.plane(); + let start_b = b.evaluate(Parameter::new(0.0)); + let (bx, by, _) = circular3::local_coords(plane, start_b); + let alpha0 = by.atan2(bx); + let winding = unit_to_vec(plane.normal()).dot_mm2(unit_to_vec(b.plane().normal())); + let sweep_b = b.sweep_rad() * if winding >= 0.0 { 1.0 } else { -1.0 }; + + let (Ok(a2), Ok(b2)) = ( + Arc2::new( + Point2::origin(), + a.radius(), + a.start_angle(), + a.sweep_angle(), + tolerance, + ), + Arc2::new( + Point2::origin(), + a.radius(), + Angle::new::(alpha0), + Angle::new::(sweep_b), + tolerance, + ), + ) else { + return IntersectionSet3::Coincident; + }; + lift_set( + plane, + intersect_curves(&a2.as_kind(), &b2.as_kind(), tolerance), + ) +} diff --git a/crates/bone-kernel/tests/intersect3.rs b/crates/bone-kernel/tests/intersect3.rs new file mode 100644 index 0000000..b6cb6bd --- /dev/null +++ b/crates/bone-kernel/tests/intersect3.rs @@ -0,0 +1,268 @@ +use bone_kernel::{Arc3, Circle3, Curve3Kind, IntersectionSet3, Line3, intersect_curves_3}; +use bone_types::{Angle, Length, Plane3, Point3, Tolerance, UnitVec3}; +use core::f64::consts::{FRAC_PI_2, FRAC_PI_4, PI}; +use uom::si::angle::radian; +use uom::si::length::millimeter; + +const TOL: Tolerance = Tolerance::new(1e-9); + +fn mm(value: f64) -> Length { + Length::new::(value) +} + +fn xy_plane(origin: Point3) -> Plane3 { + Plane3::new_unchecked(origin, UnitVec3::x_axis(), UnitVec3::y_axis()) +} + +fn xz_plane(origin: Point3) -> Plane3 { + Plane3::new_unchecked(origin, UnitVec3::x_axis(), UnitVec3::z_axis()) +} + +fn line(start: Point3, end: Point3) -> Curve3Kind { + let Ok(seg) = Line3::new(start, end, TOL) else { + panic!("line endpoints distinct"); + }; + seg.as_kind() +} + +fn circle(plane: Plane3, radius: f64) -> Curve3Kind { + let Ok(disc) = Circle3::new(plane, mm(radius), TOL) else { + panic!("radius positive"); + }; + disc.as_kind() +} + +fn arc(plane: Plane3, radius: f64, start: f64, sweep: f64) -> Curve3Kind { + let Ok(span) = Arc3::new( + plane, + mm(radius), + Angle::new::(start), + Angle::new::(sweep), + TOL, + ) else { + panic!("sweep nonzero"); + }; + span.as_kind() +} + +fn row(label: &str, first: &Curve3Kind, second: &Curve3Kind) -> String { + format!("{label} = {}", intersect_curves_3(first, second, TOL)) +} + +fn approx(point: Point3, x: f64, y: f64, z: f64) -> bool { + let (px, py, pz) = point.coords_mm(); + (px - x).abs() < 1e-9 && (py - y).abs() < 1e-9 && (pz - z).abs() < 1e-9 +} + +#[test] +fn intersection_matrix_surface() { + let diag_up = line( + Point3::from_mm(-5.0, -5.0, 0.0), + Point3::from_mm(5.0, 5.0, 0.0), + ); + let diag_down = line( + Point3::from_mm(-5.0, 5.0, 0.0), + Point3::from_mm(5.0, -5.0, 0.0), + ); + let skew = line( + Point3::from_mm(-5.0, 0.0, 3.0), + Point3::from_mm(5.0, 0.0, 3.0), + ); + let collinear_overlap = line( + Point3::from_mm(-2.0, -2.0, 0.0), + Point3::from_mm(2.0, 2.0, 0.0), + ); + let collinear_touch = line( + Point3::from_mm(5.0, 5.0, 0.0), + Point3::from_mm(9.0, 9.0, 0.0), + ); + let parallel = line( + Point3::from_mm(-5.0, -4.0, 0.0), + Point3::from_mm(5.0, 6.0, 0.0), + ); + let upright_segment = line( + Point3::from_mm(3.0, 0.0, -5.0), + Point3::from_mm(3.0, 0.0, 5.0), + ); + let in_plane = line( + Point3::from_mm(-6.0, 0.0, 0.0), + Point3::from_mm(6.0, 0.0, 0.0), + ); + + let flat_circle = circle(xy_plane(Point3::origin()), 3.0); + let shifted_circle = circle(xy_plane(Point3::from_mm(4.0, 0.0, 0.0)), 3.0); + let twin_circle = circle(xy_plane(Point3::origin()), 3.0); + let upright_circle = circle(xz_plane(Point3::origin()), 3.0); + + let flat_arc = arc(xy_plane(Point3::origin()), 3.0, -FRAC_PI_2, PI); + let distant_arc = arc( + xy_plane(Point3::origin()), + 3.0, + FRAC_PI_2 + FRAC_PI_4, + FRAC_PI_4, + ); + let upright_arc = arc(xz_plane(Point3::origin()), 3.0, -FRAC_PI_2, PI); + + let rows = [ + row("line_cross_line", &diag_up, &diag_down), + row("line_skew_line", &diag_up, &skew), + row("line_collinear_overlap", &diag_up, &collinear_overlap), + row("line_collinear_touch", &diag_up, &collinear_touch), + row("line_parallel", &diag_up, ¶llel), + row("line_pierces_circle", &upright_segment, &flat_circle), + row("line_in_plane_circle", &in_plane, &flat_circle), + row("line_in_plane_arc", &in_plane, &flat_arc), + row("circle_circle_coplanar", &flat_circle, &shifted_circle), + row("circle_circle_same", &flat_circle, &twin_circle), + row("circle_circle_perpendicular", &flat_circle, &upright_circle), + row("circle_arc_coplanar", &shifted_circle, &flat_arc), + row("arc_arc_hit", &flat_arc, &upright_arc), + row("arc_arc_miss", &flat_arc, &distant_arc), + ] + .join("\n"); + insta::assert_snapshot!(rows); +} + +#[test] +fn skew_lines_do_not_meet() { + let horizontal = line( + Point3::from_mm(-5.0, 0.0, 0.0), + Point3::from_mm(5.0, 0.0, 0.0), + ); + let lifted = line( + Point3::from_mm(0.0, -5.0, 2.0), + Point3::from_mm(0.0, 5.0, 2.0), + ); + assert_eq!( + intersect_curves_3(&horizontal, &lifted, TOL), + IntersectionSet3::Empty + ); +} + +#[test] +fn crossing_lines_meet_at_origin() { + let diag_up = line( + Point3::from_mm(-5.0, -5.0, 1.0), + Point3::from_mm(5.0, 5.0, 1.0), + ); + let diag_down = line( + Point3::from_mm(-5.0, 5.0, 1.0), + Point3::from_mm(5.0, -5.0, 1.0), + ); + let IntersectionSet3::One(meet) = intersect_curves_3(&diag_up, &diag_down, TOL) else { + panic!("crossing lines meet once"); + }; + assert!(approx(meet, 0.0, 0.0, 1.0)); +} + +#[test] +fn line_pierces_circle_plane_on_curve() { + let upright_segment = line( + Point3::from_mm(3.0, 0.0, -5.0), + Point3::from_mm(3.0, 0.0, 5.0), + ); + let disc = circle(xy_plane(Point3::origin()), 3.0); + let IntersectionSet3::One(hit) = intersect_curves_3(&upright_segment, &disc, TOL) else { + panic!("line through (3,0,0) hits the circle once"); + }; + assert!(approx(hit, 3.0, 0.0, 0.0)); +} + +#[test] +fn perpendicular_circles_meet_at_two_points() { + let flat_disc = circle(xy_plane(Point3::origin()), 5.0); + let upright_disc = circle(xz_plane(Point3::origin()), 5.0); + let IntersectionSet3::Two(first, second) = intersect_curves_3(&flat_disc, &upright_disc, TOL) + else { + panic!("circles in perpendicular planes share two points"); + }; + let hit_plus = approx(first, 5.0, 0.0, 0.0) || approx(second, 5.0, 0.0, 0.0); + let hit_minus = approx(first, -5.0, 0.0, 0.0) || approx(second, -5.0, 0.0, 0.0); + assert!( + hit_plus && hit_minus, + "expected (±5, 0, 0), got {first} and {second}" + ); +} + +#[test] +fn near_parallel_circles_keep_their_two_hits() { + let center = Point3::from_mm(10.0, 20.0, 30.0); + let flat = circle(xy_plane(center), 5.0); + let theta: f64 = 1e-8; + let tilted = circle( + Plane3::new_unchecked( + center, + UnitVec3::x_axis(), + UnitVec3::new_unchecked(0.0, theta.cos(), theta.sin()), + ), + 5.0, + ); + let IntersectionSet3::Two(p, q) = intersect_curves_3(&flat, &tilted, TOL) else { + panic!("circles tilted by 1e-8 rad still cross twice on the shared x-axis"); + }; + let near = |pt: Point3, x: f64| { + let (px, py, pz) = pt.coords_mm(); + (px - x).abs() < 1e-6 && (py - 20.0).abs() < 1e-6 && (pz - 30.0).abs() < 1e-6 + }; + assert!( + (near(p, 15.0) && near(q, 5.0)) || (near(p, 5.0) && near(q, 15.0)), + "expected (15,20,30) and (5,20,30), got {p} and {q}" + ); +} + +#[test] +fn coplanar_same_circle_is_coincident() { + let original = circle(xy_plane(Point3::origin()), 3.0); + let duplicate = circle(xy_plane(Point3::origin()), 3.0); + assert_eq!( + intersect_curves_3(&original, &duplicate, TOL), + IntersectionSet3::Coincident + ); +} + +#[test] +fn coincident_arcs_with_opposite_normals_share_both_endpoints() { + let upper = arc(xy_plane(Point3::origin()), 3.0, 0.0, PI); + let lower = arc( + Plane3::new_unchecked( + Point3::origin(), + UnitVec3::x_axis(), + UnitVec3::new_unchecked(0.0, -1.0, 0.0), + ), + 3.0, + 0.0, + PI, + ); + let IntersectionSet3::Two(p, q) = intersect_curves_3(&upper, &lower, TOL) else { + panic!("two half-circles on the same circle meet at both shared endpoints"); + }; + let hit_plus = approx(p, 3.0, 0.0, 0.0) || approx(q, 3.0, 0.0, 0.0); + let hit_minus = approx(p, -3.0, 0.0, 0.0) || approx(q, -3.0, 0.0, 0.0); + assert!( + hit_plus && hit_minus, + "expected (±3, 0, 0), got {p} and {q}" + ); +} + +#[test] +fn coincident_arcs_with_one_dimensional_overlap_are_coincident() { + let lead = arc(xy_plane(Point3::origin()), 3.0, 0.0, PI); + let trail = arc(xy_plane(Point3::origin()), 3.0, FRAC_PI_2, PI); + assert_eq!( + intersect_curves_3(&lead, &trail, TOL), + IntersectionSet3::Coincident + ); +} + +#[test] +fn coincident_arcs_abutting_at_one_endpoint_meet_once() { + let lead = arc(xy_plane(Point3::origin()), 3.0, 0.0, PI); + let trail = arc(xy_plane(Point3::origin()), 3.0, PI, FRAC_PI_2); + let IntersectionSet3::One(touch) = intersect_curves_3(&lead, &trail, TOL) else { + panic!("arcs abutting only at angle pi meet exactly once"); + }; + assert!( + approx(touch, -3.0, 0.0, 0.0), + "expected (-3, 0, 0), got {touch}" + ); +} diff --git a/crates/bone-kernel/tests/snapshots/intersect3__intersection_matrix_surface.snap b/crates/bone-kernel/tests/snapshots/intersect3__intersection_matrix_surface.snap new file mode 100644 index 0000000..68b01d7 --- /dev/null +++ b/crates/bone-kernel/tests/snapshots/intersect3__intersection_matrix_surface.snap @@ -0,0 +1,18 @@ +--- +source: crates/bone-kernel/tests/intersect3.rs +expression: rows +--- +line_cross_line = one{ (0 mm, 0 mm, 0 mm) } +line_skew_line = empty +line_collinear_overlap = coincident +line_collinear_touch = one{ (5 mm, 5 mm, 0 mm) } +line_parallel = empty +line_pierces_circle = one{ (3 mm, 0 mm, 0 mm) } +line_in_plane_circle = two{ (-3 mm, 0 mm, 0 mm), (3 mm, 0 mm, 0 mm) } +line_in_plane_arc = one{ (3 mm, 0 mm, 0 mm) } +circle_circle_coplanar = two{ (2 mm, 2.23606797749979 mm, 0 mm), (2 mm, -2.23606797749979 mm, 0 mm) } +circle_circle_same = coincident +circle_circle_perpendicular = two{ (-3 mm, -0.00000000000000036739403974420594 mm, 0 mm), (3 mm, 0 mm, 0 mm) } +circle_arc_coplanar = two{ (2 mm, -2.23606797749979 mm, 0 mm), (2 mm, 2.23606797749979 mm, 0 mm) } +arc_arc_hit = one{ (3 mm, 0 mm, 0 mm) } +arc_arc_miss = empty