1b-agent/OneCAD-Draft
1
0 Fork 0
forked from 1b-agent/opencadstudio-upstream
Code Pull Requests Activity

Merge pull request #1326 from yztink/fix-polyline-intersection-snap

Browse Source
This commit is contained in:
Hakan Seven committed 2026-09-17 16:52:27 +03:00
2 parents 8e05ed552c 395256c296
commit fa6bf5e7e4
1 file changed
+199 -91
Download Patch File Download Diff File Expand all files Collapse all files
src/snap.rs
+199 -91
View File
@@ -2765,131 +2765,175 @@ impl WirePlane {
} }
fn wire_plane(wire: &WireModel) -> Option<WirePlane> { fn wire_plane(wire: &WireModel) -> Option<WirePlane> {
let [geom] = wire.tangent_geoms.as_slice() else { for geom in &wire.tangent_geoms {
return None; let plane_axes = match geom {
}; TangentGeom::Circle { center, .. } => Some((
let (origin, axis_x, axis_y) = match geom { DVec3::new(center[0] as f64, center[1] as f64, center[2] as f64),
TangentGeom::Circle { center, .. } => (DVec3::new(center[0] as f64, center[1] as f64, center[2] as f64), DVec3::X, DVec3::Y), DVec3::X,
TangentGeom::PlanarCircle { center, axis_x, axis_y, .. } | TangentGeom::Arc { center, axis_x, axis_y, .. } => ( DVec3::Y,
DVec3::new(center[0], center[1], center[2]), )),
DVec3::new(axis_x[0], axis_x[1], axis_x[2]), TangentGeom::PlanarCircle { center, axis_x, axis_y, .. }
DVec3::new(axis_y[0], axis_y[1], axis_y[2]), | TangentGeom::Arc { center, axis_x, axis_y, .. } => Some((
), DVec3::new(center[0], center[1], center[2]),
TangentGeom::PlanarEllipse { center, major_axis, normal, .. } => { DVec3::new(axis_x[0], axis_x[1], axis_x[2]),
let origin = DVec3::new(center[0], center[1], center[2]); DVec3::new(axis_y[0], axis_y[1], axis_y[2]),
let n = DVec3::new(normal[0], normal[1], normal[2]).normalize(); )),
let major = DVec3::new(major_axis[0], major_axis[1], major_axis[2]); TangentGeom::PlanarEllipse { center, major_axis, normal, .. } => {
if major.length() <= 1e-12 { let origin = DVec3::new(center[0], center[1], center[2]);
return None; let n = DVec3::new(normal[0], normal[1], normal[2]).normalize();
let major = DVec3::new(major_axis[0], major_axis[1], major_axis[2]);
if major.length() <= 1e-12 {
None
} else {
Some((origin, major.normalize(), n.cross(major.normalize())))
}
} }
(origin, major.normalize(), n.cross(major.normalize())) TangentGeom::Line { .. } => None,
};
if let Some((origin, axis_x, axis_y)) = plane_axes {
return Some(WirePlane { origin, axis_x, axis_y, normal: axis_x.cross(axis_y).normalize() });
} }
TangentGeom::Line { .. } => return None, }
}; None
Some(WirePlane { origin, axis_x, axis_y, normal: axis_x.cross(axis_y).normalize() })
} }
fn curve_in_frame(wire: &WireModel, frame: &WirePlane, tol: f64) -> Option<Curve> { fn curves_in_frame(wire: &WireModel, frame: &WirePlane, tol: f64) -> Vec<Curve> {
use cadkernel::geom2d::{Arc as KArc, Circle as KCircle, Ellipse as KEllipse, EllipseArc as KEllipseArc, Line as KLine}; use cadkernel::geom2d::{Arc as KArc, Circle as KCircle, Ellipse as KEllipse, EllipseArc as KEllipseArc, Line as KLine};
let mut curves = Vec::new();
if wire.tangent_geoms.is_empty() { if wire.tangent_geoms.is_empty() {
if wire.points.len() != 2 { if wire.points.len() == 2 {
return None; let (p0, p1) = (wp_f64(wire, 0), wp_f64(wire, 1));
if frame.contains(p0, tol) && frame.contains(p1, tol) {
curves.push(Curve::Line(KLine { start: frame.to_2d(p0), end: frame.to_2d(p1) }));
}
} else if wire.points.len() > 2 {
for i in 0..wire.points.len().saturating_sub(1) {
let p0 = wp_f64(wire, i);
let p1 = wp_f64(wire, i + 1);
if !p0.is_nan() && !p1.is_nan() && frame.contains(p0, tol) && frame.contains(p1, tol) {
curves.push(Curve::Line(KLine { start: frame.to_2d(p0), end: frame.to_2d(p1) }));
}
}
} }
let (p0, p1) = (wp_f64(wire, 0), wp_f64(wire, 1)); return curves;
return (frame.contains(p0, tol) && frame.contains(p1, tol)).then(|| Curve::Line(KLine { start: frame.to_2d(p0), end: frame.to_2d(p1) }));
} }
let [geom] = wire.tangent_geoms.as_slice() else {
return None;
};
let angle_in_frame = |world_point: DVec3, centre: DVec3| { let angle_in_frame = |world_point: DVec3, centre: DVec3| {
let (p2, c2) = (frame.to_2d(world_point), frame.to_2d(centre)); let (p2, c2) = (frame.to_2d(world_point), frame.to_2d(centre));
(p2[1] - c2[1]).atan2(p2[0] - c2[0]) (p2[1] - c2[1]).atan2(p2[0] - c2[0])
}; };
match geom { for (geom_idx, geom) in wire.tangent_geoms.iter().enumerate() {
TangentGeom::Line { p1, p2 } => { match geom {
let (p1, p2) = if wire.points.len() == 2 { TangentGeom::Line { p1, p2 } => {
(wp_f64(wire, 0), wp_f64(wire, 1)) let (p1, p2) = if wire.key_vertices.len() > geom_idx + 1 {
} else { (
(Vec3::from_array(*p1).as_dvec3(), Vec3::from_array(*p2).as_dvec3()) DVec3::from_array(wire.key_vertices[geom_idx]),
}; DVec3::from_array(wire.key_vertices[geom_idx + 1]),
(frame.contains(p1, tol) && frame.contains(p2, tol)).then(|| Curve::Line(KLine { start: frame.to_2d(p1), end: frame.to_2d(p2) })) )
} } else if wire.points.len() == 2 && wire.tangent_geoms.len() == 1 {
TangentGeom::Circle { center, radius } => { (wp_f64(wire, 0), wp_f64(wire, 1))
let c = DVec3::new(center[0] as f64, center[1] as f64, center[2] as f64); } else {
(DVec3::Z.cross(frame.normal).length() <= tol && frame.contains(c, tol)).then(|| Curve::Circle(KCircle { centre: frame.to_2d(c), radius: *radius as f64 })) (Vec3::from_array(*p1).as_dvec3(), Vec3::from_array(*p2).as_dvec3())
} };
TangentGeom::PlanarCircle { center, axis_x, axis_y, radius } => { if frame.contains(p1, tol) && frame.contains(p2, tol) {
let c = DVec3::new(center[0], center[1], center[2]); curves.push(Curve::Line(KLine { start: frame.to_2d(p1), end: frame.to_2d(p2) }));
let n = DVec3::new(axis_x[0], axis_x[1], axis_x[2]).cross(DVec3::new(axis_y[0], axis_y[1], axis_y[2])).normalize(); }
(n.cross(frame.normal).length() <= tol && frame.contains(c, tol)).then(|| Curve::Circle(KCircle { centre: frame.to_2d(c), radius: *radius }))
}
TangentGeom::Arc { center, axis_x, axis_y, radius, start_angle, end_angle } => {
let c = DVec3::new(center[0], center[1], center[2]);
let (ax, ay) = (DVec3::new(axis_x[0], axis_x[1], axis_x[2]), DVec3::new(axis_y[0], axis_y[1], axis_y[2]));
let n = ax.cross(ay).normalize();
if n.cross(frame.normal).length() > tol || !frame.contains(c, tol) {
return None;
} }
let arc = KArc { centre: frame.to_2d(c), radius: *radius, start_angle: *start_angle, end_angle: *end_angle }; TangentGeom::Circle { center, radius } => {
let sweep = arc.sweep(); let c = DVec3::new(center[0] as f64, center[1] as f64, center[2] as f64);
if (sweep - std::f64::consts::TAU).abs() <= 1e-12 { if DVec3::Z.cross(frame.normal).length() <= tol && frame.contains(c, tol) {
return Some(Curve::Circle(KCircle { centre: arc.centre, radius: *radius })); curves.push(Curve::Circle(KCircle { centre: frame.to_2d(c), radius: *radius as f64 }));
}
} }
let boundary = if n.dot(frame.normal) < 0.0 { *end_angle } else { *start_angle }; TangentGeom::PlanarCircle { center, axis_x, axis_y, radius } => {
let start_angle = angle_in_frame(c + *radius * (boundary.cos() * ax + boundary.sin() * ay), c); let c = DVec3::new(center[0], center[1], center[2]);
Some(Curve::Arc(KArc { start_angle, end_angle: start_angle + sweep, ..arc })) let n = DVec3::new(axis_x[0], axis_x[1], axis_x[2]).cross(DVec3::new(axis_y[0], axis_y[1], axis_y[2])).normalize();
} if n.cross(frame.normal).length() <= tol && frame.contains(c, tol) {
curves.push(Curve::Circle(KCircle { centre: frame.to_2d(c), radius: *radius }));
TangentGeom::PlanarEllipse { center, major_axis, normal, minor_axis_ratio, start_param, end_param } => { }
let c = DVec3::new(center[0], center[1], center[2]);
let ell_normal = DVec3::new(normal[0], normal[1], normal[2]).normalize();
if ell_normal.cross(frame.normal).length() > tol || !frame.contains(c, tol) {
return None;
} }
let major = DVec3::new(major_axis[0], major_axis[1], major_axis[2]); TangentGeom::Arc { center, axis_x, axis_y, radius, start_angle, end_angle } => {
let major_radius = major.length(); let c = DVec3::new(center[0], center[1], center[2]);
if major_radius <= 1e-12 { let (ax, ay) = (DVec3::new(axis_x[0], axis_x[1], axis_x[2]), DVec3::new(axis_y[0], axis_y[1], axis_y[2]));
return None; let n = ax.cross(ay).normalize();
if n.cross(frame.normal).length() <= tol && frame.contains(c, tol) {
let arc = KArc { centre: frame.to_2d(c), radius: *radius, start_angle: *start_angle, end_angle: *end_angle };
let sweep = arc.sweep();
if (sweep - std::f64::consts::TAU).abs() <= 1e-12 {
curves.push(Curve::Circle(KCircle { centre: arc.centre, radius: *radius }));
} else {
let boundary = if n.dot(frame.normal) < 0.0 { *end_angle } else { *start_angle };
let start_angle = angle_in_frame(c + *radius * (boundary.cos() * ax + boundary.sin() * ay), c);
curves.push(Curve::Arc(KArc { start_angle, end_angle: start_angle + sweep, ..arc }));
}
}
} }
let minor_radius = major_radius * *minor_axis_ratio; TangentGeom::PlanarEllipse { center, major_axis, normal, minor_axis_ratio, start_param, end_param } => {
let dir2d = frame.dir_to_2d(major / major_radius); let c = DVec3::new(center[0], center[1], center[2]);
let len = (dir2d[0] * dir2d[0] + dir2d[1] * dir2d[1]).sqrt(); let ell_normal = DVec3::new(normal[0], normal[1], normal[2]).normalize();
if len <= 1e-12 { if ell_normal.cross(frame.normal).length() <= tol && frame.contains(c, tol) {
return None; let major = DVec3::new(major_axis[0], major_axis[1], major_axis[2]);
let major_radius = major.length();
if major_radius > 1e-12 {
let minor_radius = major_radius * *minor_axis_ratio;
let dir2d = frame.dir_to_2d(major / major_radius);
let len = (dir2d[0] * dir2d[0] + dir2d[1] * dir2d[1]).sqrt();
if len > 1e-12 {
let major_axis_2d = [dir2d[0] / len, dir2d[1] / len];
let (start_parameter, end_parameter) = if ell_normal.dot(frame.normal) < 0.0 {
(-*end_param, -*start_param)
} else {
(*start_param, *end_param)
};
curves.push(Curve::Ellipse(KEllipseArc {
ellipse: KEllipse { centre: frame.to_2d(c), major_radius, minor_radius, major_axis: major_axis_2d },
start_parameter,
end_parameter,
}));
}
}
}
} }
let major_axis_2d = [dir2d[0] / len, dir2d[1] / len];
let (start_parameter, end_parameter) = if ell_normal.dot(frame.normal) < 0.0 {
(-*end_param, -*start_param)
} else {
(*start_param, *end_param)
};
Some(Curve::Ellipse(KEllipseArc {
ellipse: KEllipse { centre: frame.to_2d(c), major_radius, minor_radius, major_axis: major_axis_2d },
start_parameter,
end_parameter,
}))
} }
} }
curves
} }
fn exact_curve_intersections(wire_a: &WireModel, wire_b: &WireModel) -> Option<Vec<DVec3>> { fn exact_curve_intersections(wire_a: &WireModel, wire_b: &WireModel) -> Option<Vec<DVec3>> {
let frame = wire_plane(wire_a).or_else(|| wire_plane(wire_b))?; let frame = wire_plane(wire_a).or_else(|| wire_plane(wire_b))?;
const PLANE_TOL: f64 = 1e-7; const PLANE_TOL: f64 = 1e-7;
let curve_a = curve_in_frame(wire_a, &frame, PLANE_TOL)?; let curves_a = curves_in_frame(wire_a, &frame, PLANE_TOL);
let curve_b = curve_in_frame(wire_b, &frame, PLANE_TOL)?; let curves_b = curves_in_frame(wire_b, &frame, PLANE_TOL);
if curves_a.is_empty() || curves_b.is_empty() {
return None;
}
// Two lines have nothing this path can improve on (the segment sweep is // Two lines have nothing this path can improve on (the segment sweep is
// already exact for a straight pair); avoid the extra work. // already exact for a straight pair); avoid the extra work.
if matches!(curve_a, Curve::Line(_)) && matches!(curve_b, Curve::Line(_)) { let a_all_lines = curves_a.iter().all(|c| matches!(c, Curve::Line(_)));
let b_all_lines = curves_b.iter().all(|c| matches!(c, Curve::Line(_)));
if a_all_lines && b_all_lines {
return None; return None;
} }
let tolerance = cadkernel::geom2d::Tolerance::new(1e-9_f64.max(PLANE_TOL)); let tolerance = cadkernel::geom2d::Tolerance::new(1e-9_f64.max(PLANE_TOL));
let crossings = cadkernel::geom2d::intersect(&curve_a, &curve_b, tolerance); let mut points: Vec<DVec3> = Vec::new();
let points: Vec<DVec3> = crossings.into_iter().map(|c| frame.to_3d(c.point)).collect(); for ca in &curves_a {
for cb in &curves_b {
let crossings = cadkernel::geom2d::intersect(ca, cb, tolerance);
for c in crossings {
let pt = frame.to_3d(c.point);
if !points.iter().any(|existing| existing.distance_squared(pt) <= 1e-12) {
points.push(pt);
}
}
}
}
Some(points) Some(points)
} }
@@ -4182,4 +4226,68 @@ mod ext_tests {
assert!((res.world.y - 0.5).abs() < 1e-9, "y rounds to 0.5: {:?}", res.world); assert!((res.world.y - 0.5).abs() < 1e-9, "y rounds to 0.5: {:?}", res.world);
} }
#[test]
fn exact_curve_intersections_handles_multisegment_polyline_against_circle_at_vertex() {
let polyline = WireModel {
key_vertices: vec![[-10.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 10.0, 0.0]],
tangent_geoms: vec![
TangentGeom::Line { p1: [-10.0, 0.0, 0.0], p2: [0.0, 0.0, 0.0] },
TangentGeom::Line { p1: [0.0, 0.0, 0.0], p2: [0.0, 10.0, 0.0] },
],
..Default::default()
};
let circle = WireModel {
tangent_geoms: vec![TangentGeom::PlanarCircle {
center: [0.0, 5.0, 0.0],
axis_x: [1.0, 0.0, 0.0],
axis_y: [0.0, 1.0, 0.0],
radius: 5.0,
}],
..Default::default()
};
let pts = exact_curve_intersections(&polyline, &circle)
.expect("polyline must analytically intersect circle");
assert_eq!(pts.len(), 2, "expected intersections at (0,0) and (0,10), got {pts:?}");
let mut ys: Vec<f64> = pts.iter().map(|p| p.y).collect();
ys.sort_by(f64::total_cmp);
assert!((ys[0] - 0.0).abs() < 1e-9, "expected corner intersection at y=0, got {}", ys[0]);
assert!((ys[1] - 10.0).abs() < 1e-9, "expected endpoint intersection at y=10, got {}", ys[1]);
for p in &pts {
assert!(p.x.abs() < 1e-9, "expected x=0, got {}", p.x);
assert!(p.z.abs() < 1e-9, "expected z=0, got {}", p.z);
}
}
#[test]
fn exact_curve_intersections_handles_polyline_with_bulge_arc_against_circle() {
let polyline = WireModel {
tangent_geoms: vec![
TangentGeom::Line { p1: [-10.0, 0.0, 0.0], p2: [0.0, 0.0, 0.0] },
TangentGeom::Arc {
center: [0.0, 4.0, 0.0],
axis_x: [1.0, 0.0, 0.0],
axis_y: [0.0, 1.0, 0.0],
radius: 4.0,
start_angle: -std::f64::consts::FRAC_PI_2,
end_angle: std::f64::consts::FRAC_PI_2,
},
],
..Default::default()
};
let circle = WireModel {
tangent_geoms: vec![TangentGeom::PlanarCircle {
center: [0.0, 0.0, 0.0],
axis_x: [1.0, 0.0, 0.0],
axis_y: [0.0, 1.0, 0.0],
radius: 5.0,
}],
..Default::default()
};
let pts = exact_curve_intersections(&polyline, &circle)
.expect("polyline with arc must intersect circle");
assert!(!pts.is_empty());
let line_pt = pts.iter().find(|p| (p.x - -5.0).abs() < 1e-9 && p.y.abs() < 1e-9);
assert!(line_pt.is_some(), "expected line crossing at (-5, 0, 0), got {pts:?}");
}
} }