Motion links carry a value through a chain stored in any order (#1203)

Motion links carry a value through a chain stored in any order.

crates/doc/src/joints.rs

@@ -349,22 +349,40 @@ fn mismatch(a: &Mat, b: &Mat) -> (f64, f64) {
     (t, sin2.atan2(c(0, 0) + c(1, 1) + c(2, 2) - 1.0))
 }
 
-/// Passes over the motion links: a chain of this many links carries a value through in any order.
-const MAX_LINK_HOPS: usize = 4;
+/// Passes over the motion links at most: a chain of up to this many links carries its value
+/// through, whatever order its links are stored in.
+const MAX_LINK_PASSES: usize = 64;
 
-/// Motion links in order (a few passes for chains), then the linked values' limits.
+/// Motion links, pass after pass until nothing changes (a chain stored from its end takes a pass
+/// per link), each linked value within its joint's limits as it is set. The result is the same
+/// whatever the values were before and however the links are stored, so applying them again
+/// changes nothing (links that form a cycle stop after `MAX_LINK_PASSES`).
 fn apply_links(asm: &Assembly, vals: &mut Values) {
-    for _ in 0..MAX_LINK_HOPS {
+    let limits: BTreeMap<(u64, usize), (f64, f64)> = asm
+        .links
+        .iter()
+        .filter_map(|l| {
+            let (lo, hi) = asm.joints.iter().find(|j| j.id == l.b)?.limits.get(l.ib).copied().flatten()?;
+            Some(((l.b, l.ib), (lo.min(hi), hi.max(lo))))
+        })
+        .collect();
+    // The linked values, to see whether a pass changed any (several links writing one value
+    // change it within a pass, but settle from one pass to the next).
+    let linked = |vals: &Values| -> Vec<Option<f64>> { asm.links.iter().map(|l| vals.get(&l.b).and_then(|q| q.get(l.ib)).copied()).collect() };
+    for _ in 0..MAX_LINK_PASSES {
+        let before = linked(vals);
         for l in &asm.links {
             let Some(src) = vals.get(&l.a).and_then(|q| q.get(l.ia)).copied() else { continue };
+            let mut next = l.ratio * src + l.offset;
+            if let Some((lo, hi)) = limits.get(&(l.b, l.ib)) {
+                next = next.max(*lo).min(*hi);
+            }
             if let Some(v) = vals.get_mut(&l.b).and_then(|q| q.get_mut(l.ib)) {
-                *v = l.ratio * src + l.offset;
+                *v = next;
             }
         }
-    }
-    for l in &asm.links {
-        if let (Some(j), Some(q)) = (asm.joints.iter().find(|j| j.id == l.b), vals.get_mut(&l.b)) {
-            clamp(j, q);
+        if linked(vals) == before {
+            break;
         }
     }
 }
@@ -481,7 +499,7 @@ fn link_chains(asm: &Assembly, tree: &Tree, around: &BTreeSet<u64>, linked: &BTr
     let mut frontier: BTreeSet<(u64, usize)> =
         around.iter().filter_map(|id| tree.joints.get(id)).flat_map(|j| (0..j.kind.dofs().len()).map(|i| (j.id, i))).collect();
     let mut seen = frontier.clone();
-    for _ in 0..MAX_LINK_HOPS {
+    for _ in 0..MAX_LINK_PASSES {
         let next: BTreeSet<(u64, usize)> =
             asm.links.iter().filter(|l| frontier.contains(&(l.b, l.ib))).map(|l| (l.a, l.ia)).filter(|v| !seen.contains(v)).collect();
         if next.is_empty() {
@@ -990,6 +1008,48 @@ mod tests {
         t < 1e-9 && r < 1e-9
     }
 
+    /// Revolute joints 1…n (joint 1 at `start`, the others at 0) linked 1 → 2 → … → n with
+    /// ratio 2, the links stored from the end of the chain; joint 3 limited to ±50.
+    fn chain(n: u64, start: f64) -> Assembly {
+        let origin = JointOrigin { occurrence: 0, snap: Snap::Point { point: Vec3::ZERO, z: None } };
+        let joints = (1..=n)
+            .map(|id| Joint {
+                id,
+                name: format!("J{id}"),
+                kind: JointKind::Revolute,
+                a: origin.clone(),
+                b: origin.clone(),
+                values: vec![if id == 1 { start } else { 0.0 }],
+                limits: vec![(id == 3).then_some((-50.0, 50.0))],
+                flip: false,
+                offset: 0.0,
+                angle: 0.0,
+                suppressed: false,
+            })
+            .collect();
+        let links = (1..n).rev().map(|a| MotionLink { a, ia: 0, b: a + 1, ib: 0, ratio: 2.0, offset: 0.0 }).collect();
+        Assembly { joints, links, ..Assembly::default() }
+    }
+
+    /// Links carry a value down a chain stored from its end, each linked value within its limits
+    /// as it is set, and applying them again changes nothing.
+    #[test]
+    fn links_settle_whatever_order_they_are_stored_in() {
+        let asm = chain(8, 10.0);
+        let vals = joint_values(&asm);
+        let at = |id: u64| vals.get(&id).and_then(|q| q.first()).copied().unwrap_or(f64::NAN);
+        // 10 → 20 → 40 (within joint 3's limit) → 80 → … → 1280.
+        assert_eq!((at(1), at(2), at(3), at(4), at(8)), (10.0, 20.0, 40.0, 80.0, 1280.0));
+        let asm = chain(8, 40.0);
+        let vals = joint_values(&asm);
+        let at = |id: u64| vals.get(&id).and_then(|q| q.first()).copied().unwrap_or(f64::NAN);
+        // Joint 3 would be 160: held at 50, and what follows it follows the 50.
+        assert_eq!((at(3), at(4), at(8)), (50.0, 100.0, 1600.0));
+        let mut again = vals.clone();
+        apply_links(&asm, &mut again);
+        assert_eq!(again, vals);
+    }
+
     #[test]
     fn motions_compose() {
         assert!(close(&motion(JointKind::Revolute, &[std::f64::consts::FRAC_PI_2]), &rot_z(std::f64::consts::FRAC_PI_2)));

crates/engine/src/cmd/joints_tests.rs

@@ -842,3 +842,45 @@ fn older_designs_show_every_joint_and_new_joints_start_shown() {
     assert!(s.joint_visible(again), "id {again} reused from {glue}");
     let _ = std::fs::remove_dir_all(&dir);
 }
+
+/// Dials R0…R4 on revolute joints to ground, linked R0 → R1 → … → `last` (ratio 1), the links
+/// stored from the end of the chain to its start.
+fn reverse_link_chain(s: &mut Session, last: &str) {
+    for k in 0..5 {
+        let x = 100.0 + 10.0 * f64::from(k);
+        part(s, &format!("R{k}"), [x, 0.0, -5.0]);
+        pin(s, "revolute", &format!("R{k}"), "Ground", &format!("R{k}"), Vec3::new(x, 0.0, 0.0), Vec3::Z);
+    }
+    for k in (0..5).rev() {
+        let b = if k == 4 { last.to_string() } else { format!("R{}", k + 1) };
+        run(s, "joint.motion_link", json!({"a": format!("R{k}"), "b": b, "ratio": 1}));
+    }
+}
+
+/// A motion link chain carries a value through all its links, however they are stored: five
+/// links stored from the end still turn the last joint with the first.
+#[test]
+fn a_link_chain_stored_backwards_carries_its_value_through() {
+    let (mut s, _, _) = four_bar();
+    part(&mut s, "Last", [200.0, 0.0, -5.0]);
+    pin(&mut s, "revolute", "Last", "Ground", "Last", Vec3::new(200.0, 0.0, 0.0), Vec3::Z);
+    reverse_link_chain(&mut s, "Last");
+    run(&mut s, "joint.drive", json!({"joint": "R0", "value": 10}));
+    let l = run(&mut s, "joint.list", json!({}));
+    let last = l["joints"].as_array().into_iter().flatten().find(|j| j["name"] == "Last").map(|j| j["values"][0]["value"].clone());
+    assert!((last.and_then(|v| v.as_f64()).unwrap_or(f64::NAN) - 10.0).abs() < 1e-9, "{l}");
+}
+
+/// Driving the start of such a chain into a four-bar's crank turns the mechanism, and never
+/// reports its loop as opened.
+#[test]
+fn a_link_chain_stored_backwards_drives_a_loop() {
+    let (mut s, _, q0) = four_bar();
+    reverse_link_chain(&mut s, "Crank");
+    for v in [1.0, 30.0] {
+        let r = run(&mut s, "joint.drive", json!({"joint": "R0", "value": v}));
+        assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{v}: {r}");
+        assert!(r["stopped_by_loop"].is_null(), "{v}: {r}");
+        assert!(near(place(&s, "Rocker", q0), rocker_pin(0.0, v, q0)), "{v}: {:?}", place(&s, "Rocker", q0));
+    }
+}

docs/book/src/guide/assemblies.md

@@ -22,7 +22,9 @@ records are reported. Unsupported destructive links refuse the entire copy. See
 circles and vertices: rigid, revolute, slider, cylindrical, pin-slot, planar and ball, with
 offsets, flip and limits. **As-built Joint** keeps the current position, **Rigid Group** locks
 components together, **Drive Joints** moves a joint through its values, and **Motion Link**
-couples two joints. Contact sets stop motion where bodies would pass through each other,
+couples two joints. Links can be chained (one joint drives a second, which drives a
+third), and a value is carried through a chain of up to 64 links in whatever order they were
+made. Contact sets stop motion where bodies would pass through each other,
 **Motion Study** keys joint values along a timeline, and **Interference** shows overlapping
 volumes in red.