diff --git a/crates/doc/src/joints.rs b/crates/doc/src/joints.rs index 62ea962f..c2e78c94 100644 --- a/crates/doc/src/joints.rs +++ b/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> { 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, 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))); diff --git a/crates/engine/src/cmd/joints_tests.rs b/crates/engine/src/cmd/joints_tests.rs index fd8ad9ee..e74cdc29 100644 --- a/crates/engine/src/cmd/joints_tests.rs +++ b/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)); + } +} diff --git a/docs/book/src/guide/assemblies.md b/docs/book/src/guide/assemblies.md index c75dfffd..72f0b990 100644 --- a/docs/book/src/guide/assemblies.md +++ b/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.