diff --git a/crates/doc/src/assembly.rs b/crates/doc/src/assembly.rs index b375eb4ae..31f312b68 100644 --- a/crates/doc/src/assembly.rs +++ b/crates/doc/src/assembly.rs @@ -227,6 +227,22 @@ impl FeatureKind { } } +/// [`Document::component_transform`] with each component's first occurrence found by `first` +/// (an index, where many transforms are wanted at once). +pub(crate) fn transform_through<'a>(component: u64, first: impl Fn(u64) -> Option<&'a Occurrence>) -> Mat { + let mut m = IDENTITY; + let mut c = component; + for _ in 0..1000 { + if c == 0 { + break; + } + let Some(o) = first(c) else { break }; + m = mat_mul(&o.transform, &m); + c = o.parent; + } + m +} + impl Document { /// The first occurrence of a component (its placement for authoring). pub fn occurrence_of(&self, component: u64) -> Option<&crate::Occurrence> { @@ -235,17 +251,7 @@ impl Document { /// World transform of a component's frame (through its first occurrence and its parents'). pub fn component_transform(&self, component: u64) -> Mat { - let mut m = IDENTITY; - let mut c = component; - for _ in 0..1000 { - if c == 0 { - break; - } - let Some(o) = self.occurrence_of(c) else { break }; - m = mat_mul(&o.transform, &m); - c = o.parent; - } - m + transform_through(component, |c| self.occurrence_of(c)) } /// One placement: the component the occurrence `path` from the root leads to (the root for diff --git a/crates/doc/src/joints.rs b/crates/doc/src/joints.rs index e4e3d766d..53d122ccc 100644 --- a/crates/doc/src/joints.rs +++ b/crates/doc/src/joints.rs @@ -2015,19 +2015,37 @@ impl Document { /// Apply a solution: occurrence transforms (in their parents) for the occurrences it places, /// and the values of the joints that moved to close loops. Returns true when something moved. pub fn apply_joint_solution(&mut self, sol: &Solution) -> bool { + // Joints and occurrences are indexed once (the first of each id, as a search finds): + // searching them for each of a large design's thousands of values and occurrences took + // 20 ms on every change to the design. + let mut joint_at = std::collections::HashMap::new(); + for (i, j) in self.assembly.joints.iter().enumerate() { + joint_at.entry(j.id).or_insert(i); + } let mut changed = false; for (id, q) in &sol.values { - if let Some(j) = self.assembly.joints.iter_mut().find(|j| j.id == *id) { + if let Some(j) = joint_at.get(id).and_then(|&i| self.assembly.joints.get_mut(i)) { j.values.clone_from(q); changed = true; } } let parents: BTreeMap = self.occurrences.iter().map(|o| (o.id, o.parent)).collect(); + let (mut occurrence_at, mut first_of) = (std::collections::HashMap::new(), std::collections::HashMap::new()); + for (i, o) in self.occurrences.iter().enumerate() { + occurrence_at.entry(o.id).or_insert(i); + first_of.entry(o.component).or_insert(i); + } for (occ, world) in &sol.world { let parent = parents.get(occ).copied().unwrap_or(0); - let pw = if parent == 0 { IDENTITY } else { self.component_transform(parent) }; + // Through the transforms as this loop has left them, as `component_transform` reads. + let occurrences = &self.occurrences; + let pw = if parent == 0 { + IDENTITY + } else { + crate::assembly::transform_through(parent, |c| first_of.get(&c).and_then(|&i| occurrences.get(i))) + }; let Some(local) = mat_inverse(&pw).map(|inv| mat_mul(&inv, world)) else { continue }; - if let Some(o) = self.occurrences.iter_mut().find(|o| o.id == *occ) { + if let Some(o) = occurrence_at.get(occ).and_then(|&i| self.occurrences.get_mut(i)) { let (dt, da) = mismatch(&o.transform, &local); if dt > 1e-9 || da > 1e-10 { o.transform = local; @@ -2393,4 +2411,33 @@ mod tests { let (_, a) = mismatch(&rot_z(3.0), &IDENTITY); assert!((a - 3.0).abs() < 1e-12, "{a}"); } + + /// Applying a solution stores each occurrence in its parent's frame as the loop has left it: + /// a component the same solution moves is read moved, through its first occurrence. Joint + /// values land on their joints; a value for a missing joint is skipped. + #[test] + fn a_solution_places_occurrences_in_their_moved_parents() { + let occurrence = + |id, component, parent| crate::Occurrence { id, component, parent, name: format!("O{id}"), transform: IDENTITY, grounded: false }; + // Component 1 sits at the root twice (its first occurrence is the frame its children sit + // in), component 2 inside 1 and component 3 inside 2. + let mut doc = Document { + assembly: chain(2, 0.0), + occurrences: vec![occurrence(10, 1, 0), occurrence(11, 1, 0), occurrence(20, 2, 1), occurrence(30, 3, 2)], + ..Document::default() + }; + let w10 = mat_mul(&shift(3.0, 4.0, 5.0), &rot_y(0.3)); + let w20 = mat_mul(&shift(-1.0, 2.0, 0.5), &rot_x(-0.2)); + let w30 = mat_mul(&shift(0.0, 7.0, 1.0), &rot_y(1.1)); + let world = BTreeMap::from([(10, w10), (20, w20), (30, w30)]); + let sol = Solution { world: world.clone(), values: BTreeMap::from([(2, vec![0.75]), (99, vec![1.0])]), ..Solution::default() }; + assert!(doc.apply_joint_solution(&sol)); + for (component, w) in [(1, w10), (2, w20), (3, w30)] { + assert!(close(&doc.component_transform(component), &w), "component {component}"); + } + assert!(close(&doc.occurrences[1].transform, &IDENTITY), "only the occurrence the solution names moves"); + let values: Vec<_> = doc.assembly.joints.iter().map(|j| j.values.clone()).collect(); + assert_eq!(values, vec![vec![0.0], vec![0.75]]); + assert!(!doc.apply_joint_solution(&Solution { world, ..Solution::default() }), "applied twice, nothing moves"); + } }