Joint: avoid redundant analysis and residual placement work (#5185) (#5186)

* claim: #5185

* Joint: reuse residual placements and skip discarded mobility reports (#5185)
This commit is contained in:
takuto-NA
2026-10-23 13:42:49 +00:00
committed by GitHub
parent 5962f56f87
commit 4256775274
8 changed files with 544 additions and 60 deletions
+269 -38
View File
@@ -856,6 +856,15 @@ pub fn clamp(j: &Joint, q: &mut [f64]) {
}
}
/// Work requested after placing occurrences and checking their joint constraints.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Analysis {
/// Placement, loop closure, conflicts and errors only; `Solution::dof` is empty.
Placement,
/// Also count each occurrence's remaining motion, including nearby singular poses.
DegreesOfFreedom,
}
/// The result of placing occurrences by their joints.
#[derive(Clone, Debug, Default, Serialize)]
pub struct Solution {
@@ -872,7 +881,7 @@ pub struct Solution {
/// Remaining degrees of freedom per occurrence (6 = free, 0 = fixed): how many independent
/// values still move it, from the fixed occurrences, with loops kept closed and a value a
/// motion link sets moving with the values its link chain starts from (a driven gear has its
/// driving gear's one, a screw's travel its turn's).
/// driving gear's one, a screw's travel its turn's). Empty for [`Analysis::Placement`].
pub dof: BTreeMap<u64, usize>,
/// Occurrences fixed in place (grounded, or the root's).
pub fixed: Vec<u64>,
@@ -963,22 +972,36 @@ fn flip_of(j: &Joint) -> Mat {
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 }
}
/// Component-space frames stay fixed throughout one solve. Cache their inverses without
/// regrouping products: reversed joints still invert the value-dependent link as before.
struct JointFrames {
a: Mat,
b: Mat,
inverse_a: Option<Mat>,
inverse_b: Option<Mat>,
}
impl JointFrames {
fn new(a: Mat, b: Mat) -> Self {
Self { a, b, inverse_a: mat_inverse(&a), inverse_b: mat_inverse(&b) }
}
fn through(&self, joint: &Joint, values: &[f64], from_a: bool, t: &Mat) -> Option<Mat> {
let link = mat_mul(&link_of(joint, values), self.inverse_b.as_ref()?);
if from_a { Some(mat_mul(&mat_mul(t, &self.a), &link)) } else { Some(mat_mul(&mat_mul(t, &mat_inverse(&link)?), self.inverse_a.as_ref()?)) }
}
}
/// 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(&link_of(j, q), &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)?))
}
JointFrames::new(*fa, *fb).through(j, q, from_a, t)
}
/// 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)>,
frames: BTreeMap<u64, JointFrames>,
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)>,
@@ -988,8 +1011,10 @@ struct Tree<'a> {
impl Tree<'_> {
fn step(&self, joint: u64, from_a: bool, t: &Mat, vals: &Values) -> Option<Mat> {
#[cfg(test)]
tests::PLACEMENT_STEPS.with(|steps| steps.set(steps.get() + 1));
let q = vals.get(&joint).map(Vec::as_slice).unwrap_or(&[]);
through(self.joints.get(&joint)?, self.frames.get(&joint)?, q, from_a, t)
self.frames.get(&joint)?.through(self.joints.get(&joint)?, q, from_a, t)
}
/// Where every placed occurrence is for these values.
@@ -1028,6 +1053,75 @@ impl Tree<'_> {
}
}
/// Storage shared by one residual callback's evaluations, scoped to its immutable tree.
/// Keep map nodes and path capacity, but invalidate every placement before reading new values.
struct PlacementWorkspace<'tree, 'doc> {
tree: &'tree Tree<'doc>,
placed: BTreeMap<u64, Option<Mat>>,
path: Vec<(u64, u64, bool)>,
}
impl<'tree, 'doc> PlacementWorkspace<'tree, 'doc> {
fn new(tree: &'tree Tree<'doc>) -> Self {
Self { tree, placed: BTreeMap::new(), path: Vec::new() }
}
fn at<'values>(&'values mut self, values: &'values Values) -> Placements<'values, 'tree, 'doc> {
for placed in self.placed.values_mut() {
*placed = None;
}
Placements { workspace: self, values }
}
}
/// Only this guard can read placements. Its values stay immutable until it is dropped;
/// obtaining the next guard clears validity while retaining the allocated storage.
struct Placements<'values, 'tree, 'doc> {
workspace: &'values mut PlacementWorkspace<'tree, 'doc>,
values: &'values Values,
}
impl Placements<'_, '_, '_> {
fn world(&mut self, mut occurrence: u64) -> Option<Mat> {
self.workspace.path.clear();
let mut t = loop {
if let Some(t) = self.workspace.placed.get(&occurrence).and_then(Option::as_ref).or_else(|| self.workspace.tree.fixed.get(&occurrence)) {
break *t;
}
if self.workspace.path.len() >= self.workspace.tree.order.len() {
return None;
}
let &(from, joint, from_a) = self.workspace.tree.parent.get(&occurrence)?;
self.workspace.path.push((occurrence, joint, from_a));
occurrence = from;
};
while let Some((occurrence, joint, from_a)) = self.workspace.path.pop() {
t = self.workspace.tree.step(joint, from_a, &t, self.values)?;
#[cfg(test)]
if !self.workspace.placed.contains_key(&occurrence) {
tests::PLACEMENT_INSERTS.with(|inserts| inserts.set(inserts.get() + 1));
}
self.workspace.placed.insert(occurrence, Some(t));
}
Some(t)
}
fn closure_gaps(&mut self, closing: &[u64], scale: f64, out: &mut Vec<f64>) -> Option<()> {
for id in closing {
let (joint, frames) = (self.workspace.tree.joints.get(id)?, self.workspace.tree.frames.get(id)?);
let tb = self.world(joint.b.occurrence)?;
let ta = self.world(joint.a.occurrence)?;
let want = self.workspace.tree.step(*id, true, &ta, self.values)?;
for p in [Vec3::ZERO, Vec3::X * scale, Vec3::Y * scale] {
let at = apply_point(&frames.b, p);
let d = apply_point(&want, at) - apply_point(&tb, at);
out.extend([d.x, d.y, d.z]);
}
}
Some(())
}
}
/// 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;
@@ -1232,8 +1326,8 @@ fn loop_scale(tree: &Tree, around: &BTreeSet<u64>, vals: &Values) -> f64 {
let joints: Aabb3 = around
.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))
let (j, frames) = (tree.joints.get(id)?, tree.frames.get(id)?);
Some(apply_point(&mat_mul(&tree.world(j.a.occurrence, vals)?, &frames.a), Vec3::ZERO))
})
.collect();
if joints.is_empty() { 1.0 } else { joints.diagonal().clamp(1.0, 1e6) }
@@ -1241,18 +1335,8 @@ fn loop_scale(tree: &Tree, around: &BTreeSet<u64>, vals: &Values) -> f64 {
/// How far each closing joint is from meeting: three points of B's joint frame, `scale` apart,
/// where the joint wants them against where the tree puts them (appended to `out`).
fn closure_gaps(tree: &Tree, closing: &[u64], vals: &Values, scale: f64, out: &mut Vec<f64>) -> Option<()> {
for id in closing {
let (j, (_, fb)) = (tree.joints.get(id)?, tree.frames.get(id)?);
let tb = tree.world(j.b.occurrence, vals)?;
let want = tree.step(*id, true, &tree.world(j.a.occurrence, vals)?, vals)?;
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]);
}
}
Some(())
fn closure_gaps(workspace: &mut PlacementWorkspace, closing: &[u64], vals: &Values, scale: f64, out: &mut Vec<f64>) -> Option<()> {
workspace.at(vals).closure_gaps(closing, scale, out)
}
/// How far a closing joint may be off before it is a conflict: its frames apart by more than
@@ -1314,6 +1398,7 @@ fn nearby_pose(tree: &Tree, groups: &[Group], asm: &Assembly, linked: &BTreeSet<
(v + d * if k % 2 == 0 { w } else { -w }).max(*lo).min(*hi)
})
.collect();
let mut workspace = PlacementWorkspace::new(tree);
let mut work = near.clone();
let mut gaps = |x: &[f64]| -> Option<Vec<f64>> {
spent += 1;
@@ -1327,7 +1412,7 @@ fn nearby_pose(tree: &Tree, groups: &[Group], asm: &Assembly, linked: &BTreeSet<
}
apply_links(asm, &mut work);
let mut out = Vec::with_capacity(closing.len() * 9);
closure_gaps(tree, closing, &work, scale, &mut out)?;
closure_gaps(&mut workspace, closing, &work, scale, &mut out)?;
out.iter().all(|v| v.is_finite()).then_some(out)
};
if close_within(&mut gaps, &mut x, &bounds, (1e-10 * scale).min(1e-7), loose_gap(scale)).is_some()
@@ -1442,6 +1527,7 @@ fn loop_dof(tree: &Tree, closing: &[u64], asm: &Assembly, vals: &Values, dof: &m
let (n, gaps) = (values.len(), closers.len() * 9);
// How many ways the occurrence moves at a pose, with the loops kept closed.
let mut free_at = |at: &Values| -> Option<usize> {
let mut workspace = PlacementWorkspace::new(tree);
let mut work = at.clone();
// The loops' gaps, then three points of the occurrence `scale` apart.
let mut f = |x: &[f64]| -> Option<Vec<f64>> {
@@ -1453,8 +1539,9 @@ fn loop_dof(tree: &Tree, closing: &[u64], asm: &Assembly, vals: &Values, dof: &m
}
apply_links(asm, &mut work);
let mut out = Vec::with_capacity(closers.len() * 9 + 9);
closure_gaps(tree, &closers, &work, scale, &mut out)?;
let t = tree.world(*occ, &work)?;
let mut placed = workspace.at(&work);
placed.closure_gaps(&closers, scale, &mut out)?;
let t = placed.world(*occ)?;
for p in [Vec3::ZERO, Vec3::X * scale, Vec3::Y * scale] {
let q = apply_point(&t, p);
out.extend([q.x, q.y, q.z]);
@@ -1550,6 +1637,7 @@ fn close_group(
if !moves.is_empty() {
first = first.min(1.0 / stride(&effective(&x0, 0.0), &effective(&x0, 1.0)));
}
let mut workspace = PlacementWorkspace::new(tree);
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.
@@ -1560,7 +1648,7 @@ fn close_group(
}
place(&mut work, x, t);
let mut out = Vec::with_capacity(closing.len() * 9);
closure_gaps(tree, closing, &work, scale, &mut out)?;
closure_gaps(&mut workspace, closing, &work, scale, &mut out)?;
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
@@ -1710,6 +1798,13 @@ pub fn solve(doc: &Document, st: &ModelState) -> Solution {
/// 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 {
solve_holding_with_analysis(doc, st, held, Analysis::DegreesOfFreedom)
}
/// Place occurrences and check constraints with the requested diagnostic work. Placement-only
/// solves still close and check every loop, respect held values and limits, and report errors;
/// they leave `Solution::dof` empty rather than returning the spanning tree's approximate count.
pub fn solve_holding_with_analysis(doc: &Document, st: &ModelState, held: &Held, analysis: Analysis) -> Solution {
let asm = &doc.assembly;
let mut sol = Solution::default();
let mut vals = joint_values(asm);
@@ -1724,9 +1819,9 @@ pub fn solve_holding(doc: &Document, st: &ModelState, held: &Held) -> Solution {
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 };
let (Some(want), Some(frames)) = (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));
let (dt, da) = mismatch(&mat_mul(&want, &frames.b), &mat_mul(tb, &frames.b));
if dt > CONFLICT_MM || da > CONFLICT_RAD {
sol.conflicts.push((
j.id,
@@ -1739,11 +1834,15 @@ pub fn solve_holding(doc: &Document, st: &ModelState, held: &Held) -> Solution {
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);
if analysis == Analysis::DegreesOfFreedom {
if !closing.is_empty() {
loop_dof(&tree, &closing, asm, &vals, &mut dof);
}
for o in &doc.occurrences {
dof.entry(o.id).or_insert(6);
}
} else {
dof.clear();
}
placed.remove(&0);
sol.world = placed;
@@ -1769,7 +1868,7 @@ fn build<'a>(doc: &'a Document, st: &ModelState, vals: &Values, sol: &mut Soluti
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). An origin several joints
// share (a panel face every part sits on) is found once.
let mut frames: BTreeMap<u64, (Mat, Mat)> = BTreeMap::new();
let mut frames: BTreeMap<u64, JointFrames> = BTreeMap::new();
let mut found: BTreeMap<(u64, String), std::result::Result<Mat, String>> = BTreeMap::new();
let mut resolve = |o: &JointOrigin| -> Result<Mat> {
let Ok(comp) = component_of(doc, o.occurrence) else { return resolve_origin(doc, st, o) };
@@ -1790,7 +1889,7 @@ fn build<'a>(doc: &'a Document, st: &ModelState, vals: &Values, sol: &mut Soluti
}
match (resolve(&j.a), resolve(&j.b)) {
(Ok(fa), Ok(fb)) => {
frames.insert(j.id, (fa, fb));
frames.insert(j.id, JointFrames::new(fa, fb));
}
(Err(e), _) | (_, Err(e)) => sol.errors.push((j.id, j.name.clone(), e.to_string())),
}
@@ -1973,6 +2072,138 @@ pub fn frame_of(o: Vec3, z: Vec3, x: Option<Vec3>) -> Option<Mat> {
mod tests {
use super::*;
thread_local! {
pub(super) static PLACEMENT_STEPS: std::cell::Cell<usize> = const { std::cell::Cell::new(0) };
pub(super) static PLACEMENT_INSERTS: std::cell::Cell<usize> = const { std::cell::Cell::new(0) };
}
pub(super) fn placement_tree(asm: &Assembly) -> Tree<'_> {
Tree {
joints: asm.joints.iter().map(|j| (j.id, j)).collect(),
frames: asm.joints.iter().map(|j| (j.id, JointFrames::new(mat_mul(&shift(3.0, 4.0, 5.0), &rot_y(0.3)), rot_x(-0.2)))).collect(),
fixed: BTreeMap::from([(0, IDENTITY)]),
parent: BTreeMap::from([(1, (0, 1, true)), (2, (1, 2, false)), (3, (2, 3, true)), (4, (1, 4, true)), (99, (0, 5, true))]),
order: vec![1, 2, 3, 4, 99],
}
}
/// Shared ancestors are evaluated once, without visiting an unrelated assembly branch.
/// The same arithmetic must be used after values change and on a reversed tree edge.
#[test]
fn residual_placements_reuse_ancestors_only_for_the_current_values() {
let asm = chain(6, 0.2);
let tree = placement_tree(&asm);
let mut values = joint_values(&asm);
let expected: BTreeMap<u64, Mat> = [1, 2, 3, 4].into_iter().map(|id| (id, tree.world(id, &values).unwrap())).collect();
let mut workspace = PlacementWorkspace::new(&tree);
PLACEMENT_STEPS.with(|steps| steps.set(0));
{
let mut placed = workspace.at(&values);
for id in [3, 4, 3, 2, 4] {
assert_eq!(placed.world(id), expected.get(&id).copied());
}
assert_eq!(placed.world(0), Some(IDENTITY));
assert_eq!(placed.world(1234), None);
}
assert_eq!(PLACEMENT_STEPS.with(|steps| steps.get()), 4, "one step per needed occurrence; no unrelated branch");
values.get_mut(&1).unwrap()[0] += 0.1;
apply_links(&asm, &mut values);
let changed = tree.world(3, &values).unwrap();
assert_ne!(Some(&changed), expected.get(&3));
let mut placed = workspace.at(&values);
assert_eq!(placed.world(3), Some(changed), "a new residual evaluation cannot reuse the previous values' placements");
}
/// Cached static inverses retain the original multiplication order for every joint kind,
/// flip and traversal direction. A non-invertible frame remains a failed step.
#[test]
fn prepared_joint_frames_match_fresh_inverses() {
fn reference(j: &Joint, fa: &Mat, fb: &Mat, q: &[f64], from_a: bool, t: &Mat) -> Option<Mat> {
let link = mat_mul(&link_of(j, q), &mat_inverse(fb)?);
if from_a { Some(mat_mul(&mat_mul(t, fa), &link)) } else { Some(mat_mul(&mat_mul(t, &mat_inverse(&link)?), &mat_inverse(fa)?)) }
}
let mut joint = chain(1, 0.0).joints.remove(0);
let fa = mat_mul(&shift(3.0, -7.0, 11.0), &rot_y(0.3));
let fb = mat_mul(&shift(-2.0, 5.0, 13.0), &rot_x(-0.2));
let t = mat_mul(&shift(17.0, 19.0, -23.0), &rot_z(0.7));
let singular = [[0.0; 4]; 4];
for (a, b) in [(fa, fb), (singular, fb), (fa, singular)] {
let frames = JointFrames::new(a, b);
for kind in [
JointKind::Rigid,
JointKind::Revolute,
JointKind::Slider,
JointKind::Cylindrical,
JointKind::PinSlot,
JointKind::Planar,
JointKind::Ball,
JointKind::Parallel,
JointKind::Perpendicular,
JointKind::Angle,
JointKind::PointOnPlane,
JointKind::PointOnLine,
JointKind::LineOnPlane,
] {
joint.kind = kind;
joint.offset = 2.7;
joint.angle = -0.41;
for flip in [false, true] {
joint.flip = flip;
for angle in [0.0, 1e-6, -0.37, std::f64::consts::PI] {
let q: Vec<f64> = (0..kind.dofs().len()).map(|i| angle * (i + 1) as f64).collect();
for forward in [false, true] {
assert_eq!(
frames.through(&joint, &q, forward, &t),
reference(&joint, &a, &b, &q, forward, &t),
"{kind:?}, {flip}, {angle}, {forward}"
);
}
}
}
}
}
}
/// The loop solver's residual entry point reuses shared ancestors for each perturbation;
/// row order, duplicate rows, append semantics and missing-origin failure stay unchanged.
#[test]
fn closure_residuals_reuse_shared_ancestors_for_each_perturbation() {
let mut asm = chain(6, 0.2);
asm.joints[5].a.occurrence = 3;
asm.joints[5].b.occurrence = 4;
let tree = placement_tree(&asm);
let mut workspace = PlacementWorkspace::new(&tree);
let mut cold = true;
for start in [0.2, 0.2 + 1e-6, 0.2 - 1e-6, 1.3] {
let mut values = joint_values(&asm);
values.get_mut(&1).unwrap()[0] = start;
apply_links(&asm, &mut values);
let mut expected = vec![42.0];
for id in [6, 6] {
let joint = tree.joints.get(&id).unwrap();
let frames = tree.frames.get(&id).unwrap();
let a = tree.world(joint.a.occurrence, &values).unwrap();
let b = tree.world(joint.b.occurrence, &values).unwrap();
let want = through(joint, &(frames.a, frames.b), values.get(&id).unwrap(), true, &a).unwrap();
for point in [Vec3::ZERO, Vec3::X * 37.0, Vec3::Y * 37.0] {
let at = apply_point(&frames.b, point);
let d = apply_point(&want, at) - apply_point(&b, at);
expected.extend([d.x, d.y, d.z]);
}
}
PLACEMENT_STEPS.with(|steps| steps.set(0));
PLACEMENT_INSERTS.with(|inserts| inserts.set(0));
let mut actual = vec![42.0];
assert_eq!(closure_gaps(&mut workspace, &[6, 6], &values, 37.0, &mut actual), Some(()));
assert_eq!(actual, expected);
assert_eq!(PLACEMENT_INSERTS.with(|inserts| inserts.get()), if cold { 4 } else { 0 }, "reuse cache storage between perturbations");
cold = false;
assert_eq!(PLACEMENT_STEPS.with(|steps| steps.get()), 6, "four tree placements and two closing-joint steps");
assert_eq!(closure_gaps(&mut workspace, &[u64::MAX], &values, 37.0, &mut actual), None);
assert_eq!(actual, expected, "an unresolved joint does not insert a zero residual");
}
}
fn close(a: &Mat, b: &Mat) -> bool {
let (t, r) = mismatch(a, b);
t < 1e-9 && r < 1e-9
@@ -1980,7 +2211,7 @@ mod tests {
/// Revolute joints 1…n (joint 1 at `start`, the others at 0) linked 1 → 2 → … → n with
/// ratio 2, the links stored from the end of the chain; joint 3 limited to ±50.
fn chain(n: u64, start: f64) -> Assembly {
pub(super) fn chain(n: u64, start: f64) -> Assembly {
let origin = JointOrigin { occurrence: 0, snap: Snap::Point { point: Vec3::ZERO, z: None, x: None } };
let joints = (1..=n)
.map(|id| Joint {
+80 -14
View File
@@ -9,8 +9,8 @@ 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, close_within, closure_gaps, jacobian,
joint_values, largest, least_squares, limit_bounds, link_chains, loop_groups, loop_scale, loose_gap, unknowns,
Assembly, Held, MAX_LOOP_VALUES, MAX_LOOP_WORK, MAX_LOOPS, Solution, Tree, Values, apply_links, build, close_within, jacobian, joint_values,
largest, least_squares, limit_bounds, link_chains, loop_groups, loop_scale, loose_gap, unknowns,
};
use crate::{Document, ModelState, apply_point, apply_vector, mat_mul};
@@ -133,8 +133,8 @@ pub fn reach(doc: &Document, st: &ModelState, r: &Reach) -> Result<Reached, Stri
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?))
let (j, frames) = (tree.joints.get(id)?, tree.frames.get(id)?);
Some(apply_point(&mat_mul(&tree.world(j.a.occurrence, &vals)?, &frames.a), Vec3::ZERO).dist(point?))
})
.fold(0.0, f64::max);
let span = point.map_or(scale, |p| scale.max(p.dist(r.to)).max(lever));
@@ -155,7 +155,8 @@ pub fn reach(doc: &Document, st: &ModelState, r: &Reach) -> Result<Reached, Stri
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 workspace = super::PlacementWorkspace::new(tree);
let mut f = |x: &[f64]| misses(&mut workspace, 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));
@@ -216,7 +217,8 @@ pub fn reach(doc: &Document, st: &ModelState, r: &Reach) -> Result<Reached, Stri
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 alone_workspace = super::PlacementWorkspace::new(tree);
let mut g = |x: &[f64]| misses(&mut alone_workspace, 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, loose_gap(scale))
@@ -249,7 +251,7 @@ pub fn reach(doc: &Document, st: &ModelState, r: &Reach) -> Result<Reached, Stri
/// 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,
workspace: &mut super::PlacementWorkspace,
asm: &Assembly,
r: &Reach,
scale: f64,
@@ -269,9 +271,18 @@ fn misses(
}
}
apply_links(asm, work);
let mut out = target(tree, r, scale, work)?;
let n = out.len();
closure_gaps(tree, closers, work, scale, &mut out)?;
let tree = workspace.tree;
// An open chain has only one target placement to read. Loops share their ancestors with
// that target, but a single placement needs no residual-local map.
let mut out = if closers.is_empty() {
target(tree, r, scale, work)?
} else {
let mut placed = workspace.at(work);
let mut out = target_at(&placed.world(r.occurrence)?, r, scale);
placed.closure_gaps(closers, scale, &mut out)?;
out
};
let n = if r.axis.is_some() { 6 } else { 3 };
for v in out.iter_mut().skip(n) {
*v *= LOOP_WEIGHT;
}
@@ -280,14 +291,17 @@ fn misses(
/// 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;
Some(target_at(&tree.world(r.occurrence, vals)?, r, scale))
}
fn target_at(t: &crate::Mat, r: &Reach, scale: f64) -> Vec<f64> {
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;
let w = (apply_vector(t, a) - z) * scale;
out.extend([w.x, w.y, w.z]);
}
Some(out)
out
}
/// How many steps apart the values `vars` at `a` and at `b` put the `paced` values (each with its
@@ -447,3 +461,55 @@ fn approach(
}
Some(r)
}
#[cfg(test)]
mod tests {
use super::super::tests::{PLACEMENT_INSERTS, PLACEMENT_STEPS, chain, placement_tree};
use super::*;
/// Exercise the IK callback itself: removing sharing at this call site must fail even if
/// the reusable placement helper still passes its own tests.
#[test]
fn ik_residual_reuses_target_and_loop_ancestors_at_the_call_site() {
let mut asm = chain(6, 0.2);
asm.joints[5].a.occurrence = 3;
asm.joints[5].b.occurrence = 4;
let tree = placement_tree(&asm);
let base = joint_values(&asm);
let mut workspace = super::super::PlacementWorkspace::new(&tree);
let mut cold = true;
for axis in [None, Some((Vec3::Y, Vec3::Z))] {
let r = Reach { occurrence: 3, point: Vec3::new(1.0, 2.0, 3.0), to: Vec3::new(7.0, 11.0, 13.0), axis, joints: None };
for x in [0.2, 0.2 + 1e-6, 0.2 - 1e-6, 1.3] {
let mut reference = base.clone();
reference.get_mut(&1).unwrap()[0] = x;
apply_links(&asm, &mut reference);
let mut expected = target(&tree, &r, 37.0, &reference).unwrap();
let n = expected.len();
super::super::closure_gaps(&mut super::super::PlacementWorkspace::new(&tree), &[6], &reference, 37.0, &mut expected).unwrap();
for v in expected.iter_mut().skip(n) {
*v *= LOOP_WEIGHT;
}
let mut work = base.clone();
let spent = Cell::new(0);
PLACEMENT_STEPS.with(|steps| steps.set(0));
PLACEMENT_INSERTS.with(|inserts| inserts.set(0));
let actual = misses(&mut workspace, &asm, &r, 37.0, &[(1, 0)], &[6], &[x], &mut work, &spent);
assert_eq!(actual, Some(expected));
assert_eq!(
PLACEMENT_INSERTS.with(|inserts| inserts.get()),
if cold { 4 } else { 0 },
"IK must retain cache storage across evaluations"
);
cold = false;
assert_eq!(spent.get(), 1, "reuse must not skip a finite-difference evaluation or bypass its work cap");
assert_eq!(PLACEMENT_STEPS.with(|steps| steps.get()), 5, "four tree placements shared by the target and loop, plus the closing step");
}
let mut work = base.clone();
let spent = Cell::new(MAX_LOOP_WORK);
PLACEMENT_STEPS.with(|steps| steps.set(0));
assert!(misses(&mut workspace, &asm, &r, 37.0, &[(1, 0)], &[6], &[0.2], &mut work, &spent).is_none());
assert_eq!(PLACEMENT_STEPS.with(|steps| steps.get()), 0);
}
}
}
+53 -4
View File
@@ -106,6 +106,14 @@ pub fn resolve(s: &mut Session) -> joints::Solution {
resolve_holding(s, &BTreeMap::new())
}
/// Update placements after an edit without computing a DOF report that the caller discards.
/// Expressions and geometry-dependent couplings are refreshed just as in [`resolve`].
pub fn place(s: &mut Session) -> joints::Solution {
reexpress(s);
recouple(s);
place_holding(s, &BTreeMap::new())
}
/// Work out the offsets, angles and values joints were given as expressions again (a parameter
/// may have changed). One that can't be worked out (its parameter gone) keeps its numbers; the
/// solve leaves that joint out and says why.
@@ -165,8 +173,22 @@ fn recouple(s: &mut Session) {
/// 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_holding(&s.doc, &st, held);
resolve_with_analysis(s, held, joints::Analysis::DegreesOfFreedom)
}
/// Contact and loop-search trials need placements and conflicts, not a DOF report.
pub(super) fn place_holding(s: &mut Session, held: &joints::Held) -> joints::Solution {
resolve_with_analysis(s, held, joints::Analysis::Placement)
}
fn solution(s: &Session, held: &joints::Held, analysis: joints::Analysis) -> joints::Solution {
#[cfg(test)]
solve_counts::record(analysis);
joints::solve_holding_with_analysis(&s.doc, &s.model.state(), held, analysis)
}
fn resolve_with_analysis(s: &mut Session, held: &joints::Held, analysis: joints::Analysis) -> joints::Solution {
let sol = solution(s, held, analysis);
if !sol.world.is_empty() {
let mut next = (*s.doc).clone();
if next.apply_joint_solution(&sol) {
@@ -177,6 +199,11 @@ pub fn resolve_holding(s: &mut Session, held: &joints::Held) -> joints::Solution
sol
}
/// A quiet drive/reach does not publish DOF, so it needs only placement and constraint checks.
fn response_analysis(p: &Value) -> joints::Analysis {
if bool_(p, "quiet").unwrap_or(false) { joints::Analysis::Placement } else { joints::Analysis::DegreesOfFreedom }
}
/// A solve's answer; `quiet: true` in the command's params leaves out the per-occurrence lists.
fn sol_json(sol: &joints::Solution, p: &Value) -> Value {
let mut out = json!({
@@ -787,7 +814,7 @@ fn drive(s: &mut Session, p: &Value) -> Result<Value> {
}
let before = super::motion::positions(s);
let r = super::motion::drive_joint(s, id, q, given)?;
let sol = joints::solve(&s.doc, &s.model.state());
let sol = solution(s, &BTreeMap::new(), response_analysis(p));
let mut out = sol_json(&sol, p);
out["joint"] = json!(id);
out["values"] = json!(effective_values(s, id));
@@ -852,7 +879,7 @@ fn reach(s: &mut Session, p: &Value) -> Result<Value> {
Err(_) => joints::Reached::default(),
};
// Loops closed again from there, and where it ended up.
let sol = resolve(s);
let sol = if response_analysis(p) == joints::Analysis::Placement { place(s) } else { 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);
@@ -1439,3 +1466,25 @@ mod tests;
#[cfg(test)]
#[path = "joints_turn_tests.rs"]
mod turn_tests;
/// Per-thread work counts for ordinary engine regression tests; no production instrumentation.
#[cfg(test)]
pub(super) mod solve_counts {
use solidcraft_doc::joints::Analysis;
use std::cell::Cell;
thread_local! {
static COUNTS: Cell<(usize, usize)> = const { Cell::new((0, 0)) };
}
pub fn record(analysis: Analysis) {
COUNTS.with(|c| {
let (all, full) = c.get();
c.set((all + 1, full + usize::from(analysis == Analysis::DegreesOfFreedom)));
});
}
pub fn take() -> (usize, usize) {
COUNTS.with(|c| c.replace((0, 0)))
}
}
@@ -313,3 +313,74 @@ fn reach_refuses_bad_params_and_says_why_nothing_moves() {
assert!(r["reason"].as_str().is_some_and(|w| w.contains(why)), "{p}: {r}");
}
}
/// Reach still publishes fresh full diagnostics after a lightweight refresh, including stationary
/// mouse frames, loop-limited targets and undo/redo. Full analysis runs once only when requested.
#[test]
fn reach_preserves_full_diagnostics_without_repeating_dof_analysis() {
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);
for (angle, quiet) in [(-10.0_f64, false), (-5.0, true), (-5.0, true), (100.0, false)] {
let a = start + angle.to_radians();
let to = o4 + Vec3::new(40.0 * a.cos(), 40.0 * a.sin(), 0.0);
super::solve_counts::take();
let r = reach(&mut s, "Rocker", q0, json!({"point":pt(to), "continuous":true, "quiet":quiet}));
assert_eq!(super::solve_counts::take().1, usize::from(!quiet));
assert_eq!(r["reached"], angle != 100.0, "{r}");
assert_eq!(r["stopped_by_loop"].is_object(), angle == 100.0, "{r}");
let expected = super::sol_json(&solidcraft_doc::joints::solve(&s.doc, &s.model.state()), &json!({"quiet":quiet}));
for key in ["dof", "fixed", "errors", "conflicts"] {
assert_eq!(r.get(key), expected.get(key), "{key}: {r}");
}
let listed = run(&mut s, "joint.list", json!({}));
let full = super::sol_json(&solidcraft_doc::joints::solve(&s.doc, &s.model.state()), &json!({}));
assert_eq!(listed["dof"], full["dof"]);
}
for command in ["edit.undo", "edit.redo"] {
run(&mut s, command, json!({}));
let full = solidcraft_doc::joints::solve(&s.doc, &s.model.state());
assert!(full.conflicts.is_empty() && full.errors.is_empty());
for part in ["Crank", "Coupler", "Rocker"] {
assert_eq!(full.dof.get(&occ(&s, part)), Some(&1), "{command}: {part}");
}
}
}
/// Successive large pointer goals must use the new joint values at every perturbation and
/// solve, keep all loop pins together and preserve mobility reports through undo/redo.
#[test]
fn quarter_turn_reaches_keep_the_four_bar_closed_and_diagnostics_current() {
let (mut s, p0, q0) = four_bar();
let ground = place(&s, "Ground", Vec3::ZERO);
for (i, point) in
[Vec3::new(-20.0, 0.0, 0.0), Vec3::new(0.0, -20.0, 0.0), Vec3::new(20.0, 0.0, 0.0), Vec3::new(0.0, 20.0, 0.0)].into_iter().enumerate()
{
let r = reach(&mut s, "Crank", p0, json!({"point":pt(point), "continuous":true, "quiet":i % 2 == 0}));
assert_eq!((&r["reached"], &r["stopped_by_loop"], &r["stopped_by_limit"]), (&json!(true), &Value::Null, &Value::Null), "{r}");
assert!(place(&s, "Crank", p0).dist(point) < 1e-5, "{r}");
let rocker = place(&s, "Rocker", q0);
let [a, b] = meet(point, 50.0, Vec3::new(45.0, 0.0, 0.0), 40.0);
assert!(rocker.dist(a).min(rocker.dist(b)) < 1e-5, "{r}");
assert!(place(&s, "Coupler", q0).dist(rocker) < 1e-6, "{r}");
assert!(place(&s, "Coupler", p0).dist(point) < 1e-5, "{r}");
assert_eq!(place(&s, "Ground", Vec3::ZERO), ground);
let full = solidcraft_doc::joints::solve(&s.doc, &s.model.state());
assert!(full.errors.is_empty() && full.conflicts.is_empty(), "{full:?}");
for part in ["Crank", "Coupler", "Rocker"] {
assert_eq!(full.dof.get(&occ(&s, part)), Some(&1));
}
let steps = s.undo.len();
let again = reach(&mut s, "Crank", p0, json!({"point":pt(point), "continuous":true, "quiet":true}));
assert_eq!((&again["reached"], &again["moved"], s.undo.len()), (&json!(true), &json!(false), steps), "{again}");
}
for (command, point) in [("edit.undo", Vec3::new(20.0, 0.0, 0.0)), ("edit.redo", p0)] {
run(&mut s, command, json!({}));
assert!(place(&s, "Crank", p0).dist(point) < 1e-5, "{command}");
let full = solidcraft_doc::joints::solve(&s.doc, &s.model.state());
assert!(full.errors.is_empty() && full.conflicts.is_empty(), "{command}: {full:?}");
for part in ["Crank", "Coupler", "Rocker"] {
assert_eq!(full.dof.get(&occ(&s, part)), Some(&1));
}
}
}
+63
View File
@@ -977,6 +977,9 @@ fn a_part_at_a_dead_point_of_its_loop_keeps_its_mobility() {
pin(&mut s, "revolute", "Root pin", "Ground", "Barrel", root, Vec3::Z);
pin(&mut s, "slider", "Stroke", "Barrel", "Rod", root, Vec3::Y);
pin(&mut s, "revolute", "Tip pin", "Rod", "Panel", tip, Vec3::Z);
super::solve_counts::take();
s.refresh();
assert_eq!(super::solve_counts::take().1, 0, "refresh must not repeat the singular-pose analysis");
for part in ["Panel", "Barrel", "Rod"] {
assert_eq!(dof_of(&mut s, part), 1, "dead point: {part}");
}
@@ -1948,3 +1951,63 @@ fn joint_inspect_reads_only() {
assert_eq!(run(&mut s, "document.inspect", json!({}))["undo"], undo);
assert_eq!(serde_json::to_value(&s.doc).unwrap_or_default(), before);
}
/// Trial placements use exactly the same constraints as a full solve. Only the expensive DOF
/// report is optional: loop stops, errors and the resulting pose must not depend on it.
#[test]
fn placement_analysis_preserves_held_loop_constraints_and_diagnostics() {
use solidcraft_doc::joints::{self, Analysis};
use std::collections::BTreeMap;
let (s, _, _) = four_bar();
let rocker = s.doc.assembly.joints.iter().find(|j| j.name == "Rocker").unwrap().id;
let held = BTreeMap::from([((rocker, 0), 0.0)]);
for angle in [-10.0_f64, 40.0, 100.0] {
let mut doc = (*s.doc).clone();
doc.assembly.joints.iter_mut().find(|j| j.id == rocker).unwrap().values[0] = angle.to_radians();
let full = joints::solve_holding(&doc, &s.model.state(), &held);
let placement = joints::solve_holding_with_analysis(&doc, &s.model.state(), &held, Analysis::Placement);
assert!(placement.dof.is_empty(), "trial placement must not compute DOF");
assert_eq!(placement.world, full.world);
assert_eq!(placement.values, full.values);
assert_eq!(placement.fixed, full.fixed);
assert_eq!(placement.conflicts, full.conflicts);
assert_eq!(placement.errors, full.errors);
let mut placed = doc.clone();
placed.apply_joint_solution(&placement);
let mut reference = doc;
reference.apply_joint_solution(&full);
assert_eq!(placed, reference);
assert_eq!(joints::solve(&placed, &s.model.state()).dof, joints::solve(&reference, &s.model.state()).dof);
}
// A degenerate origin remains an error even in a placement-only trial.
let mut broken = (*s.doc).clone();
broken.assembly.joints[0].b.snap = solidcraft_doc::joints::Snap::Point { point: Vec3::ZERO, z: Some(Vec3::ZERO), x: None };
let full = joints::solve(&broken, &s.model.state());
let placement = joints::solve_holding_with_analysis(&broken, &s.model.state(), &BTreeMap::new(), Analysis::Placement);
assert!(!full.errors.is_empty());
assert_eq!(placement.errors, full.errors);
assert_eq!(placement.conflicts, full.conflicts);
}
/// Ordinary commands publish one complete report; refresh only updates constraint placements.
/// Counts are deterministic work assertions, not timing thresholds tied to a test machine.
#[test]
fn joint_drive_and_refresh_do_not_repeat_dof_analysis() {
let (mut s, _, _) = four_bar();
for (angle, quiet) in [(17, false), (58, true), (105, false)] {
super::solve_counts::take();
let r = run(&mut s, "joint.drive", json!({"joint":"Crank", "value":angle, "quiet":quiet}));
let (all, full) = super::solve_counts::take();
assert!(all > full, "placement trials must still run");
assert_eq!(full, usize::from(!quiet), "only a published DOF report needs full analysis");
let expected = super::sol_json(&solidcraft_doc::joints::solve(&s.doc, &s.model.state()), &json!({"quiet":quiet}));
for key in ["dof", "fixed", "errors", "conflicts"] {
assert_eq!(r.get(key), expected.get(key), "{key}: {r}");
}
let before = s.doc.clone();
s.refresh();
assert_eq!(super::solve_counts::take().1, 0, "refresh discards the DOF report");
assert_eq!(*s.doc, *before);
}
}
+2 -2
View File
@@ -10,7 +10,7 @@ use solidcraft_geom::{Aabb3, Vec3};
use solidcraft_kernel::Body;
use super::CommandSpec;
use super::joints::{cannot_move, effective_values, joint_id, occurrence_id, resolve_holding, value};
use super::joints::{cannot_move, effective_values, joint_id, occurrence_id, place_holding, value};
use crate::params::{bad, bool_, str_, vec3};
use crate::{EngineError, Result, Session};
@@ -377,7 +377,7 @@ fn drive_with_contact(s: &mut Session, want: &[(u64, Vec<f64>)], given: &BTreeSe
j.values = q;
}
}
resolve_holding(s, &held)
place_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();
+4
View File
@@ -479,7 +479,11 @@ fn contact_sets_stop_a_driven_joint_at_touching() {
assert!((b_x(&s) - 5.0).abs() < 1e-6);
run(&mut s, "joint.drive", json!({"joint": "Rail", "value": 10}));
run(&mut s, "contact.create", json!({"occurrences": ["A", "B"]}));
super::super::joints::solve_counts::take();
let r = run(&mut s, "joint.drive", json!({"joint": "Rail", "value": -5}));
let (all, full) = super::super::joints::solve_counts::take();
assert!(all > 4, "contact must still test intermediate placements");
assert_eq!(full, 1, "contact trials must not repeat DOF analysis");
assert!(r["stopped_by_contact"].is_object(), "{r}");
// Stopped where the faces touch (to the search's resolution, 15 mm / 2^14).
assert!((b_x(&s) - 10.0).abs() < 2e-3, "{}", b_x(&s));
+2 -2
View File
@@ -654,7 +654,7 @@ impl Session {
self.prune_occurrence_state();
// Joints follow the geometry their origins snap to.
if !self.doc.assembly.joints.is_empty() {
cmd::joints::resolve(self);
cmd::joints::place(self);
}
cmd::motion::associative::refresh(self);
if let Some(id) = self.active_sketch
@@ -673,7 +673,7 @@ impl Session {
{
self.revision += 1;
if !self.doc.assembly.joints.is_empty() {
cmd::joints::resolve(self);
cmd::joints::place(self);
}
return;
}