* claim: #5185 * Joint: reuse residual placements and skip discarded mobility reports (#5185)
This commit is contained in:
+269
-38
@@ -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 {
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -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));
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user