diff --git a/crates/doc/src/joints.rs b/crates/doc/src/joints.rs index 0330ec076..f8cfddeb0 100644 --- a/crates/doc/src/joints.rs +++ b/crates/doc/src/joints.rs @@ -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, /// Occurrences fixed in place (grounded, or the root's). pub fixed: Vec, @@ -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, + inverse_b: Option, +} + +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 { + 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 { - 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, /// Joint frames in component coordinates (A's, B's). - frames: BTreeMap, + frames: BTreeMap, fixed: BTreeMap, /// Placed occurrence → (the occurrence it is placed from, the joint, from the joint's A side). parent: BTreeMap, @@ -988,8 +1011,10 @@ struct Tree<'a> { impl Tree<'_> { fn step(&self, joint: u64, from_a: bool, t: &Mat, vals: &Values) -> Option { + #[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>, + 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 { + 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) -> 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, 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, 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) -> 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) -> 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> { 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 { + 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> { @@ -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 = BTreeMap::new(); + let mut frames: BTreeMap = BTreeMap::new(); let mut found: BTreeMap<(u64, String), std::result::Result> = BTreeMap::new(); let mut resolve = |o: &JointOrigin| -> Result { 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) -> Option { mod tests { use super::*; + thread_local! { + pub(super) static PLACEMENT_STEPS: std::cell::Cell = const { std::cell::Cell::new(0) }; + pub(super) static PLACEMENT_INSERTS: std::cell::Cell = 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 = [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 { + 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 = (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 { diff --git a/crates/doc/src/joints_reach.rs b/crates/doc/src/joints_reach.rs index 4a5f4fbf2..2274bfc10 100644 --- a/crates/doc/src/joints_reach.rs +++ b/crates/doc/src/joints_reach.rs @@ -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 Result().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 Result Option> { - 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 { + 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); + } + } +} diff --git a/crates/engine/src/cmd/joints.rs b/crates/engine/src/cmd/joints.rs index c440459b7..8055ba37d 100644 --- a/crates/engine/src/cmd/joints.rs +++ b/crates/engine/src/cmd/joints.rs @@ -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 { } 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 { 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))) + } +} diff --git a/crates/engine/src/cmd/joints_reach_tests.rs b/crates/engine/src/cmd/joints_reach_tests.rs index ee4918b1c..d9003e756 100644 --- a/crates/engine/src/cmd/joints_reach_tests.rs +++ b/crates/engine/src/cmd/joints_reach_tests.rs @@ -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)); + } + } +} diff --git a/crates/engine/src/cmd/joints_tests.rs b/crates/engine/src/cmd/joints_tests.rs index 8f0ee1f48..06ec308c4 100644 --- a/crates/engine/src/cmd/joints_tests.rs +++ b/crates/engine/src/cmd/joints_tests.rs @@ -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); + } +} diff --git a/crates/engine/src/cmd/motion.rs b/crates/engine/src/cmd/motion.rs index 0d9225e8b..a4ac4531c 100644 --- a/crates/engine/src/cmd/motion.rs +++ b/crates/engine/src/cmd/motion.rs @@ -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)], 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 = set(s, 0.0).conflicts.iter().map(|c| c.0).collect(); diff --git a/crates/engine/src/cmd/motion_tests.rs b/crates/engine/src/cmd/motion_tests.rs index cd98625d9..e4ac363a1 100644 --- a/crates/engine/src/cmd/motion_tests.rs +++ b/crates/engine/src/cmd/motion_tests.rs @@ -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)); diff --git a/crates/engine/src/lib.rs b/crates/engine/src/lib.rs index ebc62f00b..edd100e8a 100644 --- a/crates/engine/src/lib.rs +++ b/crates/engine/src/lib.rs @@ -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; }