Close kinematic loops: driven joints move four-bars and sliders (#1141)
Driven joints now solve closed kinematic loops, so a four-bar or slider-crank moves as a whole when one joint is driven.
This commit is contained in:
Mateusz Sych authored and GitHub committed
1 parent
2e6951135a
commit
94faf9ce1b
7 files changed
+1025
-64
No files matched your search
+520
-46
@@ -7,14 +7,17 @@
|
||||
//! `T_B · F_b = T_A · F_a · M(q) · Flip`, so `T_B = T_A · F_a · M(q) · Flip · F_b⁻¹`, where `T`
|
||||
//! are occurrence world transforms, `F` the joint frames in each component's own frame, and `q`
|
||||
//! the joint's values (angles in radians, distances in mm). The solver walks the graph from
|
||||
//! grounded occurrences (and the root, which never moves); joints that close a loop are checked
|
||||
//! and reported when they disagree.
|
||||
//! grounded occurrences (and the root, which never moves). Joints that close a loop (a four-bar
|
||||
//! linkage, a slider-crank) are met by moving the values of the other joints around the loop;
|
||||
//! those that still disagree are reported.
|
||||
|
||||
use std::collections::{BTreeMap, BTreeSet, VecDeque};
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
use solidcraft_geom::Vec3;
|
||||
|
||||
use solidcraft_sketch::linalg;
|
||||
|
||||
use crate::{DocError, Document, IDENTITY, Mat, ModelState, Result, apply_point, apply_vector, mat_inverse, mat_mul};
|
||||
|
||||
/// Joints the solver (and the URDF export) reads at most.
|
||||
@@ -320,6 +323,9 @@ pub struct Solution {
|
||||
/// New world transforms of occurrences the joints place.
|
||||
#[serde(skip)]
|
||||
pub world: BTreeMap<u64, Mat>,
|
||||
/// New values of the joints that moved to close loops (joint id → values).
|
||||
#[serde(skip)]
|
||||
pub values: BTreeMap<u64, Vec<f64>>,
|
||||
/// Joints that disagree with the placement found through others: (joint id, name, why).
|
||||
pub conflicts: Vec<(u64, String, String)>,
|
||||
/// Joints that could not be applied (bad origin…): (joint id, name, why).
|
||||
@@ -330,21 +336,42 @@ pub struct Solution {
|
||||
pub fixed: Vec<u64>,
|
||||
}
|
||||
|
||||
/// Joint values by joint id.
|
||||
type Values = BTreeMap<u64, Vec<f64>>;
|
||||
|
||||
/// How far two placements differ: (translation mm, rotation rad).
|
||||
fn mismatch(a: &Mat, b: &Mat) -> (f64, f64) {
|
||||
let t = Vec3::new(a[3][0] - b[3][0], a[3][1] - b[3][1], a[3][2] - b[3][2]).len();
|
||||
let mut tr = 0.0;
|
||||
for i in 0..3 {
|
||||
for k in 0..3 {
|
||||
tr += a[i][k] * b[i][k];
|
||||
// The turn between them from both its cosine and its sine: the cosine alone loses small
|
||||
// angles to rounding.
|
||||
let c = |i: usize, k: usize| (0..3).map(|n| a[i][n] * b[k][n]).sum::<f64>();
|
||||
let sin2 = Vec3::new(c(1, 2) - c(2, 1), c(2, 0) - c(0, 2), c(0, 1) - c(1, 0)).len();
|
||||
(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;
|
||||
|
||||
/// Motion links in order (a few passes for chains), then the linked values' limits.
|
||||
fn apply_links(asm: &Assembly, vals: &mut Values) {
|
||||
for _ in 0..MAX_LINK_HOPS {
|
||||
for l in &asm.links {
|
||||
let Some(src) = vals.get(&l.a).and_then(|q| q.get(l.ia)).copied() else { continue };
|
||||
if let Some(v) = vals.get_mut(&l.b).and_then(|q| q.get_mut(l.ib)) {
|
||||
*v = l.ratio * src + l.offset;
|
||||
}
|
||||
}
|
||||
}
|
||||
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);
|
||||
}
|
||||
}
|
||||
(t, ((tr - 1.0) / 2.0).clamp(-1.0, 1.0).acos())
|
||||
}
|
||||
|
||||
/// Effective values of every joint after motion links and limits.
|
||||
pub fn joint_values(asm: &Assembly) -> BTreeMap<u64, Vec<f64>> {
|
||||
let mut vals: BTreeMap<u64, Vec<f64>> = asm
|
||||
let mut vals: Values = asm
|
||||
.joints
|
||||
.iter()
|
||||
.map(|j| {
|
||||
@@ -353,15 +380,7 @@ pub fn joint_values(asm: &Assembly) -> BTreeMap<u64, Vec<f64>> {
|
||||
(j.id, q)
|
||||
})
|
||||
.collect();
|
||||
// Links in order, a few passes for chains.
|
||||
for _ in 0..4 {
|
||||
for l in &asm.links {
|
||||
let Some(src) = vals.get(&l.a).and_then(|q| q.get(l.ia)).copied() else { continue };
|
||||
if let Some(v) = vals.get_mut(&l.b).and_then(|q| q.get_mut(l.ib)) {
|
||||
*v = l.ratio * src + l.offset;
|
||||
}
|
||||
}
|
||||
}
|
||||
apply_links(asm, &mut vals);
|
||||
for j in &asm.joints {
|
||||
if let Some(q) = vals.get_mut(&j.id) {
|
||||
clamp(j, q);
|
||||
@@ -370,12 +389,450 @@ pub fn joint_values(asm: &Assembly) -> BTreeMap<u64, Vec<f64>> {
|
||||
vals
|
||||
}
|
||||
|
||||
/// The turn and offset from A's joint frame to B's: mated (B's z against A's, as faces meet)
|
||||
/// unless flipped.
|
||||
fn flip_of(j: &Joint) -> Mat {
|
||||
let mut m = rot_z(j.angle);
|
||||
m[3][2] = j.offset;
|
||||
if !j.flip { mat_mul(&m, &[[1.0, 0.0, 0.0, 0.0], [0.0, -1.0, 0.0, 0.0], [0.0, 0.0, -1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]) } else { m }
|
||||
}
|
||||
|
||||
/// B's world transform from A's (or A's from B's, going the other way) for the joint's values.
|
||||
fn through(j: &Joint, (fa, fb): &(Mat, Mat), q: &[f64], from_a: bool, t: &Mat) -> Option<Mat> {
|
||||
let link = mat_mul(&mat_mul(&motion(j.kind, q), &flip_of(j)), &mat_inverse(fb)?);
|
||||
if from_a {
|
||||
Some(mat_mul(&mat_mul(t, fa), &link))
|
||||
} else {
|
||||
// T_A = T_B · link⁻¹ · F_a⁻¹
|
||||
Some(mat_mul(&mat_mul(t, &mat_inverse(&link)?), &mat_inverse(fa)?))
|
||||
}
|
||||
}
|
||||
|
||||
/// The spanning tree the solver places occurrences along, out from the fixed ones.
|
||||
struct Tree<'a> {
|
||||
joints: BTreeMap<u64, &'a Joint>,
|
||||
/// Joint frames in component coordinates (A's, B's).
|
||||
frames: BTreeMap<u64, (Mat, Mat)>,
|
||||
fixed: BTreeMap<u64, Mat>,
|
||||
/// Placed occurrence → (the occurrence it is placed from, the joint, from the joint's A side).
|
||||
parent: BTreeMap<u64, (u64, u64, bool)>,
|
||||
/// Placed occurrences in placement order.
|
||||
order: Vec<u64>,
|
||||
}
|
||||
|
||||
impl Tree<'_> {
|
||||
fn step(&self, joint: u64, from_a: bool, t: &Mat, vals: &Values) -> Option<Mat> {
|
||||
let q = vals.get(&joint).map(Vec::as_slice).unwrap_or(&[]);
|
||||
through(self.joints.get(&joint)?, self.frames.get(&joint)?, q, from_a, t)
|
||||
}
|
||||
|
||||
/// Where every placed occurrence is for these values.
|
||||
fn place(&self, vals: &Values) -> BTreeMap<u64, Mat> {
|
||||
let mut placed = self.fixed.clone();
|
||||
for occ in &self.order {
|
||||
let Some(&(from, joint, from_a)) = self.parent.get(occ) else { continue };
|
||||
if let Some(m) = placed.get(&from).and_then(|t| self.step(joint, from_a, t, vals)) {
|
||||
placed.insert(*occ, m);
|
||||
}
|
||||
}
|
||||
placed
|
||||
}
|
||||
|
||||
/// The fixed occurrence an occurrence is placed from, and the tree joints from it up to that one.
|
||||
fn path(&self, mut occ: u64) -> (u64, Vec<(u64, bool)>) {
|
||||
let mut up = Vec::new();
|
||||
while let Some(&(from, joint, from_a)) = self.parent.get(&occ) {
|
||||
if up.len() > self.order.len() {
|
||||
break;
|
||||
}
|
||||
up.push((joint, from_a));
|
||||
occ = from;
|
||||
}
|
||||
(occ, up)
|
||||
}
|
||||
|
||||
/// One occurrence's world placement for these values.
|
||||
fn world(&self, occ: u64, vals: &Values) -> Option<Mat> {
|
||||
let (root, up) = self.path(occ);
|
||||
let mut t = *self.fixed.get(&root)?;
|
||||
for (joint, from_a) in up.iter().rev() {
|
||||
t = self.step(*joint, *from_a, &t, vals)?;
|
||||
}
|
||||
Some(t)
|
||||
}
|
||||
}
|
||||
|
||||
/// Values the loop solver moves at most in one group of loops, and loop-closing joints it takes
|
||||
/// on at most: larger ones are only checked.
|
||||
const MAX_LOOP_VALUES: usize = 64;
|
||||
const MAX_LOOPS: usize = 100;
|
||||
/// Residual evaluations one solve may spend on all its loops.
|
||||
const MAX_LOOP_WORK: usize = 200_000;
|
||||
|
||||
/// Held joint values by (joint, value index): the value each moves from.
|
||||
pub type Held = BTreeMap<(u64, usize), f64>;
|
||||
|
||||
/// The values that set the `around` joints' values through motion links, followed back through
|
||||
/// every link to them (several links may write one value; `apply_links`' order decides which
|
||||
/// wins on the way) for as many links as `apply_links` passes: all the values reached, and those
|
||||
/// that no link sets (the ones a solve can move).
|
||||
fn link_chains(asm: &Assembly, tree: &Tree, around: &BTreeSet<u64>, linked: &BTreeSet<(u64, usize)>) -> (BTreeSet<(u64, usize)>, Vec<(u64, usize)>) {
|
||||
let (mut feeds, mut starts) = (BTreeSet::new(), Vec::new());
|
||||
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 {
|
||||
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() {
|
||||
break;
|
||||
}
|
||||
for &(id, i) in &next {
|
||||
feeds.insert((id, i));
|
||||
if !linked.contains(&(id, i)) && tree.joints.get(&id).is_some_and(|j| i < j.kind.dofs().len()) {
|
||||
starts.push((id, i));
|
||||
}
|
||||
}
|
||||
seen.extend(next.iter().copied());
|
||||
frontier = next;
|
||||
}
|
||||
(feeds, starts)
|
||||
}
|
||||
|
||||
/// The held values among `touches` that move: (joint, index, from, to), `from` within the
|
||||
/// joint's limits. A suppressed joint still sets values through its motion links.
|
||||
fn held_moves(asm: &Assembly, held: &Held, touches: &BTreeSet<(u64, usize)>, vals: &Values) -> Vec<(u64, usize, f64, f64)> {
|
||||
let mut moves = Vec::new();
|
||||
for ((id, i), from) in held.iter().filter(|(v, _)| touches.contains(v)) {
|
||||
let (Some(j), Some(to)) = (asm.joints.iter().find(|j| j.id == *id), vals.get(id).and_then(|q| q.get(*i))) else { continue };
|
||||
let from = match j.limits.get(*i).copied().flatten() {
|
||||
Some((lo, hi)) => from.max(lo.min(hi)).min(hi.max(lo)),
|
||||
None => *from,
|
||||
};
|
||||
if from != *to {
|
||||
moves.push((*id, *i, from, *to));
|
||||
}
|
||||
}
|
||||
moves
|
||||
}
|
||||
|
||||
/// Close the loops, each group of loops that share joints on its own: see `close_group`.
|
||||
/// Returns the moved joints' new stored values (and updates `vals`), and the closing joints of
|
||||
/// the groups whose held values moved but which can't stay closed on the way (nothing in those
|
||||
/// moves; a group that can't close without moving is left to the conflict check).
|
||||
fn close_loops(tree: &Tree, closing: &[u64], asm: &Assembly, held: &Held, vals: &mut Values) -> (Values, BTreeSet<u64>) {
|
||||
let (mut changed, mut stuck) = (BTreeMap::new(), BTreeSet::new());
|
||||
if closing.len() > MAX_LOOPS {
|
||||
return (Values::new(), stuck);
|
||||
}
|
||||
let linked: BTreeSet<(u64, usize)> = asm.links.iter().map(|l| (l.b, l.ib)).collect();
|
||||
// Each loop: its closing joint and the joints on the tree paths between its sides; loops that
|
||||
// share a joint (or a value feeding them through links) are solved together.
|
||||
let mut groups: Vec<(Vec<u64>, BTreeSet<u64>, BTreeSet<(u64, usize)>)> = Vec::new();
|
||||
for id in closing {
|
||||
let Some(j) = tree.joints.get(id) else { continue };
|
||||
// A joint between an occurrence and itself (only a hand-edited file has one) is only checked.
|
||||
if j.a.occurrence == j.b.occurrence {
|
||||
continue;
|
||||
}
|
||||
let side = |occ: u64| -> BTreeSet<u64> { tree.path(occ).1.iter().map(|(k, _)| *k).collect() };
|
||||
let mut around: BTreeSet<u64> = side(j.a.occurrence).symmetric_difference(&side(j.b.occurrence)).copied().collect();
|
||||
around.insert(*id);
|
||||
// The values the loop depends on: its joints' and those its links read. Loops tie together
|
||||
// through a shared value only (a rigid joint on both paths has none; two values of one
|
||||
// joint can feed two independent loops).
|
||||
let mut touches = link_chains(asm, tree, &around, &linked).0;
|
||||
touches.extend(around.iter().filter_map(|id| tree.joints.get(id)).flat_map(|j| (0..j.kind.dofs().len()).map(|i| (j.id, i))));
|
||||
let mut members = vec![*id];
|
||||
groups.retain(|(c, a, t)| {
|
||||
if t.is_disjoint(&touches) {
|
||||
return true;
|
||||
}
|
||||
members.extend(c.iter().copied());
|
||||
around.extend(a.iter().copied());
|
||||
touches.extend(t.iter().copied());
|
||||
false
|
||||
});
|
||||
groups.push((members, around, touches));
|
||||
}
|
||||
let spent = std::cell::Cell::new(0usize);
|
||||
for (group, around, touches) in &groups {
|
||||
let moves = held_moves(asm, held, touches, vals);
|
||||
match close_group(tree, group, around, &moves, asm, held, &linked, &spent, vals) {
|
||||
Some(m) => changed.extend(m),
|
||||
None if !moves.is_empty() => stuck.extend(group.iter().copied()),
|
||||
None => {}
|
||||
}
|
||||
}
|
||||
// The stored values with what each group moved (groups may move different values of one
|
||||
// joint).
|
||||
let mut moved = Values::new();
|
||||
for ((id, i), v) in changed {
|
||||
let Some(j) = tree.joints.get(&id) else { continue };
|
||||
let q = moved.entry(id).or_insert_with(|| {
|
||||
let mut q = j.values.clone();
|
||||
q.resize(j.kind.dofs().len(), 0.0);
|
||||
q
|
||||
});
|
||||
if let Some(slot) = q.get_mut(i) {
|
||||
*slot = v;
|
||||
}
|
||||
}
|
||||
(moved, stuck)
|
||||
}
|
||||
|
||||
/// Close one group of loops: move the values of the joints `around` them (all but the `held`
|
||||
/// values, motion-link targets and rigid joints, within their limits; a linked value moves
|
||||
/// through the start of its link chain) until every loop-closing joint meets. A loop open at the
|
||||
/// start is closed first, from where it is. Then the held values `moves` go from the values they
|
||||
/// are held from to their current ones in small steps, each solved from the last, so the mechanism
|
||||
/// stays on its branch (an open four-bar does not flip crossed). Returns the values that changed
|
||||
/// from what is stored, by (joint, index), and updates `vals` (empty when there is nothing to do
|
||||
/// or the group is too big to solve), or `None` when the loops can't close, or can't stay closed
|
||||
/// on the way.
|
||||
fn close_group(
|
||||
tree: &Tree,
|
||||
closing: &[u64],
|
||||
around: &BTreeSet<u64>,
|
||||
moves: &[(u64, usize, f64, f64)],
|
||||
asm: &Assembly,
|
||||
held: &Held,
|
||||
linked: &BTreeSet<(u64, usize)>,
|
||||
spent: &std::cell::Cell<usize>,
|
||||
vals: &mut Values,
|
||||
) -> Option<BTreeMap<(u64, usize), f64>> {
|
||||
let starts = link_chains(asm, tree, around, linked).1;
|
||||
let mut vars: Vec<(u64, usize)> = Vec::new();
|
||||
for j in around.iter().filter_map(|id| tree.joints.get(id)) {
|
||||
vars.extend((0..j.kind.dofs().len()).map(|i| (j.id, i)).filter(|v| !linked.contains(v) && !held.contains_key(v)));
|
||||
}
|
||||
for v in starts {
|
||||
if !held.contains_key(&v) && !vars.contains(&v) {
|
||||
vars.push(v);
|
||||
}
|
||||
}
|
||||
if vars.len() > MAX_LOOP_VALUES {
|
||||
return Some(BTreeMap::new());
|
||||
}
|
||||
let bounds: Vec<(f64, f64)> = vars
|
||||
.iter()
|
||||
.map(|(id, i)| match tree.joints.get(id).and_then(|j| j.limits.get(*i).copied().flatten()) {
|
||||
Some((lo, hi)) => (lo.min(hi), hi.max(lo)),
|
||||
None => (f64::NEG_INFINITY, f64::INFINITY),
|
||||
})
|
||||
.collect();
|
||||
// Turns weigh like moves through points a loop's size apart.
|
||||
let (mut lo, mut hi) = (Vec3::new(f64::MAX, f64::MAX, f64::MAX), Vec3::new(f64::MIN, f64::MIN, f64::MIN));
|
||||
for id in around {
|
||||
let (Some(j), Some((fa, _))) = (tree.joints.get(id), tree.frames.get(id)) else { continue };
|
||||
let Some(t) = tree.world(j.a.occurrence, vals) else { continue };
|
||||
let p = apply_point(&mat_mul(&t, fa), Vec3::ZERO);
|
||||
lo = Vec3::new(lo.x.min(p.x), lo.y.min(p.y), lo.z.min(p.z));
|
||||
hi = Vec3::new(hi.x.max(p.x), hi.y.max(p.y), hi.z.max(p.z));
|
||||
}
|
||||
let scale = if lo.x <= hi.x { (hi - lo).len().clamp(1.0, 1e6) } else { 1.0 };
|
||||
// Closer than the conflict check needs: 1e-7 mm, and turns of 1e-10 rad across the loop.
|
||||
let tol = (1e-10 * scale).min(1e-7);
|
||||
// The values with the held ones `t` of the way along (and the unknowns at `x`).
|
||||
let place = |work: &mut Values, x: &[f64], t: f64| {
|
||||
for (id, i, from, to) in moves {
|
||||
if let Some(slot) = work.get_mut(id).and_then(|q| q.get_mut(*i)) {
|
||||
*slot = from + (to - from) * t;
|
||||
}
|
||||
}
|
||||
for ((id, i), (v, (lo, hi))) in vars.iter().zip(x.iter().zip(&bounds)) {
|
||||
if let Some(slot) = work.get_mut(id).and_then(|q| q.get_mut(*i)) {
|
||||
*slot = v.max(*lo).min(*hi);
|
||||
}
|
||||
}
|
||||
apply_links(asm, work);
|
||||
};
|
||||
let x0: Vec<f64> = vars.iter().map(|(id, i)| vals.get(id).and_then(|q| q.get(*i)).copied().unwrap_or(0.0)).collect();
|
||||
// Steps that move no value around the loops (links included) more than 5 degrees or a
|
||||
// twentieth of the loops' size.
|
||||
// How many such steps apart two sets of values are, over the loops' values.
|
||||
let stride = |a: &Values, b: &Values| -> f64 {
|
||||
let mut most = 0.0f64;
|
||||
for j in around.iter().filter_map(|id| tree.joints.get(id)) {
|
||||
for i in 0..j.kind.dofs().len() {
|
||||
let (Some(p), Some(q)) = (a.get(&j.id).and_then(|v| v.get(i)), b.get(&j.id).and_then(|v| v.get(i))) else { continue };
|
||||
let step = if j.kind.is_angle(i) { 5f64.to_radians() } else { 0.05 * scale };
|
||||
most = most.max((q - p).abs() / step);
|
||||
}
|
||||
}
|
||||
most
|
||||
};
|
||||
let effective = |x: &[f64], t: f64| {
|
||||
let mut v = vals.clone();
|
||||
place(&mut v, x, t);
|
||||
v
|
||||
};
|
||||
let mut first = 1.0f64;
|
||||
if !moves.is_empty() {
|
||||
first = first.min(1.0 / stride(&effective(&x0, 0.0), &effective(&x0, 1.0)));
|
||||
}
|
||||
let mut work = vals.clone();
|
||||
// How far each closing joint is from meeting with the held values `t` of the way along:
|
||||
// three points of B's joint frame, where the joint wants them against where the tree puts them.
|
||||
let mut gaps = |x: &[f64], t: f64| -> Option<Vec<f64>> {
|
||||
spent.set(spent.get() + 1);
|
||||
if spent.get() > MAX_LOOP_WORK {
|
||||
return None;
|
||||
}
|
||||
place(&mut work, x, t);
|
||||
let mut out = Vec::with_capacity(closing.len() * 9);
|
||||
for id in closing {
|
||||
let (j, (_, fb)) = (tree.joints.get(id)?, tree.frames.get(id)?);
|
||||
let tb = tree.world(j.b.occurrence, &work)?;
|
||||
let want = tree.step(*id, true, &tree.world(j.a.occurrence, &work)?, &work)?;
|
||||
for p in [Vec3::ZERO, Vec3::X * scale, Vec3::Y * scale] {
|
||||
let at = apply_point(fb, p);
|
||||
let d = apply_point(&want, at) - apply_point(&tb, at);
|
||||
out.extend([d.x, d.y, d.z]);
|
||||
}
|
||||
}
|
||||
out.iter().all(|v| v.is_finite()).then_some(out)
|
||||
};
|
||||
// Already closed, with no held value moving. (A moving one is followed all the way: a
|
||||
// rocker driven a full turn ends where it started, but can't get there.)
|
||||
if moves.is_empty() && gaps(&x0, 1.0).is_some_and(|r| largest(&r) <= tol) {
|
||||
return Some(BTreeMap::new());
|
||||
}
|
||||
// A loop open at the start closes first.
|
||||
let mut x = x0;
|
||||
if largest(&gaps(&x, 0.0)?) > tol && !least_squares(&mut |x| gaps(x, 0.0), &mut x, &bounds, tol) {
|
||||
return None;
|
||||
}
|
||||
let (mut t, mut dt) = (if moves.is_empty() { 1.0 } else { 0.0 }, first);
|
||||
let mut at = effective(&x, t);
|
||||
while t < 1.0 {
|
||||
if dt < first * 1e-4 {
|
||||
return None;
|
||||
}
|
||||
let t1 = (t + dt).min(1.0);
|
||||
let mut trial = x.clone();
|
||||
if least_squares(&mut |x| gaps(x, t1), &mut trial, &bounds, tol) {
|
||||
// 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);
|
||||
if stride(&at, &next) > 2.0 {
|
||||
dt *= 0.5;
|
||||
continue;
|
||||
}
|
||||
(x, t, dt, at) = (trial, t1, (dt * 2.0).min(first), next);
|
||||
} else if spent.get() > MAX_LOOP_WORK {
|
||||
return None;
|
||||
} else {
|
||||
dt *= 0.5;
|
||||
}
|
||||
}
|
||||
if largest(&gaps(&x, 1.0)?) > tol {
|
||||
return None;
|
||||
}
|
||||
// The values that changed from what is stored.
|
||||
let moved = vars
|
||||
.iter()
|
||||
.zip(&x)
|
||||
.filter(|((id, i), v)| tree.joints.get(id).is_some_and(|j| j.values.get(*i).is_none_or(|s| (s - *v).abs() > 1e-12)))
|
||||
.map(|(var, v)| (*var, *v))
|
||||
.collect();
|
||||
*vals = work;
|
||||
Some(moved)
|
||||
}
|
||||
|
||||
fn largest(r: &[f64]) -> f64 {
|
||||
r.iter().fold(0.0f64, |m, v| m.max(v.abs()))
|
||||
}
|
||||
|
||||
/// Levenberg–Marquardt: move `x` (kept within `bounds`) until every value of `f` is within
|
||||
/// `tol` of zero. False when it can't get there.
|
||||
fn least_squares(f: &mut dyn FnMut(&[f64]) -> Option<Vec<f64>>, x: &mut Vec<f64>, bounds: &[(f64, f64)], tol: f64) -> bool {
|
||||
const H: f64 = 1e-6;
|
||||
let n = x.len();
|
||||
let norm = |r: &[f64]| r.iter().map(|v| v * v).sum::<f64>();
|
||||
let Some(mut r) = f(x) else { return false };
|
||||
if n == 0 {
|
||||
return largest(&r) <= tol;
|
||||
}
|
||||
let mut lambda = 1e-3;
|
||||
for _ in 0..50 {
|
||||
if largest(&r) <= tol {
|
||||
return true;
|
||||
}
|
||||
// The Jacobian by central differences, then the normal equations JᵀJ δ = −Jᵀr.
|
||||
let m = r.len();
|
||||
let mut jac = vec![0.0; m * n];
|
||||
for k in 0..n {
|
||||
let (mut xp, mut xm) = (x.clone(), x.clone());
|
||||
if let (Some(p), Some(q)) = (xp.get_mut(k), xm.get_mut(k)) {
|
||||
*p += H;
|
||||
*q -= H;
|
||||
}
|
||||
let (Some(rp), Some(rm)) = (f(&xp), f(&xm)) else { return false };
|
||||
for (i, (a, b)) in rp.iter().zip(&rm).enumerate() {
|
||||
if let Some(slot) = jac.get_mut(i * n + k) {
|
||||
*slot = (a - b) / (2.0 * H);
|
||||
}
|
||||
}
|
||||
}
|
||||
let mut jtj = linalg::Mat::zeros(n, n);
|
||||
let mut g = vec![0.0; n];
|
||||
for (i, ri) in r.iter().enumerate() {
|
||||
let row = jac.get(i * n..(i + 1) * n).unwrap_or(&[]);
|
||||
for (p, a) in row.iter().enumerate().filter(|(_, a)| **a != 0.0) {
|
||||
if let Some(gp) = g.get_mut(p) {
|
||||
*gp -= a * ri;
|
||||
}
|
||||
for (q, b) in row.iter().enumerate().skip(p) {
|
||||
jtj.add(p, q, a * b);
|
||||
}
|
||||
}
|
||||
}
|
||||
for p in 0..n {
|
||||
for q in 0..p {
|
||||
jtj.set(p, q, jtj.get(q, p));
|
||||
}
|
||||
}
|
||||
let mut stepped = false;
|
||||
while lambda < 1e12 {
|
||||
let mut a = jtj.clone();
|
||||
for p in 0..n {
|
||||
a.add(p, p, lambda * (1.0 + jtj.get(p, p)));
|
||||
}
|
||||
if let Some(d) = linalg::solve(a, g.clone()) {
|
||||
let next: Vec<f64> = x.iter().zip(&d).zip(bounds).map(|((v, dv), (lo, hi))| (v + dv).max(*lo).min(*hi)).collect();
|
||||
if let Some(rn) = f(&next)
|
||||
&& norm(&rn) < norm(&r)
|
||||
{
|
||||
*x = next;
|
||||
r = rn;
|
||||
lambda = (lambda / 3.0).max(1e-9);
|
||||
stepped = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
lambda *= 4.0;
|
||||
}
|
||||
if !stepped {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
largest(&r) <= tol
|
||||
}
|
||||
|
||||
/// Place occurrences from the joint graph.
|
||||
pub fn solve(doc: &Document, st: &ModelState) -> Solution {
|
||||
solve_holding(doc, st, &BTreeMap::new())
|
||||
}
|
||||
|
||||
/// Place occurrences from the joint graph. Joints that close loops are met by moving the values
|
||||
/// of the other joints around the loop. The `held` values (the ones being driven) keep theirs,
|
||||
/// moving there from the values given (where the loops last closed) so the others follow them.
|
||||
pub fn solve_holding(doc: &Document, st: &ModelState, held: &Held) -> Solution {
|
||||
let asm = &doc.assembly;
|
||||
let mut sol = Solution::default();
|
||||
let joints: Vec<&Joint> = asm.joints.iter().filter(|j| !j.suppressed).take(MAX_JOINTS).collect();
|
||||
let vals = joint_values(asm);
|
||||
let mut vals = joint_values(asm);
|
||||
// Joint frames in component coordinates (errors leave the joint out).
|
||||
let mut frames: BTreeMap<u64, (Mat, Mat)> = BTreeMap::new();
|
||||
for j in &joints {
|
||||
@@ -403,30 +860,15 @@ pub fn solve(doc: &Document, st: &ModelState) -> Solution {
|
||||
dof.insert(j.a.occurrence, 0);
|
||||
}
|
||||
sol.fixed = placed.keys().copied().collect();
|
||||
let flip_m = |j: &Joint| {
|
||||
let mut m = rot_z(j.angle);
|
||||
m[3][2] = j.offset;
|
||||
// Origins mate (B's z against A's, as faces meet) unless flipped.
|
||||
if !j.flip { mat_mul(&m, &[[1.0, 0.0, 0.0, 0.0], [0.0, -1.0, 0.0, 0.0], [0.0, 0.0, -1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]) } else { m }
|
||||
};
|
||||
// B's world transform from A's (or A's from B's, going the other way).
|
||||
let through = |j: &Joint, from_a: bool, t: &Mat| -> Option<Mat> {
|
||||
let (fa, fb) = frames.get(&j.id)?;
|
||||
let q = vals.get(&j.id).cloned().unwrap_or_default();
|
||||
let link = mat_mul(&mat_mul(&motion(j.kind, &q), &flip_m(j)), &mat_inverse(fb)?);
|
||||
if from_a {
|
||||
Some(mat_mul(&mat_mul(t, fa), &link))
|
||||
} else {
|
||||
// T_A = T_B · link⁻¹ · F_a⁻¹
|
||||
Some(mat_mul(&mat_mul(t, &mat_inverse(&link)?), &mat_inverse(fa)?))
|
||||
}
|
||||
};
|
||||
let mut tree =
|
||||
// Every joint's values are known, suppressed ones too (their motion links still set values).
|
||||
Tree { joints: asm.joints.iter().map(|j| (j.id, j)).collect(), frames, fixed: placed.clone(), parent: BTreeMap::new(), order: Vec::new() };
|
||||
let mut used: BTreeSet<u64> = BTreeSet::new();
|
||||
let mut queue: VecDeque<u64> = placed.keys().copied().collect();
|
||||
while let Some(occ) = queue.pop_front() {
|
||||
let Some(t) = placed.get(&occ).copied() else { continue };
|
||||
for j in &joints {
|
||||
if used.contains(&j.id) || !frames.contains_key(&j.id) {
|
||||
if used.contains(&j.id) || !tree.frames.contains_key(&j.id) {
|
||||
continue;
|
||||
}
|
||||
let (other, from_a) = if j.a.occurrence == occ {
|
||||
@@ -439,27 +881,45 @@ pub fn solve(doc: &Document, st: &ModelState) -> Solution {
|
||||
if placed.contains_key(&other) {
|
||||
continue;
|
||||
}
|
||||
let Some(m) = through(j, from_a, &t) else { continue };
|
||||
let Some(m) = tree.step(j.id, from_a, &t, &vals) else { continue };
|
||||
used.insert(j.id);
|
||||
placed.insert(other, m);
|
||||
tree.parent.insert(other, (occ, j.id, from_a));
|
||||
tree.order.push(other);
|
||||
dof.insert(other, dof.get(&occ).copied().unwrap_or(0) + j.kind.dofs().len());
|
||||
queue.push_back(other);
|
||||
}
|
||||
}
|
||||
// Joints that close loops: do they agree?
|
||||
for j in &joints {
|
||||
if used.contains(&j.id) || !frames.contains_key(&j.id) {
|
||||
continue;
|
||||
// Joints that close loops: move the loops' values to meet them, then check they agree.
|
||||
let closing: Vec<u64> = joints
|
||||
.iter()
|
||||
.filter(|j| !used.contains(&j.id) && tree.frames.contains_key(&j.id))
|
||||
.filter(|j| placed.contains_key(&j.a.occurrence) && placed.contains_key(&j.b.occurrence))
|
||||
.map(|j| j.id)
|
||||
.collect();
|
||||
let mut stuck = BTreeSet::new();
|
||||
if !closing.is_empty() {
|
||||
(sol.values, stuck) = close_loops(&tree, &closing, asm, held, &mut vals);
|
||||
if !sol.values.is_empty() {
|
||||
placed = tree.place(&vals);
|
||||
}
|
||||
}
|
||||
for id in &closing {
|
||||
let Some(j) = tree.joints.get(id) else { continue };
|
||||
let (Some(ta), Some(tb)) = (placed.get(&j.a.occurrence), placed.get(&j.b.occurrence)) else { continue };
|
||||
let Some(want) = through(j, true, ta) else { continue };
|
||||
let (dt, da) = mismatch(&want, tb);
|
||||
let (Some(want), Some((_, fb))) = (tree.step(*id, true, ta, &vals), tree.frames.get(id)) else { continue };
|
||||
// Measured at the joint, where the loop solver closes it.
|
||||
let (dt, da) = mismatch(&mat_mul(&want, fb), &mat_mul(tb, fb));
|
||||
if dt > 1e-6 || da > 1e-8 {
|
||||
sol.conflicts.push((
|
||||
j.id,
|
||||
j.name.clone(),
|
||||
format!("off by {dt:.4} mm and {:.3}° from the placement the other joints give", da.to_degrees()),
|
||||
));
|
||||
} else if stuck.contains(id) {
|
||||
// It meets there, but the loop would have to come apart on the way (a rocker
|
||||
// driven a full turn ends where it started).
|
||||
sol.conflicts.push((j.id, j.name.clone(), "the loop can't stay closed on the way to these values".into()));
|
||||
}
|
||||
}
|
||||
for o in &doc.occurrences {
|
||||
@@ -472,10 +932,16 @@ pub fn solve(doc: &Document, st: &ModelState) -> Solution {
|
||||
}
|
||||
|
||||
impl Document {
|
||||
/// Apply a solution: occurrence transforms (in their parents) for the occurrences it places.
|
||||
/// Returns true when something moved.
|
||||
/// Apply a solution: occurrence transforms (in their parents) for the occurrences it places,
|
||||
/// and the values of the joints that moved to close loops. Returns true when something moved.
|
||||
pub fn apply_joint_solution(&mut self, sol: &Solution) -> bool {
|
||||
let mut changed = false;
|
||||
for (id, q) in &sol.values {
|
||||
if let Some(j) = self.assembly.joints.iter_mut().find(|j| j.id == *id) {
|
||||
j.values.clone_from(q);
|
||||
changed = true;
|
||||
}
|
||||
}
|
||||
let parents: BTreeMap<u64, u64> = self.occurrences.iter().map(|o| (o.id, o.parent)).collect();
|
||||
for (occ, world) in &sol.world {
|
||||
let parent = parents.get(occ).copied().unwrap_or(0);
|
||||
@@ -536,4 +1002,12 @@ mod tests {
|
||||
let f = frame(Vec3::new(1.0, 2.0, 3.0), Vec3::Z, None).unwrap();
|
||||
assert!(close(&mat_mul(&f, &mat_inverse(&f).unwrap()), &IDENTITY));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn mismatch_resolves_tiny_turns() {
|
||||
let (_, a) = mismatch(&rot_z(1e-9), &IDENTITY);
|
||||
assert!((a - 1e-9).abs() < 1e-15, "{a}");
|
||||
let (_, a) = mismatch(&rot_z(3.0), &IDENTITY);
|
||||
assert!((a - 3.0).abs() < 1e-12, "{a}");
|
||||
}
|
||||
}
|
||||
@@ -3,6 +3,8 @@
|
||||
//! joint graph and moves the occurrences it places; the session re-solves after any edit too,
|
||||
//! so joints follow the geometry their origins snap to.
|
||||
|
||||
use std::collections::BTreeMap;
|
||||
|
||||
use serde_json::{Value, json};
|
||||
use solidcraft_doc::expr::Kind;
|
||||
use solidcraft_doc::joints::{self, Joint, JointKind, JointOrigin, MotionLink, NamedOrigin, Snap};
|
||||
@@ -25,7 +27,8 @@ pub static COMMANDS: &[CommandSpec] = &[
|
||||
),
|
||||
CommandSpec::new("joint.rigid_group", "Rigid Group", rigid_group).at("SOLID", "ASSEMBLE").icon("rigid_group").params("occurrences: [ids or names] (move together, as they are)"),
|
||||
CommandSpec::new("joint.drive", "Drive Joints", drive).at("SOLID", "ASSEMBLE").icon("drive").params(
|
||||
"joint: id|name; value | values: [angles in deg or expressions, distances] (clamped to the limits)",
|
||||
"joint: id|name; value | values: [angles in deg or expressions, distances] (clamped to the limits; the other joints of a \
|
||||
closed loop follow, and the motion stops where the loop can no longer close → stopped_by_loop)",
|
||||
),
|
||||
CommandSpec::new("joint.motion_link", "Motion Link", motion_link)
|
||||
.at("SOLID", "ASSEMBLE")
|
||||
@@ -43,8 +46,14 @@ pub static COMMANDS: &[CommandSpec] = &[
|
||||
|
||||
/// Re-solve the joints and move the occurrences they place (after any edit).
|
||||
pub fn resolve(s: &mut Session) -> joints::Solution {
|
||||
resolve_holding(s, &BTreeMap::new())
|
||||
}
|
||||
|
||||
/// Re-solve the joints, keeping the `held` values (the ones being driven, mapped to the values
|
||||
/// they move from) where loops are closed.
|
||||
pub fn resolve_holding(s: &mut Session, held: &joints::Held) -> joints::Solution {
|
||||
let st = s.model.state();
|
||||
let sol = joints::solve(&s.doc, &st);
|
||||
let sol = joints::solve_holding(&s.doc, &st, held);
|
||||
if !sol.world.is_empty() {
|
||||
let mut next = (*s.doc).clone();
|
||||
if next.apply_joint_solution(&sol) {
|
||||
@@ -353,13 +362,19 @@ fn drive(s: &mut Session, p: &Value) -> Result<Value> {
|
||||
}
|
||||
}
|
||||
joints::clamp(j, &mut q);
|
||||
// Moved as far toward `q` as contact sets allow.
|
||||
let r = super::motion::drive_joint(s, id, q)?;
|
||||
// Moved as far toward `q` as contact sets and closed loops allow. Values left out of
|
||||
// `values` are not driven: in a closed loop they follow.
|
||||
let given = match p.get("values") {
|
||||
Some(Value::Array(a)) => a.len(),
|
||||
_ => 1,
|
||||
};
|
||||
let r = super::motion::drive_joint(s, id, q, given)?;
|
||||
let sol = joints::solve(&s.doc, &s.model.state());
|
||||
let mut out = sol_json(&sol);
|
||||
out["joint"] = json!(id);
|
||||
out["values"] = json!(s.doc.assembly.joints.iter().find(|j| j.id == id).map(|j| j.values.clone()));
|
||||
out["stopped_by_contact"] = r["stopped_by_contact"].clone();
|
||||
out["stopped_by_loop"] = r["stopped_by_loop"].clone();
|
||||
Ok(out)
|
||||
}
|
||||
|
||||
|
||||
@@ -295,3 +295,428 @@ fn interference_box_sweep_matches_every_pair_check() {
|
||||
let (pairs, skipped) = super::overlapping_pairs(&boxes, 10);
|
||||
assert_eq!((pairs.len(), skipped), (10, every.len() - 10));
|
||||
}
|
||||
|
||||
/// 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]) {
|
||||
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) {
|
||||
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] {
|
||||
let d = q - p;
|
||||
let l = d.len();
|
||||
let a = (r * r - s * s + l * l) / (2.0 * l);
|
||||
let m = p + d * (a / l);
|
||||
let n = Vec3::new(-d.y, d.x, 0.0) * ((r * r - a * a).max(0.0).sqrt() / l);
|
||||
[m + n, m - n]
|
||||
}
|
||||
|
||||
/// 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) {
|
||||
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), ("Rocker", 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, "revolute", "Rocker pin", "Coupler", "Rocker", q0, Vec3::Z);
|
||||
pin(&mut s, "revolute", "Rocker", "Ground", "Rocker", o4, Vec3::Z);
|
||||
(s, p0, q0)
|
||||
}
|
||||
|
||||
/// The four-bar's rocker pin with the crank driven to `to`°, followed a degree at a time from
|
||||
/// `from`° (where it is at `q`), so it stays on its branch.
|
||||
fn rocker_pin(from: f64, to: f64, mut q: Vec3) -> Vec3 {
|
||||
let n = ((to - from).abs().ceil() as usize).max(1);
|
||||
for k in 1..=n {
|
||||
let th = (90.0 + from + (to - from) * k as f64 / n as f64).to_radians();
|
||||
let [m1, m2] = meet(Vec3::new(20.0 * th.cos(), 20.0 * th.sin(), 0.0), 50.0, Vec3::new(45.0, 0.0, 0.0), 40.0);
|
||||
q = if m1.dist(q) <= m2.dist(q) { m1 } else { m2 };
|
||||
}
|
||||
q
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn four_bar_follows_its_crank() {
|
||||
let (mut s, p0, q0) = four_bar();
|
||||
let mut q = q0;
|
||||
let mut at_330 = q0;
|
||||
for k in 1..=12 {
|
||||
let v = 30.0 * f64::from(k);
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": v}));
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{v}°: {r}");
|
||||
assert!(r["stopped_by_loop"].is_null(), "{v}°: {r}");
|
||||
// The driven joint keeps exactly the value asked for.
|
||||
assert!((r["values"][0].as_f64().unwrap_or(f64::NAN) - v.to_radians()).abs() < 1e-12, "{r}");
|
||||
q = rocker_pin(v - 30.0, v, q);
|
||||
let th = (90.0 + v).to_radians();
|
||||
let p = Vec3::new(20.0 * th.cos(), 20.0 * th.sin(), 0.0);
|
||||
for (part, local, want) in [("Crank", p0, p), ("Coupler", p0, p), ("Coupler", q0, q), ("Rocker", q0, q)] {
|
||||
assert!(near(place(&s, part, local), want), "{v}°: {part} {:?} wants {want:?}", place(&s, part, local));
|
||||
}
|
||||
if k == 11 {
|
||||
at_330 = q;
|
||||
}
|
||||
}
|
||||
// Back where it started after a full turn, and undo steps back.
|
||||
assert!(near(q, q0));
|
||||
run(&mut s, "edit.undo", json!({}));
|
||||
assert!(near(place(&s, "Rocker", q0), at_330), "{:?}", place(&s, "Rocker", q0));
|
||||
// One 180° drive lands on the same (open) branch as the stepped path.
|
||||
let (mut s, _, q0) = four_bar();
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": 180}));
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
assert!(near(place(&s, "Rocker", q0), rocker_pin(0.0, 180.0, q0)), "{:?}", place(&s, "Rocker", q0));
|
||||
// A motion study playing the crank places the rocker the same way.
|
||||
run(&mut s, "motion.study", json!({"name": "Turn", "steps": 10, "keys": [{"joint": "Crank", "points": [[0, 0], [10, 120]]}]}));
|
||||
let r = run(&mut s, "motion.play", json!({"study": "Turn", "step": 5}));
|
||||
assert!(r["stopped_by_loop"].is_null(), "{r}");
|
||||
assert!(near(place(&s, "Rocker", q0), rocker_pin(180.0, 60.0, rocker_pin(0.0, 180.0, q0))), "{:?}", place(&s, "Rocker", q0));
|
||||
}
|
||||
|
||||
/// The rocker swings between its toggle positions, where crank and coupler line up (|Q| = 70
|
||||
/// and |Q| = 30, about 69.3° and 139.2° from the ground line); driving it further stops where
|
||||
/// the loop still closes.
|
||||
#[test]
|
||||
fn four_bar_rocker_stops_where_the_loop_cannot_close() {
|
||||
let (mut s, _, q0) = four_bar();
|
||||
let start = q0.y.atan2(q0.x - 45.0).to_degrees();
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Rocker", "value": 100}));
|
||||
assert!(r["stopped_by_loop"].is_object(), "{r}");
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
let reached = r["values"][0].as_f64().unwrap_or(f64::NAN).to_degrees();
|
||||
// |Q|² = 45² + 40² + 2·45·40·cos φ = 30².
|
||||
let limit = ((900.0f64 - 3625.0) / 3600.0).acos().to_degrees() - start;
|
||||
assert!(reached > limit - 5.0 && reached <= limit, "stopped at {reached}°, the limit is {limit}°");
|
||||
// Inside its swing it moves freely, and the crank follows.
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Rocker", "value": -10}));
|
||||
assert!(r["stopped_by_loop"].is_null() && r["conflicts"].as_array().map(Vec::len) == Some(0), "{r}");
|
||||
let q = place(&s, "Rocker", q0);
|
||||
assert!((q.y.atan2(q.x - 45.0).to_degrees() - (start - 10.0)).abs() < 1e-6, "{q:?}");
|
||||
assert!(place(&s, "Coupler", q0).dist(q) < 1e-6);
|
||||
// A full turn would end where it started, but the rocker can't get there.
|
||||
let (mut s, _, _) = four_bar();
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Rocker", "value": 360}));
|
||||
assert!(r["stopped_by_loop"].is_object(), "{r}");
|
||||
assert!(r["values"][0].as_f64().unwrap_or(f64::NAN).to_degrees() <= limit, "{r}");
|
||||
}
|
||||
|
||||
/// A loop value a motion link sets follows through the start of its link chain: a dial outside
|
||||
/// the loop turns a gear, which sets the rocker's angle, so turning the crank turns the dial.
|
||||
#[test]
|
||||
fn four_bar_turns_the_source_of_a_linked_loop_value() {
|
||||
let linked = || {
|
||||
let (mut s, _, q0) = four_bar();
|
||||
for (name, x) in [("Dial", 80.0), ("Gear", 100.0)] {
|
||||
part(&mut s, name, [x, 0.0, -5.0]);
|
||||
pin(&mut s, "revolute", name, "Ground", name, Vec3::new(x, 0.0, 0.0), Vec3::Z);
|
||||
}
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Dial", "b": "Gear", "ratio": 1}));
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Gear", "b": "Rocker", "ratio": 0.5}));
|
||||
(s, q0)
|
||||
};
|
||||
let (mut s, q0) = linked();
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": 60}));
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
let q = rocker_pin(0.0, 60.0, q0);
|
||||
assert!(near(place(&s, "Rocker", q0), q), "{:?} wants {q:?}", place(&s, "Rocker", q0));
|
||||
let turn = q.y.atan2(q.x - 45.0) - q0.y.atan2(q0.x - 45.0);
|
||||
let dial = s.doc.assembly.joints.iter().find(|j| j.name == "Dial").and_then(|j| j.values.first().copied()).unwrap_or(f64::NAN);
|
||||
assert!((dial - 2.0 * turn).abs() < 1e-9, "dial {dial}, rocker turned {turn}");
|
||||
// Driving the dial drives the rocker through the chain: two turns of the dial would turn
|
||||
// the rocker a full turn, which it can't make.
|
||||
let (mut s, _) = linked();
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Dial", "value": 720}));
|
||||
assert!(r["stopped_by_loop"].is_object(), "{r}");
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
}
|
||||
|
||||
/// Values left out of a drive are free to follow in a loop: a plate on a planar joint whose
|
||||
/// travel a diagonal slider limits. Driving only the planar joint's x moves its y along.
|
||||
#[test]
|
||||
fn values_left_out_of_a_drive_follow_the_loop() {
|
||||
let mut s = Session::default();
|
||||
part(&mut s, "Ground", [-1.0, -1.0, -5.0]);
|
||||
part(&mut s, "Plate", [-1.0, -1.0, 0.0]);
|
||||
let g = occ(&s, "Ground");
|
||||
run(&mut s, "occurrence.ground", json!({"occurrence": g, "grounded": true}));
|
||||
let frame = |o: &str, z: [f64; 3], x: [f64; 3]| json!({"occurrence": o, "frame": {"origin": [0, 0, 0], "z": z, "x": x}});
|
||||
let flat = |o: &str| frame(o, [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]);
|
||||
let r = run(&mut s, "joint.create", json!({"type": "planar", "a": flat("Ground"), "b": flat("Plate"), "flip": true, "name": "Table"}));
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
let diagonal = |o: &str| frame(o, [1.0, 1.0, 0.0], [0.0, 0.0, 1.0]);
|
||||
let r = run(&mut s, "joint.create", json!({"type": "slider", "a": diagonal("Ground"), "b": diagonal("Plate"), "flip": true, "name": "Rail"}));
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Table", "values": [10]}));
|
||||
assert!(r["stopped_by_loop"].is_null() && r["conflicts"].as_array().map(Vec::len) == Some(0), "{r}");
|
||||
assert!(near(place(&s, "Plate", Vec3::ZERO), Vec3::new(10.0, 10.0, 0.0)), "{:?}", place(&s, "Plate", Vec3::ZERO));
|
||||
}
|
||||
|
||||
/// A slider-crank: crank 15 about the origin, rod 40, a piston sliding along X. The piston
|
||||
/// sits at a·cos θ + √(b² − (a·sin θ)²).
|
||||
#[test]
|
||||
fn slider_crank_moves_the_piston() {
|
||||
let mut s = Session::default();
|
||||
let (o, p0) = (Vec3::ZERO, Vec3::new(0.0, 15.0, 0.0));
|
||||
let s0 = Vec3::new((40.0f64 * 40.0 - 15.0 * 15.0).sqrt(), 0.0, 0.0);
|
||||
for (name, c) in [("Ground", o), ("Crank", p0), ("Rod", s0 * 0.5), ("Piston", s0)] {
|
||||
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", o, Vec3::Z);
|
||||
pin(&mut s, "revolute", "Crank pin", "Crank", "Rod", p0, Vec3::Z);
|
||||
pin(&mut s, "revolute", "Wrist pin", "Rod", "Piston", s0, Vec3::Z);
|
||||
pin(&mut s, "slider", "Bore", "Ground", "Piston", s0, Vec3::X);
|
||||
for v in [40.0, 135.0, 270.0] {
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": v}));
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{v}°: {r}");
|
||||
let th = (90.0f64 + v).to_radians();
|
||||
let x = 15.0 * th.cos() + (40.0f64.powi(2) - (15.0 * th.sin()).powi(2)).sqrt();
|
||||
assert!(near(place(&s, "Piston", s0), Vec3::new(x, 0.0, 0.0)), "{v}°: {:?} wants x = {x}", place(&s, "Piston", s0));
|
||||
assert!(near(place(&s, "Rod", p0), Vec3::new(15.0 * th.cos(), 15.0 * th.sin(), 0.0)));
|
||||
}
|
||||
}
|
||||
|
||||
/// 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}})
|
||||
}
|
||||
|
||||
/// A loop open from the start closes from where it is: an arm on a revolute joint, and a rigid
|
||||
/// joint that wants it turned 90°.
|
||||
#[test]
|
||||
fn an_open_loop_closes_from_where_it_is() {
|
||||
let mut s = Session::default();
|
||||
part(&mut s, "Ground", [-1.0, -1.0, -5.0]);
|
||||
part(&mut s, "Arm", [0.0, -1.0, 0.0]);
|
||||
let g = occ(&s, "Ground");
|
||||
run(&mut s, "occurrence.ground", json!({"occurrence": g, "grounded": true}));
|
||||
pin(&mut s, "revolute", "Pivot", "Ground", "Arm", Vec3::ZERO, Vec3::Z);
|
||||
let r = run(
|
||||
&mut s,
|
||||
"joint.create",
|
||||
json!({"type": "rigid", "a": framed("Ground", [0.0; 3], [0.0, 0.0, 1.0], [0.0, 1.0, 0.0]),
|
||||
"b": framed("Arm", [0.0; 3], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]), "flip": true, "name": "Lock"}),
|
||||
);
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
assert!(near(place(&s, "Arm", Vec3::new(10.0, 0.0, 0.0)), Vec3::new(0.0, 10.0, 0.0)), "{:?}", place(&s, "Arm", Vec3::new(10.0, 0.0, 0.0)));
|
||||
}
|
||||
|
||||
/// A loop that can't close does not hold up another one: the four-bar still drives beside a
|
||||
/// block whose rigid joint wants it where its pivot can't take it.
|
||||
#[test]
|
||||
fn a_broken_loop_does_not_stop_another() {
|
||||
let (mut s, _, q0) = four_bar();
|
||||
part(&mut s, "Block", [200.0, -1.0, -5.0]);
|
||||
pin(&mut s, "revolute", "Block pivot", "Ground", "Block", Vec3::new(200.0, 0.0, 0.0), Vec3::Z);
|
||||
run(
|
||||
&mut s,
|
||||
"joint.create",
|
||||
json!({"type": "rigid", "a": {"occurrence": "Ground", "point": [200, 0, 0]}, "b": {"occurrence": "Block", "point": [210, 0, 0]}, "flip": true, "name": "Wrong"}),
|
||||
);
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": 60}));
|
||||
let names: Vec<&str> = r["conflicts"].as_array().into_iter().flatten().filter_map(|c| c["name"].as_str()).collect();
|
||||
assert_eq!(names, ["Wrong"], "{r}");
|
||||
assert!(r["stopped_by_loop"].is_null(), "{r}");
|
||||
assert!(near(place(&s, "Rocker", q0), rocker_pin(0.0, 60.0, q0)), "{:?}", place(&s, "Rocker", q0));
|
||||
}
|
||||
|
||||
/// A loop locked rigid can't move at all: driving its crank stops at once.
|
||||
#[test]
|
||||
fn a_locked_loop_does_not_move() {
|
||||
let (mut s, _, _) = four_bar();
|
||||
run(&mut s, "joint.as_built", json!({"type": "rigid", "a": "Ground", "b": "Rocker", "name": "Lock"}));
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": 360}));
|
||||
assert!(r["stopped_by_loop"].is_object(), "{r}");
|
||||
assert!(r["values"][0].as_f64().unwrap_or(f64::NAN).abs() < 1e-3, "{r}");
|
||||
}
|
||||
|
||||
/// A link that turns the rocker 100 times as far as its dial: a small turn of the dial is a full
|
||||
/// turn of the rocker, which it can't make.
|
||||
#[test]
|
||||
fn a_geared_down_loop_value_is_followed_all_the_way() {
|
||||
let (mut s, _, _) = four_bar();
|
||||
part(&mut s, "Dial", [80.0, 0.0, -5.0]);
|
||||
pin(&mut s, "revolute", "Dial", "Ground", "Dial", Vec3::new(80.0, 0.0, 0.0), Vec3::Z);
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Dial", "b": "Rocker", "ratio": 100}));
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Dial", "value": 3.6}));
|
||||
assert!(r["stopped_by_loop"].is_object(), "{r}");
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
}
|
||||
|
||||
/// A suppressed dial still turns the rocker through its motion link, and a rocker whose value
|
||||
/// the file leaves out starts from 0: both are followed all the way, so a full turn stops.
|
||||
#[test]
|
||||
fn a_full_turn_stops_from_a_suppressed_dial_or_a_missing_value() {
|
||||
let (mut s, _, _) = four_bar();
|
||||
part(&mut s, "Dial", [80.0, 0.0, -5.0]);
|
||||
pin(&mut s, "revolute", "Dial", "Ground", "Dial", Vec3::new(80.0, 0.0, 0.0), Vec3::Z);
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Dial", "b": "Rocker", "ratio": 0.5}));
|
||||
run(&mut s, "joint.edit", json!({"joint": "Dial", "suppressed": true}));
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Dial", "value": 720}));
|
||||
assert!(r["stopped_by_loop"].is_object(), "{r}");
|
||||
// Undriven, the suppressed dial still follows the rocker when the crank turns.
|
||||
run(&mut s, "joint.drive", json!({"joint": "Dial", "value": 0}));
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": 60}));
|
||||
assert!(r["stopped_by_loop"].is_null() && r["conflicts"].as_array().map(Vec::len) == Some(0), "{r}");
|
||||
let (mut s, _, _) = four_bar();
|
||||
if let Some(j) = s.doc_mut().assembly.joints.iter_mut().find(|j| j.name == "Rocker") {
|
||||
j.values.clear();
|
||||
}
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Rocker", "value": 360}));
|
||||
assert!(r["stopped_by_loop"].is_object(), "{r}");
|
||||
}
|
||||
|
||||
/// Loops that only share a rigid joint are independent: an arm whose lock can't close does not
|
||||
/// stop another arm, beside it on the same rigidly mounted base, from closing its own loop.
|
||||
#[test]
|
||||
fn loops_sharing_only_a_rigid_joint_are_solved_apart() {
|
||||
let mut s = Session::default();
|
||||
for (name, x) in [("Ground", -1.0), ("Base", -1.0), ("Arm", 0.0), ("Other", 100.0)] {
|
||||
part(&mut s, name, [x, -1.0, 0.0]);
|
||||
}
|
||||
let g = occ(&s, "Ground");
|
||||
run(&mut s, "occurrence.ground", json!({"occurrence": g, "grounded": true}));
|
||||
run(&mut s, "joint.as_built", json!({"type": "rigid", "a": "Ground", "b": "Base", "name": "Mount"}));
|
||||
pin(&mut s, "revolute", "Pivot", "Base", "Arm", Vec3::ZERO, Vec3::Z);
|
||||
pin(&mut s, "revolute", "Other pivot", "Base", "Other", Vec3::new(100.0, 0.0, 0.0), Vec3::Z);
|
||||
run(
|
||||
&mut s,
|
||||
"joint.create",
|
||||
json!({"type": "rigid", "a": {"occurrence": "Ground", "point": [100, 0, 0]}, "b": {"occurrence": "Other", "point": [110, 0, 0]}, "flip": true, "name": "Wrong"}),
|
||||
);
|
||||
let r = run(
|
||||
&mut s,
|
||||
"joint.create",
|
||||
json!({"type": "rigid", "a": framed("Ground", [0.0; 3], [0.0, 0.0, 1.0], [0.0, 1.0, 0.0]),
|
||||
"b": framed("Arm", [0.0; 3], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]), "flip": true, "name": "Lock"}),
|
||||
);
|
||||
// The broken loop is reported (on whichever of its joints closes it), the arm's is not.
|
||||
let names: Vec<&str> = r["conflicts"].as_array().into_iter().flatten().filter_map(|c| c["name"].as_str()).collect();
|
||||
assert!(names.len() == 1 && ["Wrong", "Other pivot"].contains(&names[0]), "{r}");
|
||||
assert!(near(place(&s, "Arm", Vec3::new(10.0, 0.0, 0.0)), Vec3::new(0.0, 10.0, 0.0)), "{:?}", place(&s, "Arm", Vec3::new(10.0, 0.0, 0.0)));
|
||||
}
|
||||
|
||||
/// Two links writing one value: whichever carries a driven dial's turn into the loop, the turn
|
||||
/// is followed (here dial A turns gear B, which turns the rocker, before dial D rewrites B).
|
||||
#[test]
|
||||
fn a_turn_carried_in_by_any_link_is_followed() {
|
||||
let (mut s, _, _) = four_bar();
|
||||
for (name, x) in [("A", 80.0), ("B", 100.0), ("D", 120.0)] {
|
||||
part(&mut s, name, [x, 0.0, -5.0]);
|
||||
pin(&mut s, "revolute", name, "Ground", name, Vec3::new(x, 0.0, 0.0), Vec3::Z);
|
||||
}
|
||||
run(&mut s, "joint.motion_link", json!({"a": "A", "b": "B", "ratio": 1}));
|
||||
run(&mut s, "joint.motion_link", json!({"a": "B", "b": "Rocker", "ratio": 0.5}));
|
||||
run(&mut s, "joint.motion_link", json!({"a": "D", "b": "B", "ratio": 1}));
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "A", "value": 720}));
|
||||
assert!(r["stopped_by_loop"].is_object(), "{r}");
|
||||
}
|
||||
|
||||
/// Several links writing one value: every value a link chain starts from can move to close the
|
||||
/// loop, whichever of them the link order lets through (here dial A's, before dial B rewrites
|
||||
/// the gear).
|
||||
#[test]
|
||||
fn a_loop_value_moves_through_whichever_link_carries_it() {
|
||||
let (mut s, _, q0) = four_bar();
|
||||
for (name, x) in [("A", 80.0), ("G", 100.0), ("B", 120.0)] {
|
||||
part(&mut s, name, [x, 0.0, -5.0]);
|
||||
pin(&mut s, "revolute", name, "Ground", name, Vec3::new(x, 0.0, 0.0), Vec3::Z);
|
||||
}
|
||||
run(&mut s, "joint.motion_link", json!({"a": "A", "b": "G", "ratio": 1}));
|
||||
run(&mut s, "joint.motion_link", json!({"a": "G", "b": "Rocker", "ratio": 1}));
|
||||
run(&mut s, "joint.motion_link", json!({"a": "B", "b": "G", "ratio": 1}));
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": 60}));
|
||||
assert!(r["stopped_by_loop"].is_null() && r["conflicts"].as_array().map(Vec::len) == Some(0), "{r}");
|
||||
assert!(near(place(&s, "Rocker", q0), rocker_pin(0.0, 60.0, q0)), "{:?}", place(&s, "Rocker", q0));
|
||||
}
|
||||
|
||||
/// Two values of one joint can feed two independent loops: a puck's x drives a pair of parallel
|
||||
/// slides, its y a slide whose rigid lock can't close. Driving x moves the healthy pair.
|
||||
#[test]
|
||||
fn values_of_one_joint_feed_independent_loops() {
|
||||
let mut s = Session::default();
|
||||
for (name, y) in [("Ground", -10.0), ("Puck", -20.0), ("Car", 50.0), ("Other", 100.0)] {
|
||||
part(&mut s, name, [-1.0, y, 0.0]);
|
||||
}
|
||||
let g = occ(&s, "Ground");
|
||||
run(&mut s, "occurrence.ground", json!({"occurrence": g, "grounded": true}));
|
||||
let flat = |o: &str| framed(o, [0.0, -20.0, 0.0], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]);
|
||||
run(&mut s, "joint.create", json!({"type": "planar", "a": flat("Ground"), "b": flat("Puck"), "flip": true, "name": "Table"}));
|
||||
pin(&mut s, "slider", "Rail", "Ground", "Car", Vec3::new(0.0, 50.0, 0.0), Vec3::X);
|
||||
pin(&mut s, "slider", "Guide", "Ground", "Car", Vec3::new(10.0, 50.0, 0.0), Vec3::X);
|
||||
pin(&mut s, "slider", "Lift", "Ground", "Other", Vec3::new(0.0, 100.0, 0.0), Vec3::Y);
|
||||
run(
|
||||
&mut s,
|
||||
"joint.create",
|
||||
json!({"type": "rigid", "a": {"occurrence": "Ground", "point": [0, 100, 0]}, "b": {"occurrence": "Other", "point": [10, 100, 0]}, "flip": true, "name": "Wrong"}),
|
||||
);
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Table", "ia": 0, "b": "Rail", "ratio": 1}));
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Table", "ia": 1, "b": "Lift", "ratio": 1}));
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Table", "values": [10]}));
|
||||
assert!(r["stopped_by_loop"].is_null(), "{r}");
|
||||
let names: Vec<&str> = r["conflicts"].as_array().into_iter().flatten().filter_map(|c| c["name"].as_str()).collect();
|
||||
assert_eq!(names, ["Wrong"], "{r}");
|
||||
assert!(near(place(&s, "Car", Vec3::new(0.0, 50.0, 0.0)), Vec3::new(10.0, 50.0, 0.0)), "{:?}", place(&s, "Car", Vec3::new(0.0, 50.0, 0.0)));
|
||||
}
|
||||
|
||||
/// Two loops can move different values of one joint: each keeps what it moved. A table's x
|
||||
/// feeds one pair of parallel slides and its y another; a study drives both pairs.
|
||||
#[test]
|
||||
fn loops_moving_different_values_of_one_joint_keep_both() {
|
||||
let mut s = Session::default();
|
||||
for (name, y) in [("Ground", -10.0), ("Puck", -20.0), ("CarA", 50.0), ("CarB", 100.0)] {
|
||||
part(&mut s, name, [-1.0, y, 0.0]);
|
||||
}
|
||||
let g = occ(&s, "Ground");
|
||||
run(&mut s, "occurrence.ground", json!({"occurrence": g, "grounded": true}));
|
||||
let flat = |o: &str| framed(o, [0.0, -20.0, 0.0], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]);
|
||||
run(&mut s, "joint.create", json!({"type": "planar", "a": flat("Ground"), "b": flat("Puck"), "flip": true, "name": "Table"}));
|
||||
for (car, y) in [("CarA", 50.0), ("CarB", 100.0)] {
|
||||
pin(&mut s, "slider", &format!("{car} rail"), "Ground", car, Vec3::new(0.0, y, 0.0), Vec3::X);
|
||||
pin(&mut s, "slider", &format!("{car} guide"), "Ground", car, Vec3::new(10.0, y, 0.0), Vec3::X);
|
||||
}
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Table", "ia": 0, "b": "CarA rail", "ratio": 1}));
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Table", "ia": 1, "b": "CarB rail", "ratio": 1}));
|
||||
let keys = json!([{"joint": "CarA guide", "points": [[0, 0], [10, 10]]}, {"joint": "CarB guide", "points": [[0, 0], [10, 20]]}]);
|
||||
run(&mut s, "motion.study", json!({"name": "Both", "steps": 10, "keys": keys}));
|
||||
let r = run(&mut s, "motion.play", json!({"study": "Both", "step": 10}));
|
||||
assert!(r["stopped_by_loop"].is_null(), "{r}");
|
||||
let table = s.doc.assembly.joints.iter().find(|j| j.name == "Table").map(|j| j.values.clone()).unwrap_or_default();
|
||||
assert!(table.len() == 3 && (table[0] - 10.0).abs() < 1e-6 && (table[1] - 20.0).abs() < 1e-6, "{table:?}");
|
||||
// A solve with nothing driven leaves them where they are.
|
||||
run(&mut s, "joint.solve", json!({}));
|
||||
assert!(near(place(&s, "CarA", Vec3::new(0.0, 50.0, 0.0)), Vec3::new(10.0, 50.0, 0.0)), "{:?}", place(&s, "CarA", Vec3::new(0.0, 50.0, 0.0)));
|
||||
assert!(near(place(&s, "CarB", Vec3::new(0.0, 100.0, 0.0)), Vec3::new(20.0, 100.0, 0.0)), "{:?}", place(&s, "CarB", Vec3::new(0.0, 100.0, 0.0)));
|
||||
}
|
||||
|
||||
/// A clamped link can jump: a 100:1 link to a rocker limited to a full turn reaches its limit
|
||||
/// after the dial's first few degrees, back where it started. Each step is checked, so the drive
|
||||
/// still stops where the rocker can't go.
|
||||
#[test]
|
||||
fn a_clamped_link_is_followed_step_by_step() {
|
||||
let (mut s, _, _) = four_bar();
|
||||
part(&mut s, "Dial", [80.0, 0.0, -5.0]);
|
||||
pin(&mut s, "revolute", "Dial", "Ground", "Dial", Vec3::new(80.0, 0.0, 0.0), Vec3::Z);
|
||||
run(&mut s, "joint.limits", json!({"joint": "Rocker", "min": 0, "max": 360}));
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Dial", "b": "Rocker", "ratio": 100}));
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Dial", "value": 360}));
|
||||
assert!(r["stopped_by_loop"].is_object(), "{r}");
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
}
|
||||
@@ -1,5 +1,7 @@
|
||||
//! Contact Sets, Motion Study and Exploded views.
|
||||
|
||||
use std::collections::BTreeSet;
|
||||
|
||||
use serde_json::{Value, json};
|
||||
use solidcraft_doc::Mat;
|
||||
use solidcraft_doc::joints::{self, JointKind};
|
||||
@@ -8,7 +10,7 @@ use solidcraft_geom::Vec3;
|
||||
use solidcraft_kernel::Body;
|
||||
|
||||
use super::CommandSpec;
|
||||
use super::joints::{joint_id, occurrence_id, resolve, value};
|
||||
use super::joints::{joint_id, occurrence_id, resolve_holding, value};
|
||||
use crate::params::{bad, bool_, str_, vec3};
|
||||
use crate::{EngineError, Result, Session};
|
||||
|
||||
@@ -254,27 +256,41 @@ fn closest(p: Vec3, a: Vec3, b: Vec3, c: Vec3) -> Vec3 {
|
||||
a + ab * (vb / den) + ac * (vc / den)
|
||||
}
|
||||
|
||||
/// Set joint values, stopping short where contact sets would collide: the values are moved
|
||||
/// from where they are toward `want` as far as they go without a collision.
|
||||
fn drive_with_contact(s: &mut Session, want: &[(u64, Vec<f64>)]) -> Result<Value> {
|
||||
/// Set joint values, stopping short where contact sets would collide or a closed loop could no
|
||||
/// longer close: the values are moved from where they are toward `want` as far as they go. The
|
||||
/// `given` values (joint, index) are driven; in a closed loop the others follow.
|
||||
fn drive_with_contact(s: &mut Session, want: &[(u64, Vec<f64>)], given: &BTreeSet<(u64, usize)>) -> Result<Value> {
|
||||
let baseline = s.doc.clone();
|
||||
let start: Vec<(u64, Vec<f64>)> =
|
||||
want.iter().map(|(id, _)| (*id, s.doc.assembly.joints.iter().find(|j| j.id == *id).map(|j| j.values.clone()).unwrap_or_default())).collect();
|
||||
// Closed loops follow the driven values from where they start.
|
||||
let held: joints::Held = given
|
||||
.iter()
|
||||
.filter(|&&(id, i)| want.iter().any(|(w, q)| *w == id && i < q.len()))
|
||||
.map(|&(id, i)| {
|
||||
// A value the file leaves out is 0, as the solver reads it.
|
||||
let from = start.iter().find(|(s, _)| *s == id).and_then(|(_, q)| q.get(i).copied());
|
||||
((id, i), from.unwrap_or(0.0))
|
||||
})
|
||||
.collect();
|
||||
let set = |s: &mut Session, t: f64| {
|
||||
// Trial order must not affect free anchors or linked joint placements.
|
||||
s.doc = baseline.clone();
|
||||
let doc = s.doc_mut();
|
||||
for ((id, a), (_, b)) in start.iter().zip(want) {
|
||||
if let Some(j) = doc.assembly.joints.iter_mut().find(|j| j.id == *id) {
|
||||
let mut q: Vec<f64> = b.iter().enumerate().map(|(i, bv)| a.get(i).copied().unwrap_or(*bv) * (1.0 - t) + bv * t).collect();
|
||||
let mut q: Vec<f64> = b.iter().enumerate().map(|(i, bv)| a.get(i).copied().unwrap_or(0.0) * (1.0 - t) + bv * t).collect();
|
||||
joints::clamp(j, &mut q);
|
||||
j.values = q;
|
||||
}
|
||||
}
|
||||
resolve(s)
|
||||
resolve_holding(s, &held)
|
||||
};
|
||||
// Loops already open at the start stay as they are; a loop the motion opens stops it.
|
||||
let open_at_start: BTreeSet<u64> = set(s, 0.0).conflicts.iter().map(|c| c.0).collect();
|
||||
let opened = |sol: &joints::Solution| sol.conflicts.iter().find(|c| !open_at_start.contains(&c.0)).map(|c| c.1.clone());
|
||||
let cache = if s.doc.assembly.contact_enabled { Some(contact_sweep::ContactGeometry::new(s)?) } else { None };
|
||||
let sol;
|
||||
let mut sol;
|
||||
let mut stopped = None;
|
||||
if let Some(cache) = cache.filter(|cache| cache.has_pairs()) {
|
||||
let mut end = s.scratch();
|
||||
@@ -313,16 +329,33 @@ fn drive_with_contact(s: &mut Session, want: &[(u64, Vec<f64>)]) -> Result<Value
|
||||
} else {
|
||||
sol = set(s, 1.0);
|
||||
}
|
||||
let mut stopped_by_loop = None;
|
||||
if let Some(mut name) = opened(&sol) {
|
||||
// The furthest the loops still close, short of where contact stopped; the joint named is
|
||||
// the one whose loop opens first.
|
||||
let (mut lo, mut hi) = (0.0, stopped.as_ref().and_then(|v| v["fraction"].as_f64()).unwrap_or(1.0));
|
||||
for _ in 0..14 {
|
||||
let mid = (lo + hi) / 2.0;
|
||||
match opened(&set(s, mid)) {
|
||||
None => lo = mid,
|
||||
Some(n) => (hi, name) = (mid, n),
|
||||
}
|
||||
}
|
||||
sol = set(s, lo);
|
||||
stopped = None;
|
||||
stopped_by_loop = Some(json!({"joint": name, "fraction": lo}));
|
||||
}
|
||||
let values: Vec<Value> = want
|
||||
.iter()
|
||||
.map(|(id, _)| json!({"joint": id, "values": s.doc.assembly.joints.iter().find(|j| j.id == *id).map(|j| j.values.clone())}))
|
||||
.collect();
|
||||
Ok(json!({"values": values, "stopped_by_contact": stopped, "conflicts": sol.conflicts.len()}))
|
||||
Ok(json!({"values": values, "stopped_by_contact": stopped, "stopped_by_loop": stopped_by_loop, "conflicts": sol.conflicts.len()}))
|
||||
}
|
||||
|
||||
/// Drive one joint (joint.drive), honouring contact sets.
|
||||
pub fn drive_joint(s: &mut Session, id: u64, q: Vec<f64>) -> Result<Value> {
|
||||
drive_with_contact(s, &[(id, q)])
|
||||
pub fn drive_joint(s: &mut Session, id: u64, q: Vec<f64>, given: usize) -> Result<Value> {
|
||||
let given = (0..given.min(q.len())).map(|i| (id, i)).collect();
|
||||
drive_with_contact(s, &[(id, q)], &given)
|
||||
}
|
||||
|
||||
fn enable_sets(s: &mut Session, p: &Value) -> Result<Value> {
|
||||
@@ -517,6 +550,11 @@ fn values_at(s: &Session, m: &MotionStudy, step: f64) -> Vec<(u64, Vec<f64>)> {
|
||||
out
|
||||
}
|
||||
|
||||
/// The values a study keys: (joint, index).
|
||||
fn keyed(m: &MotionStudy) -> BTreeSet<(u64, usize)> {
|
||||
m.keys.iter().map(|k| (k.joint, k.index)).collect()
|
||||
}
|
||||
|
||||
/// Where each occurrence is: (name, world transform).
|
||||
fn positions(s: &Session) -> Vec<(String, Mat)> {
|
||||
s.doc.occurrences.iter().map(|o| (o.name.clone(), joints::occurrence_world(&s.doc, o.id))).collect()
|
||||
@@ -532,7 +570,7 @@ fn play(s: &mut Session, p: &Value) -> Result<Value> {
|
||||
let m = s.doc.assembly.studies.get(i).cloned().ok_or_else(|| bad(cmd, "study"))?;
|
||||
let step = p.get("step").and_then(Value::as_f64).filter(|x| x.is_finite()).ok_or_else(|| bad(cmd, "`step` must be a number"))?;
|
||||
let want = values_at(s, &m, step.clamp(0.0, f64::from(m.steps)));
|
||||
let mut out = drive_with_contact(s, &want)?;
|
||||
let mut out = drive_with_contact(s, &want, &keyed(&m))?;
|
||||
out["positions"] = positions(s).iter().map(|(n, t)| json!({"occurrence": n, "pose": pose_json(t)})).collect();
|
||||
Ok(out)
|
||||
}
|
||||
@@ -551,10 +589,11 @@ fn export(s: &mut Session, p: &Value) -> Result<Value> {
|
||||
for k in 0..samples {
|
||||
let step = f64::from(m.steps) * k as f64 / (samples - 1) as f64;
|
||||
let want = values_at(&t, &m, step);
|
||||
let r = drive_with_contact(&mut t, &want)?;
|
||||
let r = drive_with_contact(&mut t, &want, &keyed(&m))?;
|
||||
rows.push(json!({
|
||||
"step": step,
|
||||
"stopped_by_contact": r["stopped_by_contact"],
|
||||
"stopped_by_loop": r["stopped_by_loop"],
|
||||
"positions": positions(&t).iter().map(|(n, x)| json!({"occurrence": n, "pose": pose_json(x)})).collect::<Vec<_>>(),
|
||||
}));
|
||||
}
|
||||
|
||||
@@ -22,7 +22,7 @@ mod decompose;
|
||||
mod dims;
|
||||
mod fonts;
|
||||
mod freeform_offset;
|
||||
mod linalg;
|
||||
pub mod linalg;
|
||||
mod link;
|
||||
mod loci;
|
||||
mod model;
|
||||
|
||||
Reference in new issue
Block a user