From 12496c2c1d01a8fd2157b668d2514612d5a4e34e Mon Sep 17 00:00:00 2001 From: jyviko <2501648+jyviko@users.noreply.github.com> Date: Fri, 30 Oct 2026 10:46:10 -0400 Subject: [PATCH 1/4] Tests: a round flange stays an exact circle through unfold, refold and DXF, with a cut near its rim The test it replaces probed for a polyline by placing a 0.2 um square in the gap between the rim and a 1 um polyline: a micro feature in a millimetre part. This one asserts the property itself, in millimetres: every rim segment of the flat pattern is an arc of radius 10, the DXF draws one circle of radius 10, and a 1 mm cut 0.5 mm inside the rim stays one hole. Co-Authored-By: Claude Opus 5.5 --- crates/engine/src/cmd/sheet_tests.rs | 30 +++++++++++++++------------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/crates/engine/src/cmd/sheet_tests.rs b/crates/engine/src/cmd/sheet_tests.rs index 1f6553bec..627c87591 100644 --- a/crates/engine/src/cmd/sheet_tests.rs +++ b/crates/engine/src/cmd/sheet_tests.rs @@ -2324,36 +2324,38 @@ fn cut_across_a_concave_notch_uses_crossings_not_just_contained_corners() { assert!((volume(&mut s) - 10500.0).abs() < 1e-6); } +/// A round flange stays an exact circle through unfold, refold and DXF (never a polyline), and a +/// cut close to its rim stays a hole in the flat pattern. #[test] -fn tiny_cut_inside_exact_circle_survives_unfold_refold_and_dxf() { +fn a_round_flange_stays_a_circle_and_keeps_a_cut_near_its_rim_through_unfold_refold_and_dxf() { for initially_flat in [false, true] { let mut s = Session::default(); run(&mut s, "sketch.create", json!({"plane": "XY", "name": "Disc"})); run(&mut s, "sketch.circle.center", json!({"center": [0,0], "radius": 10})); run(&mut s, "sketch.finish", json!({})); run(&mut s, "sheet.flange", json!({"sketch": "Disc"})); - let circle = s.model.state().sheets[0].base.outer.clone(); - let displayed = circle.polyline(1e-3); - // A square 0.2 µm across, centred 0.4 µm inside the circle on the bisector of the - // display polyline's first chord (its middle 0.98 µm inside): outside the polyline, inside - // the exact circle, each of its walls and its gap to the rim wider than SMLib's 0.1 µm - // modelling tolerance. - let (half, at) = (0.0001, (displayed[0] + displayed[1]).normalized().unwrap() * 9.9996); - assert!(!circle.contains(at)); if initially_flat { run(&mut s, "sheet.unfold", json!({})); } - run(&mut s, "sketch.create", json!({"plane": "XY", "name": "TinyCut"})); - run(&mut s, "sketch.rectangle.two_point", json!({"p0": [at.x-half,at.y-half], "p1": [at.x+half,at.y+half]})); + // A 1 mm square whose outer side is 0.5 mm inside the rim. + run(&mut s, "sketch.create", json!({"plane": "XY", "name": "Cut"})); + run(&mut s, "sketch.rectangle.two_point", json!({"p0": [8.5,-0.5], "p1": [9.5,0.5]})); run(&mut s, "sketch.finish", json!({})); - run(&mut s, "solid.extrude", json!({"sketch": "TinyCut", "through_all": true, "operation": "cut", "direction": "symmetric"})); + run(&mut s, "solid.extrude", json!({"sketch": "Cut", "through_all": true, "operation": "cut", "direction": "symmetric"})); for stage in ["sheet.unfold", "sheet.refold"] { let regions = s.model.state().sheets[0].flat_regions().unwrap(); + let rim = ®ions[0].outer.segs; + assert!( + !rim.is_empty() && rim.iter().all(|seg| matches!(seg, solidcraft_geom::Seg2::Arc { radius, .. } if (*radius - 10.0).abs() < 1e-12)), + "{rim:?}" + ); assert_eq!(regions[0].holes.len(), 1); - assert!((regions[0].holes[0].signed_area().abs() - 4e-8).abs() < 1e-13); + assert!((regions[0].holes[0].signed_area().abs() - 1.0).abs() < 1e-12); assert_eq!(s.model.state().bodies.len(), 1); let dxf = run(&mut s, "sheet.export_dxf", json!({}))["dxf"].as_str().unwrap().to_string(); - assert_eq!(solidcraft_io::read_dxf(&dxf).unwrap().len(), 5); + let drawn = solidcraft_io::read_dxf(&dxf).unwrap(); + assert!(drawn.iter().any(|g| matches!(g, solidcraft_io::Geom2::Circle(_, r) if (*r - 10.0).abs() < 1e-12)), "{drawn:?}"); + assert_eq!(drawn.len(), 5, "the circle and the square's four sides: {drawn:?}"); if stage == "sheet.unfold" && initially_flat { run(&mut s, "sheet.refold", json!({})); } From 7fe880a7bfd9192cb5093734185f4f3e2180d857 Mon Sep 17 00:00:00 2001 From: jyviko <2501648+jyviko@users.noreply.github.com> Date: Fri, 30 Oct 2026 11:23:09 -0400 Subject: [PATCH 2/4] SMLib: pin f8fd83e (tangency-labs/smlib#66) The batch brings history Kept for a face only cut by a vertex, revolve of a curve touching its axis, loops touching at a vertex, a torus rail on its axis, and the fillet causes behind the owner's bar running into a cylinder: a tangent edge past a tangent side edge is the corner's edge, the rails of a round collapsing at its end end at one point, and a trace from a tangent point starts along the curve. The two peg cases with the sketch's solver residues come off the fillets known-failing list: both build. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01NvoxKQ2rEZvcXNQnhE3Sgs --- crates/smlib/SMLIB_PREBUILT | 16 ++++++++-------- crates/smlib/SMLIB_REV | 2 +- tests/kernel-cases/fillets/KNOWN_FAILING | 2 -- 3 files changed, 9 insertions(+), 11 deletions(-) diff --git a/crates/smlib/SMLIB_PREBUILT b/crates/smlib/SMLIB_PREBUILT index 6e6ca87dd..8f2d58094 100644 --- a/crates/smlib/SMLIB_PREBUILT +++ b/crates/smlib/SMLIB_PREBUILT @@ -1,10 +1,10 @@ # Rust target, SHA-256 of smlib-.tar.gz in the release prebuilt- of # tangency-labs/smlib. Written by scripts/fetch-smlib.sh --pin; never edited by hand. -aarch64-apple-darwin 449768fb27dddbc2613bba876e96a762fd4dc36f8ce43e498679a98c3a9c03f1 -aarch64-pc-windows-msvc 1ce7c9e59d390c855c8d73688340fb747ae6e00fe969065ba646980735abc447 -aarch64-unknown-linux-gnu 014ba0b4734de8fb8665b58394624a68dced523b09a804f2258a3808cfcda7b2 -i686-pc-windows-msvc 38ff5885eaba1f0681eb2be777b16fdfdb986ca66c0496c4f4235ec3dc045e84 -wasm32-unknown-unknown 7227d3b5f208122d39dfab0b9573efc4037d0ea9fd0bfec6f91dba27a9434fe0 -x86_64-apple-darwin cde31baadd1a807f6a58b3ab17815a939940e2bf82ee032c3a526ecf6d0a41d4 -x86_64-pc-windows-msvc c0d02f0e0e59c977fd134d872e049d7eb3e027be4ea9c888418fea0cbabd675c -x86_64-unknown-linux-gnu edc4a1e08d3263bafbcd44dbd243a7815ca4d535dc308a89b1844126c0bea613 +aarch64-apple-darwin a0fe19504aee96d2e5de3ac9e4ef7215d649ebb420373a7cc93d62eca3761e2f +aarch64-pc-windows-msvc ad359c3996e4255086b193722a987286c1d094556eb053df80fd1538c5f9340d +aarch64-unknown-linux-gnu 4d0ac76e060de13a67ab60de6b207598bfc79d68b649875caaedbab4cac63d5a +i686-pc-windows-msvc 1218c4f9fa94f769fe8c45ee29d166d1aecd27cb57486de8883c7fba699f6812 +wasm32-unknown-unknown d61597525a3e1c6910890f9848cb85bb5934b2bcdfd8e6551246d90f8b5f94b8 +x86_64-apple-darwin 395f629a4e88132ae69874ff55898f493df75b4d8627a6a663f92ca6b633eb58 +x86_64-pc-windows-msvc 89876ff51ced8dd0513f3837ea1c12965ff957fec3d10051321da31014d24619 +x86_64-unknown-linux-gnu b4bd2990bd100f9a00a24b77f743208f4395adb8725924bcf939fd04a873ef10 diff --git a/crates/smlib/SMLIB_REV b/crates/smlib/SMLIB_REV index be726d954..504115e8a 100644 --- a/crates/smlib/SMLIB_REV +++ b/crates/smlib/SMLIB_REV @@ -1 +1 @@ -985faaeed0311d5eaf5e6f091f62ac55c382597c +f8fd83e2fd5a70cf036fffbfaff0e7bc88e0563a diff --git a/tests/kernel-cases/fillets/KNOWN_FAILING b/tests/kernel-cases/fillets/KNOWN_FAILING index a319fc13f..f18c6d3f8 100644 --- a/tests/kernel-cases/fillets/KNOWN_FAILING +++ b/tests/kernel-cases/fillets/KNOWN_FAILING @@ -46,7 +46,5 @@ every-hole-top-rim-r12-past-plate-bottom SMLib status 1001: a convex round past every-pocket-floor-and-rim-edges-r2 a corner patch where two rim rounds meet at each concave pocket corner (23 faces, not 19) instead of the mitre the rounds make running into each other every-pocket-rim-edges-r2 a corner patch where two rim rounds meet at each concave pocket corner (19 faces, not 15) instead of the mitre the rounds make running into each other every-pocket-rim-edges-r5-as-deep-as-pocket a corner patch where two rim rounds meet at each concave pocket corner (15 faces, not 11) instead of the mitre the rounds make running into each other -report-fillet-concave-arc-bar-top-into-cylinder-solver-residues SMLib status 1001: SmFilletNx1Corner cannot make the topology at the arc's end (fillet error 3, corner type 8) -report-fillet-corner-after-round-running-into-cylinder-solver-residues SMLib's check refuses the round: its corner's surface of revolution is close to singular at its boundary (SmSurface test 5) report-fillet-curvature-three-edges-at-bar-corner SMLib's check refuses the open setback sheet of curvature rounds: a 2D B-spline curve cusp (SmBSplineCurve test 13) report-fillet-edge-tangent-at-one-end-into-cylinder SMLib refuses the round as larger than the faces beside its edges allow (SMLIB_ERR_NO_ROOM): it fades to nothing at the edge's tangent end, on the wall SMLib now gets back as its cylinder From b4af45dd07f3d3c2c4c71062ee26dd03b516b916 Mon Sep 17 00:00:00 2001 From: jyviko <2501648+jyviko@users.noreply.github.com> Date: Fri, 30 Oct 2026 12:05:02 -0400 Subject: [PATCH 3/4] Kernel rig: the horn torus and the circle revolved touching its axis build SMLib's revolve now splits a curve where it touches the axis inside itself (tangency-labs/smlib#66), so both cases come off their known-failing lists. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01NvoxKQ2rEZvcXNQnhE3Sgs --- tests/kernel-cases/import/KNOWN_FAILING | 1 - tests/kernel-cases/sweeps/KNOWN_FAILING | 1 - 2 files changed, 2 deletions(-) diff --git a/tests/kernel-cases/import/KNOWN_FAILING b/tests/kernel-cases/import/KNOWN_FAILING index 7eafa4152..0f9669661 100644 --- a/tests/kernel-cases/import/KNOWN_FAILING +++ b/tests/kernel-cases/import/KNOWN_FAILING @@ -2,4 +2,3 @@ # rig gate (.github/workflows/kernel-rig.yml) fails on a case that fails and is not listed here, # and on a listed case that passes: a fix removes its line in the same pull request. # Baseline: develop 5ada4fc8a with SMLib 90aabff (the run on 60b1c6a, aarch64 Linux). -cov-step-horn-torus-quarter-round-trip SMLib revolve status 1001 on a profile touching its axis (the part is never built) diff --git a/tests/kernel-cases/sweeps/KNOWN_FAILING b/tests/kernel-cases/sweeps/KNOWN_FAILING index ed57ec761..82aa00854 100644 --- a/tests/kernel-cases/sweeps/KNOWN_FAILING +++ b/tests/kernel-cases/sweeps/KNOWN_FAILING @@ -2,4 +2,3 @@ # rig gate (.github/workflows/kernel-rig.yml) fails on a case that fails and is not listed here, # and on a listed case that passes: a fix removes its line in the same pull request. # Baseline: not seeded yet; seeded from a develop run after the SMLib pin bump. -cov-revolve-circle-touching-its-axis-a-quarter-turn SMLib revolve status 1001 on a profile touching its axis From f1199f89bcb1171e8ea63091060b9a09257b58d3 Mon Sep 17 00:00:00 2001 From: jyviko <2501648+jyviko@users.noreply.github.com> Date: Fri, 30 Oct 2026 13:02:39 -0400 Subject: [PATCH 4/4] Joint drive: contact is decided on the exact bodies, not their display meshes A joint drive with contact on tested each sample and each of 14 bisection steps with a brute-force mesh-against-mesh overlap check, its cost the product of the two display meshes' triangle counts under a fixed budget. Since the display mesh puts nodes inside planar faces, a cam and its tappet or a block and a sphere exceed that budget ("motion exceeds contact sweep collision budget"), and whether a motion may run depended on display tessellation. Now, at each sample, bodies whose exact boxes meet are intersected exactly (SMLib boolean, exact volume); a shared volume is a contact, a touch (a cam and its follower) is none. Between the last clear sample and the first overlapping one the drive closes in by conservative advancement: from a clear placement it moves no point of the pair further than the pair's exact gap (body_distance), until the gap is within 1e-9 of the bodies' size. Only clear placements are measured, so no intersection is asked to tell touching from overlapping within its tolerance (SMLib's boolean fails there, status 1001, on two cubes 1e-4 into each other). The budget is 2048 intersections per drive. Fixes cmd::joints::tests::cam::a_cam_drives_a_closed_loop_and_stops_at_a_contact and contact_sweep ordinary_curved_bodies_stop_at_their_contact_within_the_sampling_budget, and the other contact, cam, motion, interference and clearance tests pass. Co-Authored-By: Claude Opus 5.5 --- crates/engine/src/cmd/contact_sweep.rs | 152 ++++++++++++++++++++++++- crates/engine/src/cmd/motion.rs | 56 +++++++-- 2 files changed, 194 insertions(+), 14 deletions(-) diff --git a/crates/engine/src/cmd/contact_sweep.rs b/crates/engine/src/cmd/contact_sweep.rs index d58534918..3b7dcdf92 100644 --- a/crates/engine/src/cmd/contact_sweep.rs +++ b/crates/engine/src/cmd/contact_sweep.rs @@ -1,5 +1,8 @@ -//! Bounded, chronological checks of a joint trajectory. This samples tessellated geometry; -//! it is deliberately not advertised as exact continuous collision detection. +//! Bounded, chronological checks of a joint trajectory. The motion is sampled, so this is not +//! continuous collision detection. A joint drive decides on the exact bodies whether they pass +//! into each other at a sample ([`ContactGeometry::overlaps`]) and closes in on where they first +//! touch through placements known to be clear ([`ContactGeometry::gap`]); gaps and interference +//! depths are measured on the bodies' contact meshes. use std::cell::Cell; use std::collections::{BTreeMap, BTreeSet, HashMap}; use std::sync::Arc; @@ -16,6 +19,9 @@ const MAX_PLACEMENTS: usize = 4096; const MAX_BODIES: usize = 256; const MAX_TRIANGLES: usize = 50_000; const MAX_WORK: usize = 50_000_000; +/// Most exact intersections one joint drive works out: one per pair of bodies whose exact boxes +/// meet, at each sample and bisection step. +const MAX_INTERSECTIONS: usize = 2048; fn limit(message: &str) -> crate::EngineError { bad("contact.sweep", message) @@ -25,6 +31,9 @@ fn limit(message: &str) -> crate::EngineError { struct CachedBody { path: Vec, offset: Mat, + /// The body itself, for the exact checks, and its exact box in its own frame. + body: solidcraft_kernel::Body, + exact_box: Aabb3, mesh: Arc, size: f64, /// How far its mesh may stray from its faces: 0 when every face is flat (the mesh is the @@ -94,6 +103,14 @@ fn contact_mesh(body: &solidcraft_doc::ModelBody) -> Result { /// A member of a contact set: (occurrence id, name). pub(super) type Member = (u64, String); +/// Two set members whose bodies pass into each other: their names, and the bodies (indices in +/// [`ContactGeometry`]) that do. +pub(super) struct Overlap { + pub(super) a: String, + pub(super) b: String, + bodies: (usize, usize), +} + pub(super) struct ContactGeometry { bodies: Vec, /// Body indices, and the set members (occurrence id, name) they are under. @@ -171,6 +188,8 @@ impl ContactGeometry { out.bodies.push(CachedBody { path: path.clone(), offset: s.doc.model_body_offset(body).copied().unwrap_or(IDENTITY), + body: body.body.clone(), + exact_box: body.body.exact_bounds()?, mesh, size: body.body.size(), chord, @@ -301,8 +320,119 @@ impl ContactGeometry { .collect() } - pub(super) fn first_hit(&self, s: &Session, work: &mut usize) -> Result> { - Ok(self.hits(s, work, MAX_WORK, false)?.into_iter().next().map(|(a, b, depth)| (a.1, b.1, depth))) + /// The pairs of bodies of set members that pass into each other in `s` (every pair, or the + /// first found when `all` is false), decided on the exact bodies: their intersection holds + /// volume. Bodies that only touch, such as a cam and its follower, share none. Only bodies + /// whose exact boxes meet are intersected; `intersections` counts them, refused past the + /// drive's budget. + pub(super) fn overlaps(&self, s: &Session, intersections: &mut usize, all: bool) -> Result> { + let frames = self.frames(s)?; + let boxes: Vec = self.bodies.iter().zip(&frames).map(|(b, f)| placed_box(&b.exact_box, f)).collect(); + let mut placed: Vec> = vec![None; self.bodies.len()]; + let mut out = Vec::new(); + for (a, b, oa, ob) in &self.pairs { + let (Some(xa), Some(xb)) = (boxes.get(*a), boxes.get(*b)) else { + return Err(limit("invalid contact geometry pair")); + }; + if !xa.meets(xb) { + continue; + } + *intersections = intersections.saturating_add(1); + if *intersections > MAX_INTERSECTIONS { + return Err(limit("motion exceeds contact sweep intersection budget; simplify the contact set or use smaller increments")); + } + let undecided = |e: &dyn std::fmt::Display| { + bad("contact.sweep", format!("whether {} and {} pass into each other could not be decided ({e})", oa.1, ob.1)) + }; + let (pa, pb) = (self.placed_body(*a, &frames, &mut placed)?, self.placed_body(*b, &frames, &mut placed)?); + let Some(shared) = solidcraft_kernel::boolean(&pa, &pb, solidcraft_kernel::BoolOp::Intersect).map_err(|e| undecided(&e))? else { + continue; + }; + if solidcraft_kernel::measure_mass(&shared).map_err(|e| undecided(&e))?.volume > 0.0 { + out.push(Overlap { a: oa.1.clone(), b: ob.1.clone(), bodies: (*a, *b) }); + if !all { + break; + } + } + } + Ok(out) + } + + /// The least gap between the bodies of `pairs` in `s`, where they are clear of each other: their + /// exact distance ([`solidcraft_kernel::body_distance`]), less the meshes' chord tolerances + /// where it could only be measured on the meshes, so the gap is never more than the bodies'. + /// Between bodies that pass into each other it means nothing (a distance does not tell them + /// from touching ones): it is asked only of placements known to be clear. + pub(super) fn gap(&self, s: &Session, pairs: &[Overlap]) -> Result { + let frames = self.frames(s)?; + let mut placed: Vec> = vec![None; self.bodies.len()]; + let mut least = f64::INFINITY; + for pair in pairs { + let (a, b) = pair.bodies; + let (Some(ca), Some(cb), Some(fa), Some(fb)) = (self.bodies.get(a), self.bodies.get(b), frames.get(a), frames.get(b)) else { + return Err(limit("invalid contact geometry pair")); + }; + let (pa, pb) = (self.placed_body(a, &frames, &mut placed)?, self.placed_body(b, &frames, &mut placed)?); + let mesh = |c: &CachedBody, f: &Mat| Mesh { + positions: c.mesh.positions.iter().map(|p| apply_point(f, *p)).collect(), + triangles: c.mesh.triangles.clone(), + ..Mesh::default() + }; + let (ma, mb) = (mesh(ca, fa), mesh(cb, fb)); + let (sa, sb) = (solidcraft_kernel::DistanceShape::new(&pa, &ma, ca.chord), solidcraft_kernel::DistanceShape::new(&pb, &mb, cb.chord)); + let Some(d) = solidcraft_kernel::body_distance(&sa, &sb, f64::INFINITY)? else { + return Err(limit("invalid contact geometry pair")); + }; + let gap = if d.exact { d.distance } else { d.distance - ca.chord - cb.chord }; + least = least.min(gap.max(0.0)); + } + Ok(least) + } + + /// The furthest a point of the bodies of `pairs` moves toward the other body of its pair + /// from where `from` ([`ContactGeometry::frames`]) placed them to where `to` has them + /// (bounded by the two bodies' own furthest moves, each by the corners of its exact box, + /// which a rigid move takes furthest). + pub(super) fn pair_travel(&self, from: &[Mat], to: &Session, pairs: &[Overlap]) -> Result { + let moved = |i: usize| -> Result { + let (Some(body), Some(fa)) = (self.bodies.get(i), from.get(i)) else { + return Err(limit("invalid contact geometry pair")); + }; + let fb = Self::frame(body, to)?; + let local = &body.exact_box; + let mut most = 0.0_f64; + for c in 0..8 { + let pick = |bit: usize, lo: f64, hi: f64| if c & bit == 0 { lo } else { hi }; + let corner = Vec3::new(pick(1, local.min.x, local.max.x), pick(2, local.min.y, local.max.y), pick(4, local.min.z, local.max.z)); + most = most.max(apply_point(fa, corner).dist(apply_point(&fb, corner))); + } + Ok(most) + }; + let mut most = 0.0_f64; + for pair in pairs { + most = most.max(moved(pair.bodies.0)? + moved(pair.bodies.1)?); + } + Ok(most) + } + + /// Where `s` places each body. + pub(super) fn frames(&self, s: &Session) -> Result> { + self.bodies.iter().map(|b| Self::frame(b, s)).collect() + } + + /// Body `i` placed by `frames`, made once per check. + fn placed_body(&self, i: usize, frames: &[Mat], placed: &mut [Option]) -> Result { + if let Some(Some(done)) = placed.get(i) { + return Ok(done.clone()); + } + let (Some(body), Some(frame)) = (self.bodies.get(i), frames.get(i)) else { + return Err(limit("invalid contact geometry pair")); + }; + let moved = solidcraft_kernel::transform_matrix(&body.body, *frame)?; + if let Some(slot) = placed.get_mut(i) { + *slot = Some(moved.clone()); + } + Ok(moved) } /// The pairs of set members whose bodies pass through each other in `s`, with how deep the @@ -431,6 +561,20 @@ impl ContactGeometry { } } +/// The box holding `local` placed by `frame` (its corners' box: a rigid motion keeps the box's +/// contents inside it). +fn placed_box(local: &Aabb3, frame: &Mat) -> Aabb3 { + let mut world = Aabb3::EMPTY; + for c in 0..8 { + let pick = |bit: usize, lo: f64, hi: f64| if c & bit == 0 { lo } else { hi }; + world.add(apply_point( + frame, + Vec3::new(pick(1, local.min.x, local.max.x), pick(2, local.min.y, local.max.y), pick(4, local.min.z, local.max.z)), + )); + } + world +} + fn limit_error(message: &str) -> crate::EngineError { limit(message) } diff --git a/crates/engine/src/cmd/motion.rs b/crates/engine/src/cmd/motion.rs index 69955852c..5de79b5bf 100644 --- a/crates/engine/src/cmd/motion.rs +++ b/crates/engine/src/cmd/motion.rs @@ -404,30 +404,62 @@ fn drive_with_contact(s: &mut Session, want: &[(u64, Vec)], given: &BTreeSe let mut end = s.scratch(); set(&mut end, 1.0); let samples = cache.samples(s, &end)?; - let mut work = 0; + let mut intersections = 0; set(s, 0.0); // Keep the existing recovery behaviour: an initially overlapping assembly may move // out of its invalid placement. Sweeps constrain motions that start collision-free. - if cache.first_hit(s, &mut work)?.is_none() { + if cache.overlaps(s, &mut intersections, false)?.is_empty() { let mut lo = 0.0; let mut bracket = None; for sample in 1..=samples { let hi = sample as f64 / samples as f64; set(s, hi); - if let Some(hit) = cache.first_hit(s, &mut work)? { - bracket = Some((lo, hi, hit)); + let hits = cache.overlaps(s, &mut intersections, true)?; + if !hits.is_empty() { + bracket = Some((lo, hi, hits)); break; } lo = hi; } - if let Some((mut lo, mut hi, hit)) = bracket { - for _ in 0..14 { - let mid = (lo + hi) / 2.0; - set(s, mid); - if cache.first_hit(s, &mut work)?.is_none() { lo = mid } else { hi = mid } + // Where the bodies found passing into each other first touch, closed in on from the + // clear side (conservative advancement): from a clear placement, a move in which no + // point of a pair goes further than the pair's gap keeps it clear. Only clear + // placements are measured, so no intersection is asked to tell touching bodies from + // overlapping ones within its tolerance. + if let Some((lo, hi, hits)) = bracket { + let touch = 1e-9 * cache.scale(); + let mut t = lo; + set(s, t); + for _ in 0..MAX_ADVANCES { + let gap = cache.gap(s, &hits)?; + if gap <= touch { + break; + } + let from = cache.frames(s)?; + // The furthest step whose move stays within the gap: guessed from the move to + // `hi` (nearly proportional over a sample), then halved until it does. + set(s, hi); + let whole = cache.pair_travel(&from, s, &hits)?; + let mut step = if whole > 0.0 { (hi - t) * (0.5 * gap / whole).min(1.0) } else { hi - t }; + let mut next = None; + for _ in 0..64 { + set(s, t + step); + if cache.pair_travel(&from, s, &hits)? < gap { + next = Some(t + step); + break; + } + step *= 0.5; + } + match next { + Some(n) if n > t => t = n, + _ => break, + } + set(s, t); } + let lo = t; sol = set(s, lo); - stopped = Some(json!({"a": hit.0, "b": hit.1, "fraction": lo})); + let (a, b) = hits.first().map(|h| (h.a.clone(), h.b.clone())).unwrap_or_default(); + stopped = Some(json!({"a": a, "b": b, "fraction": lo})); } else { sol = set(s, 1.0); } @@ -458,6 +490,10 @@ fn drive_with_contact(s: &mut Session, want: &[(u64, Vec)], given: &BTreeSe Ok((out, sol)) } +/// Most steps a joint drive takes closing in on a contact. Each moves the pair by up to half its +/// gap; past the last, the drive stops at the last clear placement, short of the touch. +const MAX_ADVANCES: usize = 200; + /// Drive one joint (joint.drive), honouring contact sets. pub fn drive_joint(s: &mut Session, id: u64, q: Vec, given: usize) -> Result { let given = (0..given.min(q.len())).map(|i| (id, i)).collect();