Joints: joint.reach (inverse kinematics), and a jointed part follows a drag (#1540)
Thanks @SychPL.
This commit is contained in:
Mateusz Sych authored and GitHub committed
1 parent
baf44657c1
commit
4d862cd76c
21 files changed
+1209
-48
No files matched your search
+62
-38
@@ -1211,8 +1211,61 @@ pub fn solve(doc: &Document, st: &ModelState) -> Solution {
|
||||
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 mut vals = joint_values(asm);
|
||||
let Built { tree, mut placed, closing, mut dof } = build(doc, st, &vals, &mut sol);
|
||||
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), 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()));
|
||||
}
|
||||
}
|
||||
if !closing.is_empty() {
|
||||
loop_dof(&tree, &closing, asm, &vals, &mut dof);
|
||||
}
|
||||
for o in &doc.occurrences {
|
||||
dof.entry(o.id).or_insert(6);
|
||||
}
|
||||
placed.remove(&0);
|
||||
sol.world = placed;
|
||||
sol.dof = dof;
|
||||
sol
|
||||
}
|
||||
|
||||
/// The joint graph laid out for a solve: the tree, where it places each occurrence for the
|
||||
/// values it was built with, the joints that close loops, and each occurrence's degrees of
|
||||
/// freedom along the tree.
|
||||
struct Built<'a> {
|
||||
tree: Tree<'a>,
|
||||
placed: BTreeMap<u64, Mat>,
|
||||
closing: Vec<u64>,
|
||||
dof: BTreeMap<u64, usize>,
|
||||
}
|
||||
|
||||
/// Lay out the joint graph: the spanning tree out from the fixed occurrences (the root, grounded
|
||||
/// ones; with neither involved, the first joint's A side) and the joints left over, which close
|
||||
/// loops. Joints whose origins can't be resolved go to `sol.errors`; `sol.fixed` is set.
|
||||
fn build<'a>(doc: &'a Document, st: &ModelState, vals: &Values, sol: &mut Solution) -> Built<'a> {
|
||||
let asm = &doc.assembly;
|
||||
let joints: Vec<&Joint> = asm.joints.iter().filter(|j| !j.suppressed).take(MAX_JOINTS).collect();
|
||||
// Joint frames in component coordinates (errors leave the joint out).
|
||||
let mut frames: BTreeMap<u64, (Mat, Mat)> = BTreeMap::new();
|
||||
for j in &joints {
|
||||
@@ -1270,7 +1323,7 @@ pub fn solve_holding(doc: &Document, st: &ModelState, held: &Held) -> Solution {
|
||||
if placed.contains_key(&other) {
|
||||
continue;
|
||||
}
|
||||
let Some(m) = tree.step(j.id, from_a, &t, &vals) 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));
|
||||
@@ -1279,48 +1332,15 @@ pub fn solve_holding(doc: &Document, st: &ModelState, held: &Held) -> Solution {
|
||||
queue.push_back(other);
|
||||
}
|
||||
}
|
||||
// Joints that close loops: move the loops' values to meet them, then check they agree.
|
||||
// Joints that close loops: the solve moves the loops' values to meet them, then checks 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), 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()));
|
||||
}
|
||||
}
|
||||
if !closing.is_empty() {
|
||||
loop_dof(&tree, &closing, asm, &vals, &mut dof);
|
||||
}
|
||||
for o in &doc.occurrences {
|
||||
dof.entry(o.id).or_insert(6);
|
||||
}
|
||||
placed.remove(&0);
|
||||
sol.world = placed;
|
||||
sol.dof = dof;
|
||||
sol
|
||||
Built { tree, placed, closing, dof }
|
||||
}
|
||||
|
||||
impl Document {
|
||||
@@ -1368,6 +1388,10 @@ impl Document {
|
||||
}
|
||||
}
|
||||
|
||||
#[path = "joints_reach.rs"]
|
||||
mod reach;
|
||||
pub use reach::{REACH_TOL, Reach, Reached, reach};
|
||||
|
||||
/// The frame matrix for an explicit origin / z / x (public for commands).
|
||||
pub fn frame_of(o: Vec3, z: Vec3, x: Option<Vec3>) -> Option<Mat> {
|
||||
frame(o, z, x)
|
||||
|
||||
@@ -0,0 +1,445 @@
|
||||
//! Inverse kinematics (`joint.reach`): move the values of the joints on an occurrence's chain so a
|
||||
//! point of it reaches a target (and an axis of it points a given way), every value within its
|
||||
//! limits, motion links followed, and the closed loops those values are in kept closed.
|
||||
|
||||
use std::cell::Cell;
|
||||
use std::collections::BTreeSet;
|
||||
|
||||
use solidcraft_geom::Vec3;
|
||||
use solidcraft_sketch::linalg;
|
||||
|
||||
use super::{
|
||||
Assembly, Held, MAX_LOOP_VALUES, MAX_LOOP_WORK, MAX_LOOPS, Solution, Tree, Values, apply_links, build, closure_gaps, jacobian, joint_values,
|
||||
largest, least_squares, limit_bounds, link_chains, loop_groups, loop_scale, unknowns,
|
||||
};
|
||||
use crate::{Document, ModelState, apply_point, apply_vector, mat_mul};
|
||||
|
||||
/// How close counts as reached: mm for the point, degrees for the axis.
|
||||
pub const REACH_TOL: f64 = 1e-4;
|
||||
|
||||
/// What a reach asks.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct Reach {
|
||||
/// The occurrence that reaches.
|
||||
pub occurrence: u64,
|
||||
/// Its point that goes to `to`, in its own (component) coordinates.
|
||||
pub point: Vec3,
|
||||
/// Where the point goes (world).
|
||||
pub to: Vec3,
|
||||
/// A unit axis of it (its own coordinates) and the unit world direction it turns to.
|
||||
pub axis: Option<(Vec3, Vec3)>,
|
||||
/// The joints whose values move toward the target (`None`: every joint on its chain). The
|
||||
/// other joints of a closed loop they are in follow.
|
||||
pub joints: Option<BTreeSet<u64>>,
|
||||
}
|
||||
|
||||
/// How a reach ended.
|
||||
#[derive(Clone, Debug, Default)]
|
||||
pub struct Reached {
|
||||
/// The new stored values of the joints that moved (joint id → values).
|
||||
pub values: Values,
|
||||
/// Short of the target: the values it ended on a limit of, with the target pulling past it
|
||||
/// (joint, value index, the upper limit).
|
||||
pub limits: Vec<(u64, usize, bool)>,
|
||||
/// Short of the target: the closing joint of a loop that stopped it (the chain alone, the
|
||||
/// loop left open, gets closer).
|
||||
pub stopped_by_loop: Option<u64>,
|
||||
}
|
||||
|
||||
/// How much a loop's gap (mm) outweighs a miss of the target (mm) in a step: enough to keep the
|
||||
/// step along the mechanism, so putting it back on the loops moves it little.
|
||||
const LOOP_WEIGHT: f64 = 1e3;
|
||||
/// Steps a reach takes at most, each moving no value more than 10° or a tenth of the chain's
|
||||
/// size (so it follows the mechanism's branch).
|
||||
const MAX_REACH_STEPS: usize = 200;
|
||||
const STEP_ANGLE: f64 = 10.0;
|
||||
const STEP_SHARE: f64 = 0.1;
|
||||
|
||||
/// Move the values of the joints on `r.occurrence`'s chain (those in `r.joints`, when given) so
|
||||
/// its point gets to `r.to`, or as close as the limits and closed loops let it: the nearest pose,
|
||||
/// found from where it is (it stays on the mechanism's branch). Values a motion link sets move
|
||||
/// through the start of their link chain; the other values of the loops they are in follow. An
|
||||
/// error says why nothing can move.
|
||||
pub fn reach(doc: &Document, st: &ModelState, r: &Reach) -> Result<Reached, String> {
|
||||
let asm = &doc.assembly;
|
||||
let vals = joint_values(asm);
|
||||
let built = build(doc, st, &vals, &mut Solution::default());
|
||||
let tree = &built.tree;
|
||||
if tree.fixed.contains_key(&r.occurrence) {
|
||||
return Err("it is fixed in place (grounded, or the part its joints are placed from)".into());
|
||||
}
|
||||
if !tree.parent.contains_key(&r.occurrence) {
|
||||
return Err("no joint places it: it is not connected to anything grounded (ground a part, or join it to a grounded part)".into());
|
||||
}
|
||||
let linked: BTreeSet<(u64, usize)> = asm.links.iter().map(|l| (l.b, l.ib)).collect();
|
||||
let allowed = |id: &u64| r.joints.as_ref().is_none_or(|js| js.contains(id));
|
||||
let path: BTreeSet<u64> = tree.path(r.occurrence).1.iter().map(|(k, _)| *k).collect();
|
||||
// The values that move it: those of the joints on its chain (the ones given), a linked one
|
||||
// through the start of its link chain (given or not), and the starts given of the chain's
|
||||
// other linked values.
|
||||
let chosen: BTreeSet<u64> = path.iter().copied().filter(allowed).collect();
|
||||
let mut drive: Vec<(u64, usize)> = Vec::new();
|
||||
for j in chosen.iter().filter_map(|id| tree.joints.get(id)) {
|
||||
drive.extend((0..j.kind.dofs().len()).map(|i| (j.id, i)).filter(|v| !linked.contains(v)));
|
||||
}
|
||||
let starts = link_chains(asm, tree, &path, &linked).1.into_iter().filter(|v| allowed(&v.0));
|
||||
for v in link_chains(asm, tree, &chosen, &linked).1.into_iter().chain(starts) {
|
||||
if !drive.contains(&v) {
|
||||
drive.push(v);
|
||||
}
|
||||
}
|
||||
if drive.is_empty() {
|
||||
return Err(if r.joints.is_some() {
|
||||
"none of the joints given moves it"
|
||||
} else {
|
||||
"no joint on its chain moves (rigid, or set by motion links)"
|
||||
}
|
||||
.into());
|
||||
}
|
||||
// The loops those values are in, and the values of theirs that follow.
|
||||
if built.closing.len() > MAX_LOOPS {
|
||||
return Err(format!("the design has more than {MAX_LOOPS} closed loops"));
|
||||
}
|
||||
let groups = loop_groups(tree, &built.closing, asm, &linked);
|
||||
let (mut vars, mut closers, mut around) = (drive.clone(), Vec::new(), path.clone());
|
||||
let mut taken = BTreeSet::new();
|
||||
while vars.len() <= MAX_LOOP_VALUES {
|
||||
let more: Vec<usize> =
|
||||
groups.iter().enumerate().filter(|(g, (_, _, t))| !taken.contains(g) && t.iter().any(|v| vars.contains(v))).map(|(g, _)| g).collect();
|
||||
if more.is_empty() {
|
||||
break;
|
||||
}
|
||||
for (c, a, _) in more.iter().filter_map(|g| groups.get(*g)) {
|
||||
closers.extend(c.iter().copied());
|
||||
around.extend(a.iter().copied());
|
||||
for v in unknowns(tree, a, asm, &linked, &Held::new()) {
|
||||
if !vars.contains(&v) {
|
||||
vars.push(v);
|
||||
}
|
||||
}
|
||||
}
|
||||
taken.extend(more);
|
||||
}
|
||||
if vars.len() > MAX_LOOP_VALUES {
|
||||
return Err(format!("its chain and loops have more than {MAX_LOOP_VALUES} values"));
|
||||
}
|
||||
let scale = loop_scale(tree, &around, &vals);
|
||||
let bounds = limit_bounds(tree, &vars);
|
||||
let x0: Vec<f64> = vars.iter().map(|(id, i)| vals.get(id).and_then(|q| q.get(*i)).copied().unwrap_or(0.0)).collect();
|
||||
// A distance steps by a share of the chain's size, of the way to the target or of the point's
|
||||
// reach from its joints, whichever is longest (one slider on its own has no size, and a part
|
||||
// turning about a joint far from its point moves that far).
|
||||
let point = tree.world(r.occurrence, &vals).map(|t| apply_point(&t, r.point));
|
||||
let lever = path
|
||||
.iter()
|
||||
.filter_map(|id| {
|
||||
let (j, (fa, _)) = (tree.joints.get(id)?, tree.frames.get(id)?);
|
||||
Some(apply_point(&mat_mul(&tree.world(j.a.occurrence, &vals)?, fa), Vec3::ZERO).dist(point?))
|
||||
})
|
||||
.fold(0.0, f64::max);
|
||||
let span = point.map_or(scale, |p| scale.max(p.dist(r.to)).max(lever));
|
||||
let cap: Vec<f64> = vars
|
||||
.iter()
|
||||
.map(|(id, i)| if tree.joints.get(id).is_some_and(|j| j.kind.is_angle(*i)) { STEP_ANGLE.to_radians() } else { STEP_SHARE * span })
|
||||
.collect();
|
||||
// Every value around the chain and its loops, links included, moves at most a step at a time:
|
||||
// a value a link sets can turn far more than the one that drives it, and a loop must stay
|
||||
// closed on the way, as joint.drive steps its loops.
|
||||
let mut paced: Vec<((u64, usize), f64)> = Vec::new();
|
||||
for j in around.iter().filter_map(|id| tree.joints.get(id)) {
|
||||
for i in 0..j.kind.dofs().len() {
|
||||
paced.push(((j.id, i), if j.kind.is_angle(i) { STEP_ANGLE.to_radians() } else { STEP_SHARE * span }));
|
||||
}
|
||||
}
|
||||
let tol = (1e-10 * scale).min(1e-7);
|
||||
let mt = if r.axis.is_some() { 6 } else { 3 };
|
||||
let spent = Cell::new(0usize);
|
||||
let mut work = vals.clone();
|
||||
let mut f = |x: &[f64]| misses(tree, asm, r, scale, &vars, &closers, x, &mut work, &spent);
|
||||
let mut x = x0.clone();
|
||||
let miss = |res: &[f64]| res.iter().take(mt).map(|v| v * v).sum::<f64>().sqrt();
|
||||
let before = f(&x0).map_or(f64::INFINITY, |r| miss(&r));
|
||||
let res = approach(&mut f, &|a, b| stride(asm, &vals, &vars, &paced, a, b), mt, &mut x, &bounds, &cap, tol)
|
||||
.ok_or("a closed loop on its chain is open and can't close (joint.list shows its conflict)")?;
|
||||
let mut out = Reached::default();
|
||||
let point_miss = res.iter().take(3).map(|v| v * v).sum::<f64>().sqrt();
|
||||
let turn_miss = (res.iter().skip(3).take(mt - 3).map(|v| v * v).sum::<f64>().sqrt() / scale).to_degrees();
|
||||
if point_miss > REACH_TOL || turn_miss > REACH_TOL {
|
||||
// The limits it rests on: the target pulls the value past them (−Jᵀr over the target's
|
||||
// rows, ignoring pulls at right angles to the miss: under a millionth of the value's own
|
||||
// reach times the miss).
|
||||
let n = x.len();
|
||||
if let Some(jac) = jacobian(&mut f, &x, res.len()) {
|
||||
for (k, (((id, i), v), (lo, hi))) in vars.iter().zip(&x).zip(&bounds).enumerate() {
|
||||
let column = |row: usize| jac.get(row * n + k).copied().unwrap_or(0.0);
|
||||
let pull: f64 = -(0..mt).map(|row| column(row) * res.get(row).copied().unwrap_or(0.0)).sum::<f64>();
|
||||
let eps = 1e-6 * (0..mt).map(|row| column(row).powi(2)).sum::<f64>().sqrt() * miss(&res);
|
||||
let at = |b: f64| b.is_finite() && (v - b).abs() <= 1e-9 * b.abs().max(1.0);
|
||||
if at(*hi) && pull > eps {
|
||||
out.limits.push((*id, *i, true));
|
||||
} else if at(*lo) && pull < -eps {
|
||||
out.limits.push((*id, *i, false));
|
||||
}
|
||||
}
|
||||
}
|
||||
// A value a motion link sets, held at its own limit (the link clamps it there): the
|
||||
// target's pull on it as if it were free.
|
||||
if f(&x).is_some() {
|
||||
for (id, i) in linked.iter().filter(|(id, _)| around.contains(id)) {
|
||||
let (Some(v), Some((lo, hi))) = (work.get(id).and_then(|q| q.get(*i)).copied(), limit_bounds(tree, &[(*id, *i)]).first().copied())
|
||||
else {
|
||||
continue;
|
||||
};
|
||||
let at = |b: f64| b.is_finite() && (v - b).abs() <= 1e-9 * b.abs().max(1.0);
|
||||
if !at(lo) && !at(hi) {
|
||||
continue;
|
||||
}
|
||||
let nudged = |dv: f64| {
|
||||
let mut w = work.clone();
|
||||
if let Some(slot) = w.get_mut(id).and_then(|q| q.get_mut(*i)) {
|
||||
*slot = v + dv;
|
||||
}
|
||||
target(tree, r, scale, &w)
|
||||
};
|
||||
let (Some(up), Some(down)) = (nudged(1e-6), nudged(-1e-6)) else { continue };
|
||||
let pull: f64 = -up.iter().zip(&down).zip(&res).map(|((a, b), r)| (a - b) / 2e-6 * r).sum::<f64>();
|
||||
let eps = 1e-6 * up.iter().zip(&down).map(|(a, b)| ((a - b) / 2e-6).powi(2)).sum::<f64>().sqrt() * miss(&res);
|
||||
if at(hi) && pull > eps {
|
||||
out.limits.push((*id, *i, true));
|
||||
} else if at(lo) && pull < -eps {
|
||||
out.limits.push((*id, *i, false));
|
||||
}
|
||||
}
|
||||
}
|
||||
// A loop stopped it when the chain alone gets closer.
|
||||
// ponytail: names the first loop of several; trying each one open would name the one that binds.
|
||||
if let Some(first) = closers.first() {
|
||||
let k = drive.len();
|
||||
let mut alone = vals.clone();
|
||||
let mut g = |x: &[f64]| misses(tree, asm, r, scale, &drive, &[], x, &mut alone, &spent);
|
||||
let (mut xo, bo, co) = (x0.get(..k).unwrap_or_default().to_vec(), bounds.get(..k).unwrap_or_default(), cap.get(..k).unwrap_or_default());
|
||||
// The loops left open, nothing to pace.
|
||||
if let Some(ro) = approach(&mut g, &|_, _| 0.0, mt, &mut xo, bo, co, tol)
|
||||
&& miss(&ro) < miss(&res) - REACH_TOL.max(1e-9 * miss(&res))
|
||||
{
|
||||
out.stopped_by_loop = Some(*first);
|
||||
}
|
||||
}
|
||||
}
|
||||
// The stored values that changed, when that got it closer (by more than the solver's noise:
|
||||
// a held part doesn't creep).
|
||||
let gained = before - miss(&res) > 1e-7;
|
||||
for ((id, i), v) in vars.iter().zip(&x).filter(|_| gained) {
|
||||
let Some(j) = tree.joints.get(id) else { continue };
|
||||
if j.values.get(*i) == Some(v) {
|
||||
continue;
|
||||
}
|
||||
let q = out.values.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;
|
||||
}
|
||||
}
|
||||
Ok(out)
|
||||
}
|
||||
|
||||
/// The target's misses with the values `vars` at `x`: the point's (mm), then the axis's (as mm
|
||||
/// across the chain's `scale`), then the gaps of the loops `closers`, weighted.
|
||||
fn misses(
|
||||
tree: &Tree,
|
||||
asm: &Assembly,
|
||||
r: &Reach,
|
||||
scale: f64,
|
||||
vars: &[(u64, usize)],
|
||||
closers: &[u64],
|
||||
x: &[f64],
|
||||
work: &mut Values,
|
||||
spent: &Cell<usize>,
|
||||
) -> Option<Vec<f64>> {
|
||||
spent.set(spent.get() + 1);
|
||||
if spent.get() > MAX_LOOP_WORK {
|
||||
return None;
|
||||
}
|
||||
for ((id, i), v) in vars.iter().zip(x) {
|
||||
if let Some(slot) = work.get_mut(id).and_then(|q| q.get_mut(*i)) {
|
||||
*slot = *v;
|
||||
}
|
||||
}
|
||||
apply_links(asm, work);
|
||||
let mut out = target(tree, r, scale, work)?;
|
||||
let n = out.len();
|
||||
closure_gaps(tree, closers, work, scale, &mut out)?;
|
||||
for v in out.iter_mut().skip(n) {
|
||||
*v *= LOOP_WEIGHT;
|
||||
}
|
||||
out.iter().all(|v| v.is_finite()).then_some(out)
|
||||
}
|
||||
|
||||
/// The target's misses for these values: the point's (mm), then the axis's (as mm across `scale`).
|
||||
fn target(tree: &Tree, r: &Reach, scale: f64, vals: &Values) -> Option<Vec<f64>> {
|
||||
let t = tree.world(r.occurrence, vals)?;
|
||||
let d = apply_point(&t, r.point) - r.to;
|
||||
let mut out = vec![d.x, d.y, d.z];
|
||||
if let Some((a, z)) = r.axis {
|
||||
let w = (apply_vector(&t, a) - z) * scale;
|
||||
out.extend([w.x, w.y, w.z]);
|
||||
}
|
||||
Some(out)
|
||||
}
|
||||
|
||||
/// How many steps apart the values `vars` at `a` and at `b` put the `paced` values (each with its
|
||||
/// step), motion links followed.
|
||||
fn stride(asm: &Assembly, vals: &Values, vars: &[(u64, usize)], paced: &[((u64, usize), f64)], a: &[f64], b: &[f64]) -> f64 {
|
||||
let at = |x: &[f64]| {
|
||||
let mut w = vals.clone();
|
||||
for ((id, i), v) in vars.iter().zip(x) {
|
||||
if let Some(slot) = w.get_mut(id).and_then(|q| q.get_mut(*i)) {
|
||||
*slot = *v;
|
||||
}
|
||||
}
|
||||
apply_links(asm, &mut w);
|
||||
w
|
||||
};
|
||||
let (wa, wb) = (at(a), at(b));
|
||||
let get = |w: &Values, id: &u64, i: usize| w.get(id).and_then(|q| q.get(i)).copied().unwrap_or(0.0);
|
||||
paced.iter().map(|((id, i), step)| (get(&wb, id, *i) - get(&wa, id, *i)).abs() / step).fold(0.0, f64::max)
|
||||
}
|
||||
|
||||
/// Levenberg-Marquardt that stays on the loops: move `x` (within `bounds`, at most `cap` per value
|
||||
/// and a step of every value `stride` measures, links included, a step) to bring the first `mt` values of `f` (the target's misses) toward zero, the rest (the
|
||||
/// loops' weighted gaps) at zero. Each step is solved over all the rows, values on a limit the
|
||||
/// step pushes past held, then put back on the loops (`least_squares` on their gaps) and taken
|
||||
/// when it gets closer. It ends at the target, or as close as the limits and loops let it get.
|
||||
/// Returns `f` there, or `None` when the loops don't close at the start.
|
||||
fn approach(
|
||||
f: &mut dyn FnMut(&[f64]) -> Option<Vec<f64>>,
|
||||
stride: &dyn Fn(&[f64], &[f64]) -> f64,
|
||||
mt: usize,
|
||||
x: &mut Vec<f64>,
|
||||
bounds: &[(f64, f64)],
|
||||
cap: &[f64],
|
||||
tol: f64,
|
||||
) -> Option<Vec<f64>> {
|
||||
let n = x.len();
|
||||
let miss = |r: &[f64]| r.iter().take(mt).map(|v| v * v).sum::<f64>();
|
||||
let close = |f: &mut dyn FnMut(&[f64]) -> Option<Vec<f64>>, x: &mut Vec<f64>| {
|
||||
least_squares(&mut |y| f(y).map(|r| r.get(mt..).unwrap_or_default().to_vec()), x, bounds, tol * LOOP_WEIGHT)
|
||||
};
|
||||
let loops = f(x)?.len() > mt;
|
||||
if loops && !close(f, x) {
|
||||
return None;
|
||||
}
|
||||
let mut r = f(x)?;
|
||||
let (mut lambda, mut kicked) = (1e-3, false);
|
||||
for _ in 0..MAX_REACH_STEPS {
|
||||
if largest(r.get(..mt).unwrap_or_default()) <= tol {
|
||||
break;
|
||||
}
|
||||
let m = r.len();
|
||||
let Some(jac) = jacobian(f, x, m) else { break };
|
||||
// The normal equations JᵀJ δ = −Jᵀr, and the target rows' share of the diagonal (the
|
||||
// damping scales with it, so the loops' weight doesn't slow the step along them).
|
||||
let (mut jtj, mut g, mut diag) = (linalg::Mat::zeros(n, n), vec![0.0; n], vec![0.0; n]);
|
||||
for (row, ri) in r.iter().enumerate() {
|
||||
let a = jac.get(row * n..(row + 1) * n).unwrap_or_default();
|
||||
for (p, ap) in a.iter().enumerate().filter(|(_, v)| **v != 0.0) {
|
||||
if let Some(gp) = g.get_mut(p) {
|
||||
*gp -= ap * ri;
|
||||
}
|
||||
if row < mt
|
||||
&& let Some(dp) = diag.get_mut(p)
|
||||
{
|
||||
*dp += ap * ap;
|
||||
}
|
||||
for (q, aq) in a.iter().enumerate().skip(p) {
|
||||
jtj.add(p, q, ap * aq);
|
||||
}
|
||||
}
|
||||
}
|
||||
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 && !stepped {
|
||||
let mut held = vec![false; n];
|
||||
// Solved, then again with the values held that it pushes past their limits.
|
||||
for _ in 0..=n {
|
||||
let mut a = jtj.clone();
|
||||
let mut b = g.clone();
|
||||
for p in 0..n {
|
||||
a.add(p, p, lambda * (1.0 + diag.get(p).copied().unwrap_or(0.0)));
|
||||
if held.get(p).copied().unwrap_or(false) {
|
||||
for q in 0..n {
|
||||
a.set(p, q, if p == q { 1.0 } else { 0.0 });
|
||||
a.set(q, p, if p == q { 1.0 } else { 0.0 });
|
||||
}
|
||||
if let Some(bp) = b.get_mut(p) {
|
||||
*bp = 0.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
let Some(d) = linalg::solve(a, b) else { break };
|
||||
let past: Vec<bool> =
|
||||
x.iter().zip(&d).zip(bounds).map(|((v, dv), (lo, hi))| (*dv > 0.0 && *v >= *hi) || (*dv < 0.0 && *v <= *lo)).collect();
|
||||
if past.iter().zip(&held).any(|(p, h)| *p && !*h) {
|
||||
held = held.iter().zip(&past).map(|(h, p)| *h || *p).collect();
|
||||
continue;
|
||||
}
|
||||
let full: Vec<f64> = x.iter().zip(&d).map(|(v, dv)| v + dv).collect();
|
||||
let over = d.iter().zip(cap).map(|(d, c)| d.abs() / c).fold(1.0f64, f64::max).max(stride(x, &full));
|
||||
let mut next: Vec<f64> = x.iter().zip(&d).zip(bounds).map(|((v, dv), (lo, hi))| (v + dv / over).max(*lo).min(*hi)).collect();
|
||||
// Checked again once taken: a link held at a limit or the loops closed again may
|
||||
// move it further than foreseen (joint.drive halves such steps too).
|
||||
if (!loops || close(f, &mut next))
|
||||
&& stride(x, &next) <= 2.0
|
||||
&& let Some(rn) = f(&next)
|
||||
&& miss(&rn) < miss(&r)
|
||||
{
|
||||
(*x, r) = (next, rn);
|
||||
// A step cut down to the cap (as at a stretched, singular pose) damps the next one
|
||||
// more instead of less, so it doesn't crawl along its longest value.
|
||||
lambda = if over > 1.0 { (lambda * over).min(1e6) } else { (lambda / 3.0).max(1e-9) };
|
||||
stepped = true;
|
||||
}
|
||||
break;
|
||||
}
|
||||
if !stepped {
|
||||
lambda *= 4.0;
|
||||
}
|
||||
}
|
||||
if !stepped {
|
||||
// Where nothing changes the misses to first order but they aren't least (the target
|
||||
// straight across a pivot from the point), a small kick off it gets closer, once.
|
||||
if kicked {
|
||||
break;
|
||||
}
|
||||
kicked = true;
|
||||
// Both ways: one may run into a limit.
|
||||
for sign in [1.0, -1.0] {
|
||||
let mut next: Vec<f64> = x
|
||||
.iter()
|
||||
.zip(cap.iter().zip(bounds))
|
||||
.enumerate()
|
||||
.map(|(k, (v, (c, (lo, hi))))| (v + sign * 1e-3 * c * if k % 2 == 0 { 1.0 } else { -1.0 }).max(*lo).min(*hi))
|
||||
.collect();
|
||||
if (!loops || close(f, &mut next))
|
||||
&& stride(x, &next) <= 2.0
|
||||
&& let Some(rn) = f(&next)
|
||||
&& miss(&rn) < miss(&r)
|
||||
{
|
||||
(*x, r, lambda, stepped) = (next, rn, 1e-3, true);
|
||||
break;
|
||||
}
|
||||
}
|
||||
if !stepped {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
Some(r)
|
||||
}
|
||||
@@ -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;
|
||||
|
||||
@@ -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}");
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
|
||||
@@ -343,7 +343,7 @@ fn pt(v: Vec3) -> Value {
|
||||
}
|
||||
|
||||
/// The occurrence placing a body (0: the root design), when the body is its primary instance.
|
||||
fn occurrence_of_body(s: &Session, body: &str) -> Option<u64> {
|
||||
pub(crate) fn occurrence_of_body(s: &Session, body: &str) -> Option<u64> {
|
||||
let st = s.world_state();
|
||||
let b = st.body(body)?;
|
||||
let comp = s.doc.model_body_component(b);
|
||||
|
||||
@@ -0,0 +1,214 @@
|
||||
//! Dragging a part that joints place moves it as far as its joints allow: each frame runs
|
||||
//! `joint.reach`, taking the point grabbed toward the pointer in the plane through it that faces
|
||||
//! the view. The whole drag is one undo step.
|
||||
|
||||
use egui::Pos2;
|
||||
use serde_json::json;
|
||||
use solidcraft_engine::doc::{apply_point, joints, mat_inverse};
|
||||
use solidcraft_engine::geom::Vec3;
|
||||
|
||||
use crate::SolveApp;
|
||||
use crate::viewport::{Hit, Proj};
|
||||
|
||||
/// A drag under way.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct Drag {
|
||||
occurrence: u64,
|
||||
/// The point grabbed, in the occurrence's own coordinates.
|
||||
local: Vec3,
|
||||
/// The plane the pointer moves the point in: through where it was grabbed, facing the view.
|
||||
origin: Vec3,
|
||||
normal: Vec3,
|
||||
/// The drag's undo step is on top of the history (it went in with the first frame that moved).
|
||||
pushed: bool,
|
||||
}
|
||||
|
||||
/// Begin a drag on the part under `p` when joints place it and leave it free to move (it is
|
||||
/// neither grounded nor fixed to what is). True when one began.
|
||||
pub fn start(app: &mut SolveApp, proj: &Proj, p: Pos2) -> bool {
|
||||
// The face under the pointer, past the part's own edges and vertices (not past sketches or
|
||||
// construction geometry in front of it).
|
||||
let hits = crate::viewport::pick(app, proj, p);
|
||||
let Some((body, at)) =
|
||||
hits.into_iter().take_while(|h| matches!(h, Hit::Face { .. } | Hit::Edge { .. } | Hit::Vertex { .. })).find_map(|h| match h {
|
||||
Hit::Face { body, point, .. } => Some((body, point)),
|
||||
_ => None,
|
||||
})
|
||||
else {
|
||||
return false;
|
||||
};
|
||||
let s = &app.session;
|
||||
let Some(occurrence) = crate::dialogs_assembly::occurrence_of_body(s, &body).filter(|o| *o != 0) else { return false };
|
||||
if !s.doc.assembly.joints.iter().any(|j| !j.suppressed && (j.a.occurrence == occurrence || j.b.occurrence == occurrence)) {
|
||||
return false;
|
||||
}
|
||||
let sol = joints::solve(&s.doc, &s.model.state());
|
||||
if !sol.world.contains_key(&occurrence) || sol.dof.get(&occurrence).is_none_or(|d| *d == 0) {
|
||||
return false;
|
||||
}
|
||||
let Some(local) = mat_inverse(&joints::occurrence_world(&s.doc, occurrence)).map(|inv| apply_point(&inv, at)) else { return false };
|
||||
let normal = proj.ray(p).1;
|
||||
app.viewport.joint_drag = Some(Drag { occurrence, local, origin: at, normal, pushed: false });
|
||||
true
|
||||
}
|
||||
|
||||
pub fn active(app: &SolveApp) -> bool {
|
||||
app.viewport.joint_drag.is_some()
|
||||
}
|
||||
|
||||
/// Reach for the point under `p`.
|
||||
pub fn to(app: &mut SolveApp, proj: &Proj, p: Pos2) {
|
||||
let Some(d) = app.viewport.joint_drag.clone() else { return };
|
||||
let (o, dir) = proj.ray(p);
|
||||
let facing = dir.dot(d.normal);
|
||||
if facing.abs() < 1e-9 {
|
||||
return;
|
||||
}
|
||||
let target = o + dir * ((d.origin - o).dot(d.normal) / facing);
|
||||
if !target.is_finite() {
|
||||
return;
|
||||
}
|
||||
let at = apply_point(&joints::occurrence_world(&app.session.doc, d.occurrence), d.local);
|
||||
let params =
|
||||
json!({"occurrence": d.occurrence, "at": [at.x, at.y, at.z], "point": [target.x, target.y, target.z], "quiet": true, "continuous": true});
|
||||
// One undo step for the whole drag: the first frame's, kept on top while the others run (not
|
||||
// truncated by depth, which a full history would make lose its oldest steps frame by frame).
|
||||
let own = if d.pushed { app.session.undo.pop() } else { None };
|
||||
// A frame that changes the design adds an undo step (a full history drops its oldest for it,
|
||||
// so its length says nothing).
|
||||
let before = app.session.doc.clone();
|
||||
let pushed = app.run("joint.reach", params).is_ok() && !std::sync::Arc::ptr_eq(&before, &app.session.doc);
|
||||
match own {
|
||||
Some(step) => {
|
||||
if pushed {
|
||||
app.session.undo.pop();
|
||||
}
|
||||
app.session.undo.push(step);
|
||||
}
|
||||
None if pushed => {
|
||||
if let Some(d) = app.viewport.joint_drag.as_mut() {
|
||||
d.pushed = true;
|
||||
}
|
||||
}
|
||||
None => {}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn end(app: &mut SolveApp) {
|
||||
app.viewport.joint_drag = None;
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use serde_json::{Value, json};
|
||||
use solidcraft_engine::doc::{apply_point, joints};
|
||||
use solidcraft_engine::geom::Vec3;
|
||||
|
||||
use crate::scenario::Harness;
|
||||
|
||||
fn run(h: &mut Harness, id: &str, p: Value) -> Value {
|
||||
h.app.run(id, p.clone()).unwrap_or_else(|e| panic!("{id} {p}: {e}"))
|
||||
}
|
||||
|
||||
fn screen(h: &mut Harness, p: Vec3) -> egui::Pos2 {
|
||||
let [x, y] = h.at(&json!({"world": [p.x, p.y, p.z]})).expect("on screen");
|
||||
egui::pos2(x as f32, y as f32)
|
||||
}
|
||||
|
||||
/// Where the arm's point `local` (its own coordinates) is.
|
||||
fn arm_at(h: &Harness, local: Vec3) -> Vec3 {
|
||||
let s = &h.app.session;
|
||||
let c = s.doc.find_component("Arm").unwrap_or(0);
|
||||
let o = s.doc.occurrence_of(c).map(|o| o.id).unwrap_or(0);
|
||||
apply_point(&joints::occurrence_world(&s.doc, o), local)
|
||||
}
|
||||
|
||||
/// An arm hinged about +Z on a grounded base (seen from the top): its far end, and the screen
|
||||
/// points of that end and of the end turned a quarter round.
|
||||
fn hinged_arm() -> (Harness, Vec3, egui::Pos2, egui::Pos2) {
|
||||
let mut h = Harness::new();
|
||||
h.app.home.open = false;
|
||||
for (name, corner, size) in [("Base", [-5, -5, 0], [10, 10, 5]), ("Arm", [-3, -3, 5], [63, 6, 5])] {
|
||||
run(&mut h, "component.activate", json!({"component": "root"}));
|
||||
run(&mut h, "component.create", json!({"name": name}));
|
||||
run(&mut h, "solid.box", json!({"length": size[0], "width": size[1], "height": size[2], "corner": corner}));
|
||||
}
|
||||
run(&mut h, "component.activate", json!({"component": "root"}));
|
||||
let base = h.app.session.doc.find_component("Base").and_then(|c| h.app.session.doc.occurrence_of(c)).map(|o| o.id).unwrap_or(0);
|
||||
run(&mut h, "occurrence.ground", json!({"occurrence": base, "grounded": true}));
|
||||
let at = |o: &str| json!({"occurrence": o, "point": [0, 0, 5], "z": [0, 0, 1]});
|
||||
run(&mut h, "joint.create", json!({"type": "revolute", "a": at("Base"), "b": at("Arm"), "flip": true, "name": "Hinge"}));
|
||||
h.call("ui.view", json!({"view": "top"}));
|
||||
h.app.cam.distance = 200.0;
|
||||
h.frames(3);
|
||||
let end = Vec3::new(55.0, 0.0, 10.0);
|
||||
let (a, b) = (screen(&mut h, end), screen(&mut h, Vec3::new(0.0, 55.0, 10.0)));
|
||||
(h, end, a, b)
|
||||
}
|
||||
|
||||
fn play(h: &mut Harness) {
|
||||
while !h.app.synthetic.is_empty() {
|
||||
h.frame();
|
||||
}
|
||||
h.frames(2);
|
||||
}
|
||||
|
||||
/// Press at `a` and move to `b` in 12 frames, holding the button there.
|
||||
fn press_and_move(h: &mut Harness, a: egui::Pos2, b: egui::Pos2) {
|
||||
h.app.synthetic.push(egui::Event::PointerMoved(a));
|
||||
h.app.synthetic.push(egui::Event::PointerButton {
|
||||
pos: a,
|
||||
button: egui::PointerButton::Primary,
|
||||
pressed: true,
|
||||
modifiers: Default::default(),
|
||||
});
|
||||
h.app.synthetic.extend((1..=12).map(|k| egui::Event::PointerMoved(a + (b - a) * (k as f32 / 12.0))));
|
||||
play(h);
|
||||
}
|
||||
|
||||
fn release(h: &mut Harness, b: egui::Pos2) {
|
||||
h.app.synthetic.push(egui::Event::PointerButton {
|
||||
pos: b,
|
||||
button: egui::PointerButton::Primary,
|
||||
pressed: false,
|
||||
modifiers: Default::default(),
|
||||
});
|
||||
play(h);
|
||||
}
|
||||
|
||||
/// Dragged by its far end a quarter turn round, the arm turns there in one undo step, and undo
|
||||
/// turns it back.
|
||||
#[test]
|
||||
fn dragging_a_jointed_part_turns_it_about_its_hinge() {
|
||||
let (mut h, end, a, b) = hinged_arm();
|
||||
let undo = h.app.session.undo.len();
|
||||
press_and_move(&mut h, a, b);
|
||||
assert!(h.app.viewport.joint_drag.is_some() && h.app.viewport.boxsel.is_none(), "the drag moves the arm, not a selection box");
|
||||
release(&mut h, b);
|
||||
assert!(h.app.viewport.joint_drag.is_none());
|
||||
assert!(arm_at(&h, end).dist(Vec3::new(0.0, 55.0, 10.0)) < 0.5, "{:?}", arm_at(&h, end));
|
||||
assert_eq!(h.app.session.undo.len(), undo + 1, "one undo step for the drag");
|
||||
assert!(!h.app.session.log.iter().any(|l| l.contains("Reach")), "frames are not echoed: {:?}", h.app.session.log);
|
||||
run(&mut h, "edit.undo", json!({}));
|
||||
assert!(arm_at(&h, end).dist(end) < 1e-9, "{:?}", arm_at(&h, end));
|
||||
}
|
||||
|
||||
/// With the undo history full, a drag still adds one step: it drops the oldest once, not one
|
||||
/// per frame.
|
||||
#[test]
|
||||
fn a_drag_on_a_full_undo_history_drops_one_step() {
|
||||
let (mut h, _, a, b) = hinged_arm();
|
||||
let step = h.app.session.undo.last().cloned().expect("an undo step");
|
||||
while h.app.session.undo.len() < 200 {
|
||||
let mut older = step.clone();
|
||||
older.label = format!("step {}", h.app.session.undo.len());
|
||||
h.app.session.undo.insert(0, older);
|
||||
}
|
||||
let second = h.app.session.undo.get(1).map(|s| s.label.clone());
|
||||
press_and_move(&mut h, a, b);
|
||||
release(&mut h, b);
|
||||
assert_eq!(h.app.session.undo.len(), 200);
|
||||
assert_eq!(h.app.session.undo.first().map(|s| s.label.clone()), second, "only the oldest step went");
|
||||
assert_eq!(h.app.session.undo.last().map(|s| s.label.as_str()), Some("Reach Point"), "the drag's step is on top");
|
||||
}
|
||||
}
|
||||
@@ -62,6 +62,7 @@ pub mod icons;
|
||||
pub mod inference;
|
||||
pub mod install;
|
||||
pub mod issue_report;
|
||||
pub mod joint_drag;
|
||||
pub mod keymap;
|
||||
mod material_card_review;
|
||||
#[cfg(test)]
|
||||
@@ -632,7 +633,9 @@ impl SolveApp {
|
||||
// reads the same way the toolbar button it came from does.
|
||||
let language = i18n::Locale::for_language(&self.preferences.language);
|
||||
let label = language.label(command_label(id));
|
||||
if !(id == "view.navigate" && v.get("continuous").and_then(Value::as_bool) == Some(true)) {
|
||||
// Frames of a gesture (a continuous view move, a part dragged by its joints) are
|
||||
// not echoed one by one.
|
||||
if v.get("continuous").and_then(Value::as_bool) != Some(true) {
|
||||
self.session.echo(language.format("ui.command_done", &[("command", label)]));
|
||||
}
|
||||
if id == "file.new" {
|
||||
|
||||
@@ -118,6 +118,8 @@ pub struct ViewportState {
|
||||
pub point_drag: Option<(String, usize, Option<Vec2>)>,
|
||||
/// Checkpoint and source for a cancellable whole-text drag.
|
||||
pub(crate) text_drag: Option<(u64, String)>,
|
||||
/// A part that joints place, being dragged.
|
||||
pub(crate) joint_drag: Option<crate::joint_drag::Drag>,
|
||||
}
|
||||
|
||||
/// Something under the cursor.
|
||||
@@ -2493,12 +2495,32 @@ pub fn show(app: &mut SolveApp, ui: &mut egui::Ui) {
|
||||
if crate::sketch_text_handles::dragging(app) && !crate::issue_report::cad_input_blocked() && !ui.input(|i| i.pointer.primary_down()) {
|
||||
crate::sketch_text_handles::drag_end(app);
|
||||
}
|
||||
// Dragging a part that joints place moves it as far as they allow (outside sketches).
|
||||
if app.tool.is_none() && app.dialog.is_none() && primary_nav.is_none() && mode.is_none() && app.session.active_sketch.is_none() && !held {
|
||||
if resp.drag_started_by(egui::PointerButton::Primary)
|
||||
&& let Some(a) = ui.input(|i| i.pointer.press_origin())
|
||||
&& rect.contains(a)
|
||||
&& !axes.contains(a)
|
||||
{
|
||||
crate::joint_drag::start(app, &proj, a);
|
||||
}
|
||||
if crate::joint_drag::active(app)
|
||||
&& resp.dragged_by(egui::PointerButton::Primary)
|
||||
&& let Some(p) = hover
|
||||
{
|
||||
crate::joint_drag::to(app, &proj, p);
|
||||
}
|
||||
}
|
||||
if crate::joint_drag::active(app) && (resp.drag_stopped() || !ui.input(|i| i.pointer.primary_down())) {
|
||||
crate::joint_drag::end(app);
|
||||
}
|
||||
// Box selection: a primary drag on the model when no navigation mode is on.
|
||||
if app.tool.is_none()
|
||||
&& !held
|
||||
&& primary_nav.is_none()
|
||||
&& mode.is_none()
|
||||
&& app.viewport.point_drag.is_none()
|
||||
&& !crate::joint_drag::active(app)
|
||||
&& !crate::dim_view::dragging()
|
||||
&& !crate::sketch3d::dragging()
|
||||
&& !crate::sketch_text_handles::dragging(app)
|
||||
|
||||
Reference in new issue
Block a user