diff --git a/crates/engine/src/cmd/contact_sweep.rs b/crates/engine/src/cmd/contact_sweep.rs index 7004e9a7e..e253aea34 100644 --- a/crates/engine/src/cmd/contact_sweep.rs +++ b/crates/engine/src/cmd/contact_sweep.rs @@ -302,9 +302,10 @@ impl ContactGeometry { let cost = ma .positions .len() - .saturating_add(ma.triangles.len().saturating_mul(4)) + // 4 sample points and 3 edges to probe per triangle of `a` against each triangle of `b` (#3323), and the other way round. + .saturating_add(ma.triangles.len().saturating_mul(7)) .saturating_mul(mb.triangles.len()) - .saturating_add(mb.positions.len().saturating_add(mb.triangles.len().saturating_mul(4)).saturating_mul(ma.triangles.len())); + .saturating_add(mb.positions.len().saturating_add(mb.triangles.len().saturating_mul(7)).saturating_mul(ma.triangles.len())); *work = work.saturating_add(cost); if *work > budget { return Err(limit("motion exceeds contact sweep collision budget; simplify the contact set or use smaller increments")); @@ -522,8 +523,8 @@ fn tri_distance(a: &[Vec3; 3], b: &[Vec3; 3]) -> (f64, Vec3, Vec3) { best } -/// Where segment pq passes through triangle t (None: it misses, or lies in its plane). -fn segment_hits_triangle(p: Vec3, q: Vec3, t: &[Vec3; 3]) -> Option { +/// Where the segment `p`→`q` hits triangle `t` (endpoints count), if it crosses its plane. +pub(super) fn segment_hits_triangle(p: Vec3, q: Vec3, t: &[Vec3; 3]) -> Option { let d = q - p; let (e1, e2) = (t[1] - t[0], t[2] - t[0]); let h = d.cross(e2); diff --git a/crates/engine/src/cmd/motion.rs b/crates/engine/src/cmd/motion.rs index d83094a84..e49e97334 100644 --- a/crates/engine/src/cmd/motion.rs +++ b/crates/engine/src/cmd/motion.rs @@ -179,6 +179,8 @@ pub fn collisions(s: &Session) -> Vec<(String, String, f64)> { /// How deep one body reaches into the other, when more than a touch (`None`: apart or /// touching). Points of each tessellation (vertices, edge midpoints, triangle centres) are /// tested against the other closed mesh: inside by ray parity, and deeper than the tolerance. +/// Edges are tested for crossing the other's surface too: plates crossing face to face have +/// no sample point inside (#3323). fn overlap(a: &Body, b: &Body) -> Option { let (ma, mb) = (a.tessellate((a.size() * 1e-3).max(1e-3)).ok()?, b.tessellate((b.size() * 1e-3).max(1e-3)).ok()?); mesh_overlap(&ma, &mb, 1e-4 * a.size().max(b.size()).max(1.0)) @@ -208,8 +210,10 @@ fn deepest(a: &solidcraft_geom::Mesh, b: &solidcraft_geom::Mesh, tol: f64) -> f6 let tb = tris(b); let bx = b.bounds(); let mut pts: Vec = a.positions.clone(); + let mut edges: Vec<(Vec3, Vec3)> = Vec::new(); for [p, q, r] in tris(a) { pts.extend([(p + q) * 0.5, (q + r) * 0.5, (r + p) * 0.5, (p + q + r) * (1.0 / 3.0)]); + edges.extend([(p, q), (q, r), (r, p)]); } let mut best = 0.0f64; for p in pts.into_iter().take(200_000) { @@ -227,6 +231,52 @@ fn deepest(a: &solidcraft_geom::Mesh, b: &solidcraft_geom::Mesh, tol: f64) -> f6 best = d; } } + for (p, q) in edges.into_iter().take(200_000) { + best = best.max(crossing(p, q, &tb, bx, tol)); + } + best +} + +/// Where the edge `p`→`q` of one mesh reaches through the surface `tb` of the other: the +/// depth, or 0 when it misses, only touches, or grazes (the middle of the chord it cuts must +/// lie inside, deeper than `tol`, as elsewhere in the check). Two plates crossing each other's +/// faces put no vertex, mid-edge or triangle centre of either inside the other — the chord an +/// edge cuts is how that crossing is seen (#3323). +fn crossing(p: Vec3, q: Vec3, tb: &[[Vec3; 3]], bx: Aabb3, tol: f64) -> f64 { + if p.x.max(q.x) < bx.min.x - tol + || p.x.min(q.x) > bx.max.x + tol + || p.y.max(q.y) < bx.min.y - tol + || p.y.min(q.y) > bx.max.y + tol + || p.z.max(q.z) < bx.min.z - tol + || p.z.min(q.z) > bx.max.z + tol + { + return 0.0; + } + let d = q - p; + let len2 = d.dot(d); + if len2 <= 0.0 { + return 0.0; + } + // Where the edge crosses the surface, along the edge. A crossing at a vertex or along an + // edge shared by two triangles is found twice (or more); merges of neighbours leave the + // entry and exit of each chord as one pair each. + let mut hits: Vec = tb.iter().filter_map(|t| contact_sweep::segment_hits_triangle(p, q, t)).map(|h| (h - p).dot(d) / len2).collect(); + if hits.len() < 2 { + return 0.0; + } + hits.sort_by(|x, y| x.total_cmp(y)); + hits.dedup_by(|x, y| (*x - *y).abs() < 1e-6); + let mut best = 0.0; + for pair in hits.chunks(2) { + if pair.len() < 2 { + break; + } + let m = p + d * ((pair[0] + pair[1]) * 0.5); + let deep = tb.iter().map(|[a, b, c]| closest(m, *a, *b, *c).dist(m)).fold(f64::MAX, f64::min); + if deep > tol.max(best) && inside(m, tb) { + best = deep; + } + } best } diff --git a/crates/engine/src/cmd/motion_tests.rs b/crates/engine/src/cmd/motion_tests.rs index 44974d82b..5d7594f7c 100644 --- a/crates/engine/src/cmd/motion_tests.rs +++ b/crates/engine/src/cmd/motion_tests.rs @@ -71,6 +71,57 @@ fn contact_sets_stop_a_driven_joint_at_touching() { assert!(s.execute("contact.create", &json!({"occurrences": ["A"]})).is_err()); } +/// A base plate and a lid plate (both 40 × 40 × 3) hinged along their shared edge, driven +/// past flat by `angle` degrees. +fn hinged_plates(angle: f64) -> Session { + let mut s = Session::default(); + run( + &mut s, + "solid.box", + json!({"length": 40.0, "width": 40.0, "height": 3.0, "corner": [-20.0, -40.0, 0.0], "operation": "new", "body_name": "BasePlate"}), + ); + run( + &mut s, + "solid.box", + json!({"length": 40.0, "width": 40.0, "height": 3.0, "corner": [-20.0, 0.0, 0.0], "operation": "new", "body_name": "LidPlate"}), + ); + run(&mut s, "component.create", json!({"name": "Base", "parent": "root", "activate": false})); + run(&mut s, "component.move_bodies", json!({"bodies": ["BasePlate"], "component": "Base"})); + run(&mut s, "component.create", json!({"name": "Lid", "parent": "root", "activate": false})); + run(&mut s, "component.move_bodies", json!({"bodies": ["LidPlate"], "component": "Lid"})); + run(&mut s, "occurrence.ground", json!({"occurrence": "Base:1", "grounded": true})); + run( + &mut s, + "joint.create", + json!({"type": "revolute", "name": "Hinge", "flip": true, "limits": [[0.0, 270.0]], + "a": {"occurrence": "Base", "point": [0.0, 0.0, 3.0], "z": [1.0, 0.0, 0.0]}, + "b": {"occurrence": "Lid", "point": [0.0, 0.0, 3.0], "z": [1.0, 0.0, 0.0]}}), + ); + run(&mut s, "joint.drive", json!({"joint": "Hinge", "value": angle, "quiet": true})); + run(&mut s, "contact.create", json!({"occurrences": ["Base:1", "Lid:1"]})); + s +} + +/// Regression (#3323): past the angles where a corner lies inside the other plate, no vertex, +/// mid-edge or triangle centre of either plate is inside it, and `contact.check` reported no +/// collision while `inspect.interference` found the overlap. The faces crossing is the +/// collision; the check must see it. +#[test] +fn contact_check_finds_plates_crossing_face_to_face() { + // Flat against the base: touching is not crossing. + let mut s = hinged_plates(180.0); + assert_eq!(run(&mut s, "contact.check", json!({}))["collisions"].as_array().map(Vec::len), Some(0), "flat"); + for angle in [210.0, 240.0, 270.0] { + let mut s = hinged_plates(angle); + let collisions = run(&mut s, "contact.check", json!({}))["collisions"].clone(); + let c = collisions.as_array().unwrap(); + assert_eq!(c.len(), 1, "{angle}°: {c:?}"); + assert_eq!(c[0]["a"], "Base:1"); + assert_eq!(c[0]["b"], "Lid:1"); + assert!(c[0]["depth_mm"].as_f64().unwrap() > 0.5, "{angle}°: {c:?}"); + } +} + #[test] fn contact_uses_preserved_body_placements_after_component_reassignment() { let mut s = Session::default();