Joints: joint.reach (inverse kinematics), and a jointed part follows a drag (#1540)

Thanks @SychPL.

crates/engine/src/cmd/joints.rs

@@ -36,6 +36,9 @@ pub static COMMANDS: &[CommandSpec] = &[
          reason: why the joint can move nothing (suppressed, its origins lost, or its chain joined to nothing grounded → unconnected: \
          the occurrence named)",
     ),
+    CommandSpec::new("joint.reach", "Reach Point", reach).params(
+        "occurrence: id|name; point: [x,y,z] (where its point goes, world); at?: [x,y,z] (its point, in the world where it is now; default: the centre of          its bodies' box); z?: [x,y,z] (the way its axis then points); axis?: [x,y,z] (that axis, in the world now; default: its own z); joints?: [ids          or names] (only these move toward the point; the other joints of a closed loop they are in follow); quiet?: bool; continuous?: bool (a frame          of a drag: the answer says continuous, and the app's command line doesn't echo it). Moves the values of the joints          on its chain from where they are to the nearest pose, within their limits, through motion links, keeping closed loops closed as joint.drive does          (contact sets are not checked) → reached; error_mm (and error_deg with z): how close it got; point: where its point is; values: [{joint, name,          values}] of the joints that moved, as joint.list gives them; moved; stopped_by_limit: [{joint, value, limit: min|max}]: the limits it rests on          short of the point; stopped_by_loop: {joint}: the closed loop that stopped it (its chain alone gets closer); reason: why nothing can move",
+    ),
     CommandSpec::new("joint.motion_link", "Motion Link", motion_link)
         .at("SOLID", "ASSEMBLE")
         .icon("motion_link")
@@ -562,6 +565,111 @@ fn drive(s: &mut Session, p: &Value) -> Result<Value> {
     Ok(out)
 }
 
+/// Joints `joint.reach` takes at most in `joints`.
+const MAX_REACH_JOINTS: usize = 1000;
+
+fn reach(s: &mut Session, p: &Value) -> Result<Value> {
+    let cmd = "joint.reach";
+    let occ = occurrence_id(s, p.get("occurrence"), cmd)?;
+    let given = |k: &str| p.get(k).and_then(vec3).filter(|v| v.is_finite() && v.len() < 1e9);
+    let to = given("point").ok_or_else(|| bad(cmd, "`point` must be [x, y, z]"))?;
+    let world = if occ == 0 { solidcraft_doc::IDENTITY } else { joints::occurrence_world(&s.doc, occ) };
+    let inv = solidcraft_doc::mat_inverse(&world).ok_or_else(|| bad(cmd, "occurrence transform"))?;
+    let at = match p.get("at") {
+        Some(_) => solidcraft_doc::apply_point(&inv, given("at").ok_or_else(|| bad(cmd, "`at` must be [x, y, z]"))?),
+        None => body_centre(s, occ),
+    };
+    let unit = |k: &str| -> Result<Option<Vec3>> {
+        match p.get(k) {
+            None => Ok(None),
+            Some(_) => given(k).and_then(Vec3::normalized).map(Some).ok_or_else(|| bad(cmd, format!("`{k}` must be a direction [x, y, z]"))),
+        }
+    };
+    let (z, axis) = (unit("z")?, unit("axis")?);
+    if axis.is_some() && z.is_none() {
+        return Err(bad(cmd, "`axis` goes with `z`, the way it points"));
+    }
+    let axis = z.map(|z| (axis.map(|a| solidcraft_doc::apply_vector(&inv, a)).and_then(Vec3::normalized).unwrap_or(Vec3::Z), z));
+    let only = match p.get("joints") {
+        None => None,
+        Some(Value::Array(a)) if a.len() <= MAX_REACH_JOINTS => Some(a.iter().map(|v| joint_id(s, Some(v), cmd)).collect::<Result<BTreeSet<u64>>>()?),
+        Some(_) => return Err(bad(cmd, format!("`joints` must be a list of joints (ids or names), at most {MAX_REACH_JOINTS}"))),
+    };
+    let before = super::motion::positions(s);
+    let st = s.model.state();
+    let r = joints::reach(&s.doc, &st, &joints::Reach { occurrence: occ, point: at, to, axis, joints: only });
+    let reached = match &r {
+        Ok(x) => {
+            // Only a reach that moves something touches the design (and so adds an undo step).
+            if !x.values.is_empty() {
+                let doc = s.doc_mut();
+                for (id, q) in &x.values {
+                    if let Some(j) = doc.assembly.joints.iter_mut().find(|j| j.id == *id) {
+                        j.values.clone_from(q);
+                    }
+                }
+            }
+            x.clone()
+        }
+        Err(_) => joints::Reached::default(),
+    };
+    // Loops closed again from there, and where it ended up.
+    let sol = resolve(s);
+    let t = joints::occurrence_world(&s.doc, occ);
+    let got = solidcraft_doc::apply_point(&t, at);
+    let mut out = sol_json(&sol, p);
+    out["occurrence"] = json!(occurrence_name(s, occ));
+    out["point"] = json!([got.x, got.y, got.z]);
+    out["error_mm"] = json!(got.dist(to));
+    let mut there = got.dist(to) <= joints::REACH_TOL;
+    if let Some((a, z)) = axis {
+        // From the sine too: the cosine alone loses small angles to rounding.
+        let v = solidcraft_doc::apply_vector(&t, a);
+        let deg = v.cross(z).len().atan2(v.dot(z)).to_degrees();
+        out["error_deg"] = json!(deg);
+        there &= deg <= joints::REACH_TOL;
+    }
+    out["reached"] = json!(there);
+    let name = |id: u64| s.doc.assembly.joints.iter().find(|j| j.id == id);
+    // As joint.list gives them: with what motion links set.
+    let effective = joints::joint_values(&s.doc.assembly);
+    out["values"] = reached
+        .values
+        .keys()
+        .filter_map(|id| name(*id))
+        .map(|j| json!({"joint": j.id, "name": j.name, "values": named_values(j, effective.get(&j.id).unwrap_or(&j.values))}))
+        .collect();
+    out["moved"] = json!(super::motion::moved(&before, &super::motion::positions(s)).next().is_some());
+    out["stopped_by_limit"] = if reached.limits.is_empty() {
+        Value::Null
+    } else {
+        reached
+            .limits
+            .iter()
+            .filter_map(|(id, i, upper)| {
+                let j = name(*id)?;
+                Some(json!({"joint": j.name, "value": j.kind.dofs().get(*i), "limit": if *upper { "max" } else { "min" }}))
+            })
+            .collect()
+    };
+    out["stopped_by_loop"] = json!(reached.stopped_by_loop.and_then(name).map(|j| json!({"joint": j.name})));
+    if let Err(why) = r {
+        out["reason"] = json!(why);
+    }
+    if bool_(p, "continuous") == Some(true) {
+        out["continuous"] = json!(true);
+    }
+    Ok(out)
+}
+
+/// The centre of the box around an occurrence's bodies, in its own coordinates (its origin when
+/// it has none).
+fn body_centre(s: &Session, occ: u64) -> Vec3 {
+    let comp = s.doc.occurrences.iter().find(|o| o.id == occ).map(|o| o.component).unwrap_or(0);
+    let st = s.model.state();
+    st.bodies.iter().filter(|b| s.doc.model_body_component(b) == comp).fold(Aabb3::EMPTY, |bb, b| bb.union(&b.mesh().bounds())).center()
+}
+
 fn motion_link(s: &mut Session, p: &Value) -> Result<Value> {
     let cmd = "joint.motion_link";
     let (a, b) = (joint_id(s, p.get("a"), cmd)?, joint_id(s, p.get("b"), cmd)?);
@@ -959,6 +1067,9 @@ fn overlapping_pairs(boxes: &[Aabb3], limit: usize) -> (Vec<(usize, usize)>, usi
     (pairs, skipped)
 }
 
+#[cfg(test)]
+#[path = "joints_reach_tests.rs"]
+mod reach_tests;
 #[cfg(test)]
 #[path = "joints_tests.rs"]
 mod tests;

crates/engine/src/cmd/joints_reach_tests.rs

@@ -0,0 +1,315 @@
+//! `joint.reach`: inverse kinematics against closed-form answers (planar arms), the nearest pose
+//! out of reach, limits, and a four-bar reached through its rocker.
+
+use serde_json::{Value, json};
+use solidcraft_geom::Vec3;
+
+use super::tests::{four_bar, meet, occ, part, pin, place, run};
+use crate::Session;
+
+/// A planar arm about +Z on a grounded base: Shoulder at the origin, Upper 60 long, Elbow, Fore
+/// 40 long, and with `hand` a Wrist and Hand 20 long, all along +X at 0°. Elbow starts at `elbow`°.
+fn arm(hand: bool, elbow: f64) -> Session {
+    let mut s = Session::default();
+    part(&mut s, "Base", [-1.0, -1.0, -1.0]);
+    part(&mut s, "Upper", [30.0, -1.0, -1.0]);
+    part(&mut s, "Fore", [80.0, -1.0, -1.0]);
+    let base = occ(&s, "Base");
+    run(&mut s, "occurrence.ground", json!({"occurrence": base, "grounded": true}));
+    pin(&mut s, "revolute", "Shoulder", "Base", "Upper", Vec3::ZERO, Vec3::Z);
+    pin(&mut s, "revolute", "Elbow", "Upper", "Fore", Vec3::new(60.0, 0.0, 0.0), Vec3::Z);
+    if hand {
+        part(&mut s, "Hand", [110.0, -1.0, -1.0]);
+        pin(&mut s, "revolute", "Wrist", "Fore", "Hand", Vec3::new(100.0, 0.0, 0.0), Vec3::Z);
+    }
+    run(&mut s, "joint.drive", json!({"joint": "Elbow", "value": elbow}));
+    s
+}
+
+/// A joint's first value as `joint.list` gives it (degrees for an angle).
+fn value(s: &mut Session, joint: &str) -> f64 {
+    let l = run(s, "joint.list", json!({"quiet": true}));
+    l["joints"]
+        .as_array()
+        .and_then(|js| js.iter().find(|j| j["name"] == joint))
+        .map_or(f64::NAN, |j| j["values"][0]["value"].as_f64().unwrap_or(f64::NAN))
+}
+
+/// `joint.reach` of the occurrence's point `local` (its own coordinates).
+fn reach(s: &mut Session, occurrence: &str, local: Vec3, mut p: Value) -> Value {
+    p["occurrence"] = json!(occurrence);
+    p["at"] = pt(place(s, occurrence, local));
+    run(s, "joint.reach", p)
+}
+
+fn pt(v: Vec3) -> Value {
+    json!([v.x, v.y, v.z])
+}
+
+fn got(r: &Value) -> Vec3 {
+    Vec3::new(r["point"][0].as_f64().unwrap_or(f64::NAN), r["point"][1].as_f64().unwrap_or(f64::NAN), r["point"][2].as_f64().unwrap_or(f64::NAN))
+}
+
+/// The two-link arm's closed-form answer for a tip at (x, y), elbow bent the positive way: the
+/// shoulder and elbow angles in degrees.
+fn two_link(x: f64, y: f64, l1: f64, l2: f64) -> (f64, f64) {
+    let c2 = (x * x + y * y - l1 * l1 - l2 * l2) / (2.0 * l1 * l2);
+    let t2 = c2.acos();
+    let t1 = y.atan2(x) - (l2 * t2.sin()).atan2(l1 + l2 * t2.cos());
+    (t1.to_degrees(), t2.to_degrees())
+}
+
+#[test]
+fn a_two_link_arm_reaches_its_closed_form_pose() {
+    let mut s = arm(false, 30.0);
+    let tip = Vec3::new(100.0, 0.0, 0.0);
+    let to = Vec3::new(50.0, 50.0, 0.0);
+    let r = reach(&mut s, "Fore", tip, json!({"point": pt(to)}));
+    assert_eq!(
+        (&r["reached"], &r["moved"], &r["stopped_by_limit"], &r["stopped_by_loop"]),
+        (&json!(true), &json!(true), &Value::Null, &Value::Null),
+        "{r}"
+    );
+    assert!(r["error_mm"].as_f64().is_some_and(|e| e < 1e-6), "{r}");
+    assert!(place(&s, "Fore", tip).dist(to) < 1e-6, "{:?}", place(&s, "Fore", tip));
+    let (t1, t2) = two_link(50.0, 50.0, 60.0, 40.0);
+    assert!((value(&mut s, "Shoulder") - t1).abs() < 1e-6 && (value(&mut s, "Elbow") - t2).abs() < 1e-6, "{r}");
+    assert_eq!(r["values"].as_array().map(Vec::len), Some(2), "{r}");
+    // Already there, a reach moves nothing and adds no undo step.
+    let steps = s.undo.len();
+    let r = reach(&mut s, "Fore", tip, json!({"point": pt(to)}));
+    assert_eq!((&r["reached"], &r["moved"], s.undo.len()), (&json!(true), &json!(false), steps), "{r}");
+    // One undo step puts it back.
+    run(&mut s, "edit.undo", json!({}));
+    assert!((value(&mut s, "Elbow") - 30.0).abs() < 1e-9 && value(&mut s, "Shoulder").abs() < 1e-9);
+}
+
+/// A three-link arm puts its tip on the point and its hand along the axis given: the wrist is
+/// then the two-link answer, and the hand's turn makes up the rest.
+#[test]
+fn a_three_link_arm_reaches_a_point_and_an_axis() {
+    let mut s = arm(true, 30.0);
+    let tip = Vec3::new(120.0, 0.0, 0.0);
+    // The hand's length, along its own X: at 30° in the world now (the elbow's turn).
+    let along = place(&s, "Hand", tip) - place(&s, "Hand", Vec3::new(100.0, 0.0, 0.0));
+    let r = reach(&mut s, "Hand", tip, json!({"point": [50, 50, 0], "z": [0, 1, 0], "axis": pt(along)}));
+    assert_eq!(r["reached"], true, "{r}");
+    assert!(r["error_deg"].as_f64().is_some_and(|e| e < 1e-6), "{r}");
+    let (t1, t2) = two_link(50.0, 30.0, 60.0, 40.0);
+    let t3 = 90.0 - t1 - t2;
+    for (joint, want) in [("Shoulder", t1), ("Elbow", t2), ("Wrist", t3)] {
+        assert!((value(&mut s, joint) - want).abs() < 1e-6, "{joint}: {} wants {want}: {r}", value(&mut s, joint));
+    }
+}
+
+#[test]
+fn out_of_reach_the_arm_points_at_the_target() {
+    let tip = Vec3::new(100.0, 0.0, 0.0);
+    // 150 mm away: the arm stretches toward it, 50 mm short.
+    let mut s = arm(false, 30.0);
+    let r = reach(&mut s, "Fore", tip, json!({"point": [-90, 120, 0]}));
+    assert_eq!(
+        (&r["reached"], &r["moved"], &r["stopped_by_limit"], &r["stopped_by_loop"]),
+        (&json!(false), &json!(true), &Value::Null, &Value::Null),
+        "{r}"
+    );
+    assert!(r["error_mm"].as_f64().is_some_and(|e| (e - 50.0).abs() < 1e-3), "{r}");
+    assert!(got(&r).dist(Vec3::new(-60.0, 80.0, 0.0)) < 1e-2, "{r}");
+    assert!(place(&s, "Fore", tip).dist(got(&r)) < 1e-9, "{r}");
+    // Out of its plane: the nearest is the point under the target.
+    let mut s = arm(false, 30.0);
+    let r = reach(&mut s, "Fore", tip, json!({"point": [50, 50, 30]}));
+    assert_eq!(r["reached"], false, "{r}");
+    assert!(r["error_mm"].as_f64().is_some_and(|e| (e - 30.0).abs() < 1e-6), "{r}");
+    assert!(got(&r).dist(Vec3::new(50.0, 50.0, 0.0)) < 1e-6, "{r}");
+}
+
+#[test]
+fn a_limited_elbow_stops_the_reach_and_says_so() {
+    let tip = Vec3::new(100.0, 0.0, 0.0);
+    let mut s = arm(false, 10.0);
+    run(&mut s, "joint.limits", json!({"joint": "Elbow", "min": -30, "max": 30}));
+    let r = reach(&mut s, "Fore", tip, json!({"point": [50, 50, 0]}));
+    // The target needs the elbow at 92°: it bends to its limit, and the arm points at the target.
+    assert_eq!(r["reached"], false, "{r}");
+    assert_eq!(r["stopped_by_limit"], json!([{"joint": "Elbow", "value": "angle", "limit": "max"}]), "{r}");
+    assert!((value(&mut s, "Elbow") - 30.0).abs() < 1e-9, "{r}");
+    let span = (60f64.powi(2) + 40f64.powi(2) + 2.0 * 60.0 * 40.0 * 30f64.to_radians().cos()).sqrt();
+    let d = 50.0 * 2f64.sqrt();
+    assert!(r["error_mm"].as_f64().is_some_and(|e| (e - (span - d)).abs() < 1e-4), "{r}");
+    // Only the shoulder moves when only it is given: the tip swings on its circle.
+    let mut s = arm(false, 30.0);
+    let r = reach(&mut s, "Fore", tip, json!({"point": [0, 100, 0], "joints": ["Shoulder"]}));
+    let span = (60f64.powi(2) + 40f64.powi(2) + 2.0 * 60.0 * 40.0 * 30f64.to_radians().cos()).sqrt();
+    assert!(r["error_mm"].as_f64().is_some_and(|e| (e - (100.0 - span)).abs() < 1e-4), "{r}");
+    assert!((value(&mut s, "Elbow") - 30.0).abs() < 1e-12, "{r}");
+}
+
+/// A linked elbow (elbow = −shoulder, so the forearm keeps pointing along +X) follows the
+/// shoulder: the tip moves on a circle about (40, 0), and the reach turns the shoulder only.
+#[test]
+fn a_linked_joint_follows_the_reach() {
+    let tip = Vec3::new(100.0, 0.0, 0.0);
+    let mut s = arm(false, 0.0);
+    run(&mut s, "joint.motion_link", json!({"a": "Shoulder", "b": "Elbow", "ratio": -1}));
+    let th = 40f64.to_radians();
+    let to = json!([40.0 + 60.0 * th.cos(), 60.0 * th.sin(), 0.0]);
+    let r = reach(&mut s, "Fore", tip, json!({"point": to}));
+    assert_eq!(r["reached"], true, "{r}");
+    assert!((value(&mut s, "Shoulder") - 40.0).abs() < 1e-6 && (value(&mut s, "Elbow") + 40.0).abs() < 1e-6, "{r}");
+    // Given only the linked elbow, the reach turns it through its link's source.
+    run(&mut s, "edit.undo", json!({}));
+    let r = reach(&mut s, "Fore", tip, json!({"point": to, "joints": ["Elbow"]}));
+    assert_eq!(r["reached"], true, "{r}");
+    assert!((value(&mut s, "Shoulder") - 40.0).abs() < 1e-6, "{r}");
+    // The elbow held at its own limit by the link stops the reach, and is named.
+    run(&mut s, "edit.undo", json!({}));
+    run(&mut s, "joint.limits", json!({"joint": "Elbow", "min": -30, "max": 30}));
+    let r = reach(&mut s, "Fore", tip, json!({"point": to}));
+    assert_eq!(r["reached"], false, "{r}");
+    assert!(r["stopped_by_limit"].as_array().is_some_and(|l| l.contains(&json!({"joint": "Elbow", "value": "angle", "limit": "min"}))), "{r}");
+}
+
+/// A target straight across the pivot from the point (where the misses don't change to first
+/// order) is still reached: the arm turns half round, the way its limits leave open.
+#[test]
+fn a_target_across_the_pivot_is_reached() {
+    let tip = Vec3::new(100.0, 0.0, 0.0);
+    for (limits, want) in [(None, None), (Some([-180, 0]), Some(-180.0)), (Some([0, 180]), Some(180.0))] {
+        let mut s = arm(false, 0.0);
+        if let Some([min, max]) = limits {
+            run(&mut s, "joint.limits", json!({"joint": "Shoulder", "min": min, "max": max}));
+        }
+        let r = reach(&mut s, "Fore", tip, json!({"point": [-100, 0, 0], "joints": ["Shoulder"]}));
+        assert_eq!(r["reached"], true, "{limits:?}: {r}");
+        let got = value(&mut s, "Shoulder");
+        assert!((got.abs() - 180.0).abs() < 1e-6 && want.is_none_or(|w| (got - w).abs() < 1e-6), "{limits:?}: {got}");
+    }
+}
+
+/// A loop that only closes again after a full turn of a value geared up from the arm's (a joint
+/// between two grounded parts, linked 36:1 to the shoulder) holds the arm: a step of a tenth of a
+/// turn would close it at both ends, but not on the way.
+#[test]
+fn a_loop_geared_up_from_the_chain_holds_it() {
+    let tip = Vec3::new(100.0, 0.0, 0.0);
+    let mut s = arm(false, 0.0);
+    part(&mut s, "Lock", [-1.0, 5.0, -1.0]);
+    let lock = occ(&s, "Lock");
+    run(&mut s, "occurrence.ground", json!({"occurrence": lock, "grounded": true}));
+    pin(&mut s, "revolute", "Lock", "Base", "Lock", Vec3::ZERO, Vec3::Z);
+    run(&mut s, "joint.motion_link", json!({"a": "Shoulder", "b": "Lock", "ratio": 36}));
+    let r = reach(&mut s, "Fore", tip, json!({"point": [0, 1e6, 0], "joints": ["Shoulder"]}));
+    assert_eq!((&r["reached"], &r["stopped_by_loop"]), (&json!(false), &json!({"joint": "Lock"})), "{r}");
+    assert!(value(&mut s, "Shoulder").abs() < 1e-6, "{r}");
+    // Geared up so far that even the small kick off a target straight across the pivot would turn
+    // the loop a whole turn: it holds too.
+    run(&mut s, "joint.delete", json!({"link": 0}));
+    run(&mut s, "joint.motion_link", json!({"a": "Shoulder", "b": "Lock", "ratio": 36000}));
+    let r = reach(&mut s, "Fore", tip, json!({"point": [-100, 0, 0], "joints": ["Shoulder"]}));
+    assert_eq!(r["reached"], false, "{r}");
+    assert!(value(&mut s, "Shoulder").abs() < 1e-6, "{r}");
+}
+
+/// A planar joint far from the point turns it and slides the long way round, and the answer
+/// gives the values as joint.list does.
+#[test]
+fn a_planar_joint_far_from_the_point_turns_and_slides() {
+    let mut s = Session::default();
+    part(&mut s, "Base", [-1.0, -1.0, -1.0]);
+    part(&mut s, "Puck", [99.0, -1.0, 2.0]);
+    let base = occ(&s, "Base");
+    run(&mut s, "occurrence.ground", json!({"occurrence": base, "grounded": true}));
+    pin(&mut s, "planar", "Table", "Base", "Puck", Vec3::ZERO, Vec3::Z);
+    let r = reach(&mut s, "Puck", Vec3::new(100.0, 0.0, 0.0), json!({"point": [100, 0, 0], "z": [0, 1, 0], "axis": [1, 0, 0]}));
+    assert_eq!(r["reached"], true, "{r}");
+    let l = run(&mut s, "joint.list", json!({"quiet": true}));
+    let listed = l["joints"].as_array().and_then(|js| js.iter().find(|j| j["name"] == "Table")).map(|j| j["values"].clone());
+    assert_eq!(Some(r["values"][0]["values"].clone()), listed, "{r}");
+    assert!((r["values"][0]["values"][2]["value"].as_f64().unwrap_or(f64::NAN) - 90.0).abs() < 1e-6, "{r}");
+}
+
+/// A slider on its own (a chain with no size) still travels the whole way.
+#[test]
+fn a_lone_slider_travels_all_the_way() {
+    let mut s = Session::default();
+    part(&mut s, "Base", [-1.0, -1.0, -1.0]);
+    part(&mut s, "Ram", [-1.0, -1.0, 2.0]);
+    let base = occ(&s, "Base");
+    run(&mut s, "occurrence.ground", json!({"occurrence": base, "grounded": true}));
+    pin(&mut s, "slider", "Rail", "Base", "Ram", Vec3::ZERO, Vec3::Z);
+    let r = reach(&mut s, "Ram", Vec3::ZERO, json!({"point": [0, 0, 100]}));
+    assert_eq!(r["reached"], true, "{r}");
+    assert!((value(&mut s, "Rail") - 100.0).abs() < 1e-6, "{r}");
+    // However far.
+    let r = reach(&mut s, "Ram", Vec3::ZERO, json!({"point": [0, 0, 3e7]}));
+    assert_eq!(r["reached"], true, "{r}");
+    // A limit on it is named whatever the chain's size.
+    run(&mut s, "edit.undo", json!({}));
+    run(&mut s, "joint.limits", json!({"joint": "Rail", "min": -50, "max": 100}));
+    let r = reach(&mut s, "Ram", Vec3::ZERO, json!({"point": [0, 0, 1e6]}));
+    assert_eq!(r["stopped_by_limit"], json!([{"joint": "Rail", "value": "distance", "limit": "max"}]), "{r}");
+    // And a reach of a thousandth of a millimetre still moves it.
+    let r = reach(&mut s, "Ram", Vec3::ZERO, json!({"point": [0, 0, 99.999]}));
+    assert_eq!((&r["reached"], &r["moved"]), (&json!(true), &json!(true)), "{r}");
+}
+
+/// The four-bar's rocker reaches a point on its swing, the crank and coupler following with the
+/// loop closed; past its toggle position it stops there, and the loop is named.
+#[test]
+fn a_four_bar_reaches_with_its_rocker() {
+    let (mut s, _, q0) = four_bar();
+    let o4 = Vec3::new(45.0, 0.0, 0.0);
+    let start = q0.y.atan2(q0.x - 45.0);
+    let swing = |deg: f64| o4 + Vec3::new((start + deg.to_radians()).cos(), (start + deg.to_radians()).sin(), 0.0) * 40.0;
+    let r = run(&mut s, "joint.reach", json!({"occurrence": "Rocker", "at": pt(q0), "point": pt(swing(-10.0))}));
+    assert_eq!((&r["reached"], &r["stopped_by_loop"]), (&json!(true), &Value::Null), "{r}");
+    assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
+    assert!(place(&s, "Rocker", q0).dist(swing(-10.0)) < 1e-6 && place(&s, "Coupler", q0).dist(swing(-10.0)) < 1e-6, "{r}");
+    // The crank turned to where the loop closes with the rocker there (its pin on the coupler's circle).
+    let th = (90.0 + value(&mut s, "Crank")).to_radians();
+    let crank_pin = Vec3::new(20.0 * th.cos(), 20.0 * th.sin(), 0.0);
+    assert!((crank_pin.dist(swing(-10.0)) - 50.0).abs() < 1e-6 && place(&s, "Coupler", Vec3::new(0.0, 20.0, 0.0)).dist(crank_pin) < 1e-6, "{r}");
+    // Past its toggle position (|Q| = 30) the loop stops it there.
+    let (mut s, _, q0) = four_bar();
+    let r = run(&mut s, "joint.reach", json!({"occurrence": "Rocker", "at": pt(q0), "point": pt(swing(100.0))}));
+    assert_eq!((&r["reached"], &r["stopped_by_loop"]), (&json!(false), &json!({"joint": "Rocker pin"})), "{r}");
+    // Its joints have no limits, so none is named.
+    assert_eq!(r["stopped_by_limit"], Value::Null, "{r}");
+    assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
+    let limit = ((900.0f64 - 3625.0) / 3600.0).acos().to_degrees() - start.to_degrees();
+    let at = value(&mut s, "Rocker");
+    assert!(at > limit - 0.5 && at <= limit + 1e-6, "stopped at {at}°, the toggle is at {limit}°: {r}");
+    let [m1, m2] = meet(o4, 40.0, Vec3::ZERO, 30.0);
+    assert!(place(&s, "Rocker", q0).dist(m1).min(place(&s, "Rocker", q0).dist(m2)) < 0.5, "{r}");
+}
+
+#[test]
+fn reach_refuses_bad_params_and_says_why_nothing_moves() {
+    let mut s = arm(false, 30.0);
+    for bad in [
+        json!({"occurrence": "Fore"}),
+        json!({"occurrence": "Nope", "point": [0, 0, 0]}),
+        json!({"occurrence": "Fore", "point": [1e300, 0, 0]}),
+        json!({"occurrence": "Fore", "point": [0, 0, "x"]}),
+        json!({"occurrence": "Fore", "point": [0, 0, 0], "z": [0, 0, 0]}),
+        json!({"occurrence": "Fore", "point": [0, 0, 0], "axis": [1, 0, 0]}),
+        json!({"occurrence": "Fore", "point": [0, 0, 0], "joints": "Elbow"}),
+        json!({"occurrence": "Fore", "point": [0, 0, 0], "joints": ["Nope"]}),
+        json!({"occurrence": "Fore", "point": [0, 0, 0], "at": [f64::MAX, 0, 0]}),
+    ] {
+        assert!(s.execute("joint.reach", &bad).is_err(), "{bad}");
+    }
+    // A grounded part, a part no joint places, and joints that don't move it: nothing moves, and
+    // the answer says why.
+    part(&mut s, "Loose", [200.0, 0.0, 0.0]);
+    for (p, why) in [
+        (json!({"occurrence": "Base", "point": [5, 5, 5]}), "fixed in place"),
+        (json!({"occurrence": "Loose", "point": [5, 5, 5]}), "not connected"),
+        (json!({"occurrence": "Upper", "point": [5, 5, 5], "joints": ["Elbow"]}), "none of the joints given"),
+    ] {
+        let r = run(&mut s, "joint.reach", p.clone());
+        assert_eq!((&r["reached"], &r["moved"]), (&json!(false), &json!(false)), "{p}: {r}");
+        assert!(r["reason"].as_str().is_some_and(|w| w.contains(why)), "{p}: {r}");
+    }
+}

crates/engine/src/cmd/joints_tests.rs

@@ -5,19 +5,19 @@ use solidcraft_geom::Vec3;
 
 use crate::Session;
 
-fn run(s: &mut Session, id: &str, p: Value) -> Value {
+pub(super) fn run(s: &mut Session, id: &str, p: Value) -> Value {
     match s.execute(id, &p) {
         Ok(v) => v,
         Err(e) => panic!("{id} {p}: {e}"),
     }
 }
 
-fn occ(s: &Session, component: &str) -> u64 {
+pub(super) fn occ(s: &Session, component: &str) -> u64 {
     let c = s.doc.find_component(component).unwrap_or(0);
     s.doc.occurrence_of(c).map(|o| o.id).unwrap_or(0)
 }
 
-fn place(s: &Session, component: &str, p: Vec3) -> Vec3 {
+pub(super) fn place(s: &Session, component: &str, p: Vec3) -> Vec3 {
     solidcraft_doc::apply_point(&solidcraft_doc::joints::occurrence_world(&s.doc, occ(s, component)), p)
 }
 
@@ -313,21 +313,21 @@ fn interference_box_sweep_matches_every_pair_check() {
 }
 
 /// A small block in a new component: a part of a linkage, where only its joints matter.
-fn part(s: &mut Session, name: &str, corner: [f64; 3]) {
+pub(super) fn part(s: &mut Session, name: &str, corner: [f64; 3]) {
     run(s, "component.activate", json!({"component": "root"}));
     run(s, "component.create", json!({"name": name}));
     run(s, "solid.box", json!({"length": 2, "width": 2, "height": 2, "corner": corner}));
 }
 
 /// A joint between two parts as they are, at a world point with its z along `z`.
-fn pin(s: &mut Session, kind: &str, name: &str, a: &str, b: &str, p: Vec3, z: Vec3) {
+pub(super) fn pin(s: &mut Session, kind: &str, name: &str, a: &str, b: &str, p: Vec3, z: Vec3) {
     let at = |o: &str| json!({"occurrence": o, "point": [p.x, p.y, p.z], "z": [z.x, z.y, z.z]});
     let r = run(s, "joint.create", json!({"type": kind, "a": at(a), "b": at(b), "flip": true, "name": name}));
     assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
 }
 
 /// Where circles about `p` (radius `r`) and `q` (radius `s`) in the XY plane meet.
-fn meet(p: Vec3, r: f64, q: Vec3, s: f64) -> [Vec3; 2] {
+pub(super) fn meet(p: Vec3, r: f64, q: Vec3, s: f64) -> [Vec3; 2] {
     let d = q - p;
     let l = d.len();
     let a = (r * r - s * s + l * l) / (2.0 * l);
@@ -339,7 +339,7 @@ fn meet(p: Vec3, r: f64, q: Vec3, s: f64) -> [Vec3; 2] {
 /// A crank-rocker four-bar about +Z: ground pivots O2 = (0, 0) and O4 = (45, 0), crank 20,
 /// coupler 50, rocker 40, assembled with the crank at 90°. Returns the crank pin P0 and the
 /// rocker pin Q0 (the upper, open assembly).
-fn four_bar() -> (Session, Vec3, Vec3) {
+pub(super) fn four_bar() -> (Session, Vec3, 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);