SolidCraft: an open-source parametric 3D CAD application built in pure Rust. Desktop, web, and automation through MCP.
Joints: a loop whose axes are a hair out of parallel still moves (#6211) (#6602)
yhamil043afcd69a5c36
diff --git a/crates/doc/src/joints.rs b/crates/doc/src/joints.rsindex 257c7a49ca..ff7f93e068 100644--- a/crates/doc/src/joints.rs+++ b/crates/doc/src/joints.rs@@ -1393,9 +1393,11 @@ fn closure_gaps(workspace: &mut PlacementWorkspace, closing: &[u64], vals: &Valu}/// How far a closing joint may be off before it is a conflict: its frames apart by more than-/// this many millimetres or turned by more than this many radians.-const CONFLICT_MM: f64 = 1e-6;-const CONFLICT_RAD: f64 = 1e-8;+/// this many millimetres or turned by more than this many radians (a micrometre over a metre).+/// Holes picked on parts are rarely exactly in line: axes a few hundredths of a microradian+/// apart (#6211) are not a conflict, and the loop through them still moves.+const CONFLICT_MM: f64 = 1e-4;+const CONFLICT_RAD: f64 = 1e-6;/// The largest gap (`closure_gaps`, for loops of size `scale`) that still passes the conflict/// check, with room to spare.@@ -1405,8 +1407,9 @@ fn loose_gap(scale: f64) -> f64 {/// Close loops whose gaps `f` gives from `x`, and the tolerance to keep them closed to from here/// on: `tol`, or, where they come no closer than that but within `loose`, a little more than they-/// do come. Faces modelled a few nanometres apart along parallel axes leave a gap no value of a-/// planar loop takes out; the loop still moves, as closed as it is. `None` when they can't close.+/// do come. Faces modelled a few nanometres apart along parallel axes, or axes a hair out of+/// parallel, leave a gap no value of a planar loop takes out (the second one that grows as the+/// loop turns); the loop still moves, as closed as it comes. `None` when they can't close.fn close_within(f: &mut dyn FnMut(&[f64]) -> Option<Vec<f64>>, x: &mut Vec<f64>, bounds: &[(f64, f64)], tol: f64, loose: f64) -> Option<f64> {if least_squares(f, x, bounds, tol) {return Some(tol);@@ -1714,7 +1717,8 @@ fn close_group(// that close is left as it is.let mut x = x0;let start = largest(&gaps(&x, 0.0)?);- let tol = match close_within(&mut |x| gaps(x, 0.0), &mut x, &bounds, tol, loose_gap(scale))? {+ let loose = loose_gap(scale);+ let tol = match close_within(&mut |x| gaps(x, 0.0), &mut x, &bounds, tol, loose)? {t if t > tol && moves.is_empty() && start <= t => return Some(BTreeMap::new()),t => t,};@@ -1726,7 +1730,9 @@ fn close_group(}let t1 = (t + dt).min(1.0);let mut trial = x.clone();- if least_squares(&mut |x| gaps(x, t1), &mut trial, &bounds, tol) {+ // Each step as closed as the start, or within the conflict check where the loop's gap+ // grows as it moves (axes a hair out of parallel, #6211).+ if close_within(&mut |x| gaps(x, t1), &mut trial, &bounds, tol, loose).is_some() {// A step that moved some loop value too far (through a link and a limit, or close to// a dead point) is taken again in halves.let next = effective(&trial, t1);@@ -1741,7 +1747,7 @@ fn close_group(dt *= 0.5;}}- if largest(&gaps(&x, 1.0)?) > tol {+ if largest(&gaps(&x, 1.0)?) > tol.max(loose) {return None;}// The values that changed from what is stored.diff --git a/crates/doc/src/joints_reach.rs b/crates/doc/src/joints_reach.rsindex 2274bfc10a..7e87749e36 100644--- a/crates/doc/src/joints_reach.rs+++ b/crates/doc/src/joints_reach.rs@@ -10,7 +10,7 @@ use solidcraft_sketch::linalg;use super::{Assembly, Held, MAX_LOOP_VALUES, MAX_LOOP_WORK, MAX_LOOPS, Solution, Tree, Values, apply_links, build, close_within, jacobian, joint_values,- largest, least_squares, limit_bounds, link_chains, loop_groups, loop_scale, loose_gap, unknowns,+ largest, limit_bounds, link_chains, loop_groups, loop_scale, loose_gap, unknowns,};use crate::{Document, ModelState, apply_point, apply_vector, mat_mul};@@ -325,7 +325,7 @@ fn stride(asm: &Assembly, vals: &Values, vars: &[(u64, usize)], paced: &[((u64,/// Levenberg-Marquardt that stays on the loops: move `x` (within `bounds`, at most `cap` per value/// and a step of every value `stride` measures, links included, a step) to bring the first `mt` values of `f` (the target's misses) toward zero, the rest (the/// loops' weighted gaps) at zero. Each step is solved over all the rows, values on a limit the-/// step pushes past held, then put back on the loops (`least_squares` on their gaps) and taken+/// step pushes past held, then put back on the loops (`close_within` on their gaps) and taken/// when it gets closer. It ends at the target, or as close as the limits and loops let it get./// Returns `f` there, or `None` when the loops don't close at the start (within `loose`).fn approach(@@ -341,14 +341,14 @@ fn approach(let n = x.len();let miss = |r: &[f64]| r.iter().take(mt).map(|v| v * v).sum::<f64>();// The loops close first, to `tol` or as near as they come within `loose` (`close_within`),- // and stay that closed.+ // and stay that closed, or within `loose` where their gap grows as they move (#6211).let loops = f(x)?.len() > mt;let mut loop_tol = tol * LOOP_WEIGHT;if loops {loop_tol = close_within(&mut |y| f(y).map(|r| r.get(mt..).unwrap_or_default().to_vec()), x, bounds, loop_tol, loose * LOOP_WEIGHT)?;}let close = |f: &mut dyn FnMut(&[f64]) -> Option<Vec<f64>>, x: &mut Vec<f64>| {- least_squares(&mut |y| f(y).map(|r| r.get(mt..).unwrap_or_default().to_vec()), x, bounds, loop_tol)+ close_within(&mut |y| f(y).map(|r| r.get(mt..).unwrap_or_default().to_vec()), x, bounds, loop_tol, loose * LOOP_WEIGHT).is_some()};let mut r = f(x)?;let (mut lambda, mut kicked) = (1e-3, false);diff --git a/crates/engine/src/cmd/joints_tests.rs b/crates/engine/src/cmd/joints_tests.rsindex 095f5856f8..c9d22a9313 100644--- a/crates/engine/src/cmd/joints_tests.rs+++ b/crates/engine/src/cmd/joints_tests.rs@@ -667,6 +667,65 @@ fn slotted_arm_drives(yaw_first: bool, gap: f64) {assert!(arm > 19.9 && arm <= 20.0 + 1e-9, "yaw first {yaw_first}, gap {gap}: the arm stopped at {arm}°");}+/// The four-bar with its rocker a rod on an axle (a cylindrical joint), and the rocker pin+/// joining the coupler to the rod 4 mm up the axle from where the rod sits, its axis turned+/// `tilt` rad from the others about Y. Returns the closing joint's answer and the rocker pin.+fn rod_on_an_axle(tilt: f64) -> (Session, Value, Vec3) {+ let mut s = Session::default();+ let (o2, o4, p0) = (Vec3::ZERO, Vec3::new(45.0, 0.0, 0.0), Vec3::new(0.0, 20.0, 0.0));+ let [q0, _] = meet(p0, 50.0, o4, 40.0);+ for (name, c) in [("Ground", o2), ("Crank", p0), ("Coupler", (p0 + q0) * 0.5), ("Rod", q0)] {+ part(&mut s, name, [c.x, c.y, -5.0]);+ }+ let g = occ(&s, "Ground");+ run(&mut s, "occurrence.ground", json!({"occurrence": g, "grounded": true}));+ pin(&mut s, "revolute", "Crank", "Ground", "Crank", o2, Vec3::Z);+ pin(&mut s, "revolute", "Crank pin", "Crank", "Coupler", p0, Vec3::Z);+ pin(&mut s, "cylindrical", "Axle", "Ground", "Rod", o4, Vec3::Z);+ let z = [tilt.sin(), 0.0, tilt.cos()];+ let r = run(+ &mut s,+ "joint.create",+ json!({"type": "revolute", "a": {"occurrence": "Coupler", "point": [q0.x, q0.y, 0.0], "z": z},+ "b": {"occurrence": "Rod", "point": [q0.x, q0.y, 4.0], "z": z}, "flip": true, "name": "Rocker pin"}),+ );+ (s, r, q0)+}++/// Axes of holes picked on parts are seldom exactly parallel (#6211: a few hundredths of a+/// microradian apart on a lever). A closed loop through them leaves a gap of nanometres no value+/// takes out; it still closes, the rod sliding down its axle, and follows its crank. A tilt that+/// shows still locks it.+#[test]+fn a_loop_whose_axes_are_a_hair_out_of_parallel_still_moves() {+ let (mut s, r, q0) = rod_on_an_axle(1e-7);+ assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");+ let up = Vec3::new(0.0, 0.0, 4.0);+ let close = |a: Vec3, b: Vec3| a.dist(b) < 1e-4;+ assert!(close(place(&s, "Rod", q0 + up), q0), "{:?}", place(&s, "Rod", q0 + up));+ for part in ["Crank", "Coupler", "Rod"] {+ assert_eq!(dof_of(&mut s, part), 1, "{part}");+ }+ for v in [30.0, 60.0, -45.0] {+ let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": v}));+ assert!(r["stopped_by_loop"].is_null() && r["conflicts"].as_array().map(Vec::len) == Some(0), "{v}°: {r}");+ let q = rocker_pin(0.0, v, q0);+ assert!(close(place(&s, "Rod", q0 + up), q), "{v}°: {:?} wants {q:?}", place(&s, "Rod", q0 + up));+ }+ // Dragged by its crank pin (the viewport's drag of a part joints hold), the rod follows too.+ let (at, th) = (place(&s, "Crank", Vec3::new(0.0, 20.0, 0.0)), 110f64.to_radians());+ let r = run(&mut s, "joint.reach", json!({"occurrence": "Crank", "at": [at.x, at.y, at.z], "point": [20.0 * th.cos(), 20.0 * th.sin(), 0.0]}));+ assert!(r["reached"] == json!(true) && r["stopped_by_loop"].is_null(), "{r}");+ let q = rocker_pin(0.0, 20.0, q0);+ assert!(close(place(&s, "Rod", q0 + up), q), "reach: {:?} wants {q:?}", place(&s, "Rod", q0 + up));+ // Tilted a milliradian, the loop is locked: no longer planar, it can't turn.+ let (mut s, r, _) = rod_on_an_axle(1e-3);+ assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");+ assert_eq!(dof_of(&mut s, "Rod"), 0);+ let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": 30}));+ assert!(r["stopped_by_loop"].is_object() && r["values"][0]["value"].as_f64().is_some_and(|v| v.abs() < 0.1), "{r}");+}+/// A joint frame on an occurrence, by origin and axes.fn framed(o: &str, origin: [f64; 3], z: [f64; 3], x: [f64; 3]) -> Value {json!({"occurrence": o, "frame": {"origin": origin, "z": z, "x": x}})