diff --git a/crates/doc/src/joints.rs b/crates/doc/src/joints.rs index 257c7a49c..ff7f93e06 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>, x: &mut Vec, bounds: &[(f64, f64)], tol: f64, loose: f64) -> Option { 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.rs index 2274bfc10..7e87749e3 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::(); // 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>, x: &mut Vec| { - 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.rs index 095f5856f..c9d22a931 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}})