diff --git a/crates/doc/src/document.rs b/crates/doc/src/document.rs index d5e14a5d..edd0719d 100644 --- a/crates/doc/src/document.rs +++ b/crates/doc/src/document.rs @@ -792,6 +792,9 @@ pub struct Document { /// Placements of components in their parents (each non-root component has at least one). #[serde(default, skip_serializing_if = "Vec::is_empty")] pub occurrences: Vec, + /// Joints between occurrences, joint origins and motion links. + #[serde(default, skip_serializing_if = "crate::joints::Assembly::is_empty")] + pub assembly: crate::joints::Assembly, /// Favourite parameters (by name). #[serde(default, skip_serializing_if = "std::collections::BTreeSet::is_empty")] pub favorites: std::collections::BTreeSet, @@ -831,6 +834,7 @@ impl Document { next_id: 1, components: Vec::new(), occurrences: Vec::new(), + assembly: Default::default(), body_components: Default::default(), materials: Default::default(), favorites: Default::default(), diff --git a/crates/doc/src/joints.rs b/crates/doc/src/joints.rs new file mode 100644 index 00000000..502e2e5d --- /dev/null +++ b/crates/doc/src/joints.rs @@ -0,0 +1,495 @@ +//! Joints between occurrences: joint origins (snap frames on component geometry, re-found on +//! every solve so they follow edits), joints (rigid, revolute, slider, cylindrical, pin-slot, +//! planar, ball) with their motion values and limits, motion links, and the solver that places +//! occurrences from the joint graph. +//! +//! A joint makes B's joint frame coincide with A's frame moved by the joint's motion: +//! `T_B · F_b = T_A · F_a · M(q) · Flip`, so `T_B = T_A · F_a · M(q) · Flip · F_b⁻¹`, where `T` +//! are occurrence world transforms, `F` the joint frames in each component's own frame, and `q` +//! the joint's values (angles in radians, distances in mm). The solver walks the graph from +//! grounded occurrences (and the root, which never moves); joints that close a loop are checked +//! and reported when they disagree. + +use std::collections::{BTreeMap, BTreeSet, VecDeque}; + +use serde::{Deserialize, Serialize}; +use solvecraft_geom::Vec3; + +use crate::{DocError, Document, IDENTITY, Mat, ModelState, Result, apply_point, apply_vector, mat_inverse, mat_mul}; + +const MAX_JOINTS: usize = 10_000; + +/// Where a joint origin sits on a component's geometry (component coordinates). +#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)] +#[serde(tag = "type", rename_all = "snake_case")] +pub enum Snap { + /// An explicit frame (origin, z axis, x axis). + Frame { + origin: Vec3, + z: Vec3, + #[serde(default, skip_serializing_if = "Option::is_none")] + x: Option, + }, + /// The centre of the planar face nearest `pick`, z along its outward normal. + FaceCenter { pick: Vec3 }, + /// The centre of the circular end nearest `pick` of the cylindrical face there, z along its + /// axis (pointing out of the face's end). + CircleCenter { pick: Vec3 }, + /// A point, z along `z` (default +Z). + Point { + point: Vec3, + #[serde(default, skip_serializing_if = "Option::is_none")] + z: Option, + }, +} + +/// A joint origin: a snap on an occurrence's component (occurrence 0 = the root design). +#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)] +pub struct JointOrigin { + pub occurrence: u64, + pub snap: Snap, +} + +/// A named joint origin (Joint Origin command), referenced by joints by name. +#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)] +pub struct NamedOrigin { + pub name: String, + pub origin: JointOrigin, +} + +#[derive(Clone, Copy, Debug, PartialEq, Eq, Serialize, Deserialize)] +#[serde(rename_all = "snake_case")] +pub enum JointKind { + Rigid, + Revolute, + Slider, + Cylindrical, + PinSlot, + Planar, + Ball, +} + +impl JointKind { + pub fn parse(s: &str) -> Option { + Some(match s.to_ascii_lowercase().replace([' ', '_', '-'], "").as_str() { + "rigid" => JointKind::Rigid, + "revolute" | "rotate" => JointKind::Revolute, + "slider" | "slide" => JointKind::Slider, + "cylindrical" => JointKind::Cylindrical, + "pinslot" => JointKind::PinSlot, + "planar" => JointKind::Planar, + "ball" => JointKind::Ball, + _ => return None, + }) + } + /// Names of the joint's values, in order (angles in radians, distances in mm). + pub fn dofs(self) -> &'static [&'static str] { + match self { + JointKind::Rigid => &[], + JointKind::Revolute => &["angle"], + JointKind::Slider => &["distance"], + JointKind::Cylindrical => &["angle", "distance"], + JointKind::PinSlot => &["angle", "slide"], + JointKind::Planar => &["x", "y", "angle"], + JointKind::Ball => &["yaw", "pitch", "roll"], + } + } + pub fn is_angle(self, i: usize) -> bool { + matches!(self.dofs().get(i), Some(&("angle" | "yaw" | "pitch" | "roll"))) + } +} + +/// A joint between two occurrences. +#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)] +pub struct Joint { + pub id: u64, + pub name: String, + pub kind: JointKind, + pub a: JointOrigin, + pub b: JointOrigin, + /// Current values of the joint's degrees of freedom. + #[serde(default)] + pub values: Vec, + /// Per value: (min, max) limits. + #[serde(default)] + pub limits: Vec>, + /// Turn B over (its z opposite to A's). + #[serde(default, skip_serializing_if = "std::ops::Not::not")] + pub flip: bool, + /// Distance from A's frame to B's along z, and a turn about it (radians). + #[serde(default)] + pub offset: f64, + #[serde(default)] + pub angle: f64, + #[serde(default, skip_serializing_if = "std::ops::Not::not")] + pub suppressed: bool, +} + +/// Value of joint `b` (index `ib`) = ratio · value of joint `a` (index `ia`) + offset. +#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)] +pub struct MotionLink { + pub a: u64, + #[serde(default)] + pub ia: usize, + pub b: u64, + #[serde(default)] + pub ib: usize, + pub ratio: f64, + #[serde(default)] + pub offset: f64, +} + +/// Joints, joint origins and motion links of a design. +#[derive(Clone, Debug, Default, PartialEq, Serialize, Deserialize)] +pub struct Assembly { + #[serde(default, skip_serializing_if = "Vec::is_empty")] + pub joints: Vec, + #[serde(default, skip_serializing_if = "Vec::is_empty")] + pub origins: Vec, + #[serde(default, skip_serializing_if = "Vec::is_empty")] + pub links: Vec, +} + +impl Assembly { + pub fn is_empty(&self) -> bool { + self.joints.is_empty() && self.origins.is_empty() && self.links.is_empty() + } +} + +/// A frame: origin and orthonormal axes, as a transform (local → parent). +fn frame(o: Vec3, z: Vec3, x: Option) -> Option { + let z = z.normalized()?; + let x = x.and_then(|x| (x - z * x.dot(z)).normalized()).unwrap_or_else(|| z.any_perp()); + let y = z.cross(x).normalized()?; + Some([[x.x, x.y, x.z, 0.0], [y.x, y.y, y.z, 0.0], [z.x, z.y, z.z, 0.0], [o.x, o.y, o.z, 1.0]]) +} + +fn rot_z(a: f64) -> Mat { + let (s, c) = a.sin_cos(); + [[c, s, 0.0, 0.0], [-s, c, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]] +} +fn rot_y(a: f64) -> Mat { + let (s, c) = a.sin_cos(); + [[c, 0.0, -s, 0.0], [0.0, 1.0, 0.0, 0.0], [s, 0.0, c, 0.0], [0.0, 0.0, 0.0, 1.0]] +} +fn shift(x: f64, y: f64, z: f64) -> Mat { + let mut m = IDENTITY; + m[3] = [x, y, z, 1.0]; + m +} + +/// The joint's motion in A's frame for its values. +pub fn motion(kind: JointKind, q: &[f64]) -> Mat { + let v = |i: usize| q.get(i).copied().unwrap_or(0.0); + match kind { + JointKind::Rigid => IDENTITY, + JointKind::Revolute => rot_z(v(0)), + JointKind::Slider => shift(0.0, 0.0, v(0)), + JointKind::Cylindrical => mat_mul(&rot_z(v(0)), &shift(0.0, 0.0, v(1))), + JointKind::PinSlot => mat_mul(&shift(v(1), 0.0, 0.0), &rot_z(v(0))), + JointKind::Planar => mat_mul(&shift(v(0), v(1), 0.0), &rot_z(v(2))), + JointKind::Ball => mat_mul(&mat_mul(&rot_z(v(0)), &rot_y(v(1))), &rot_z(v(2))), + } +} + +/// Bodies of a component in the model (its own frame). +fn component_bodies<'a>(doc: &Document, st: &'a ModelState, component: u64) -> Vec<&'a crate::ModelBody> { + st.bodies.iter().filter(|b| doc.body_component(&b.name, b.feature) == component).collect() +} + +/// The component an occurrence places (0 for the root). +fn component_of(doc: &Document, occ: u64) -> Result { + if occ == 0 { + return Ok(0); + } + doc.occurrences.iter().find(|o| o.id == occ).map(|o| o.component).ok_or_else(|| DocError::Unknown(format!("occurrence {occ}"))) +} + +/// Resolve a joint origin to a frame in its component's coordinates. +pub fn resolve_origin(doc: &Document, st: &ModelState, o: &JointOrigin) -> Result { + let bad = || DocError::Invalid("joint origin: degenerate frame".into()); + match &o.snap { + Snap::Frame { origin, z, x } => frame(*origin, *z, *x).ok_or_else(bad), + Snap::Point { point, z } => frame(*point, z.unwrap_or(Vec3::Z), None).ok_or_else(bad), + Snap::FaceCenter { pick } => { + let comp = component_of(doc, o.occurrence)?; + let mut best: Option<(f64, Vec3, Vec3)> = None; + for b in component_bodies(doc, st, comp) { + let tol = (b.body.size() * 2e-3).max(1e-3); + for f in b.body.faces(tol)? { + let Some(n) = f.plane_normal else { continue }; + let d = (*pick - f.centroid).dot(n).abs() + 1e-3 * (*pick - f.centroid).len(); + if best.as_ref().is_none_or(|(bd, _, _)| d < *bd) { + best = Some((d, f.centroid, n)); + } + } + } + let (_, c, n) = best.ok_or_else(|| DocError::Invalid("joint origin: no planar face there".into()))?; + frame(c, n, None).ok_or_else(bad) + } + Snap::CircleCenter { pick } => { + let comp = component_of(doc, o.occurrence)?; + let cyl = component_bodies(doc, st, comp) + .into_iter() + .filter_map(|b| solvecraft_kernel::cylinder_face_at(&b.body, *pick)) + .min_by(|a, b| { + let off = |c: &solvecraft_kernel::CylinderFace| { + let v = *pick - c.axis_point; + ((v - c.axis * v.dot(c.axis)).len() - c.radius).abs() + }; + off(a).total_cmp(&off(b)) + }) + .ok_or_else(|| DocError::Invalid("joint origin: no cylindrical face there".into()))?; + let s = (*pick - cyl.axis_point).dot(cyl.axis); + let (at, dir) = if (s - cyl.start).abs() <= (cyl.end - s).abs() { (cyl.start, -1.0) } else { (cyl.end, 1.0) }; + frame(cyl.axis_point + cyl.axis * at, cyl.axis * dir, None).ok_or_else(bad) + } + } +} + +/// World transform of an occurrence (through its parents). +pub fn occurrence_world(doc: &Document, occ: u64) -> Mat { + let mut m = IDENTITY; + let mut cur = doc.occurrences.iter().find(|o| o.id == occ); + for _ in 0..1000 { + let Some(o) = cur else { break }; + m = mat_mul(&o.transform, &m); + if o.parent == 0 { + break; + } + cur = doc.occurrence_of(o.parent); + } + m +} + +/// Clamp values to the joint's limits. +pub fn clamp(j: &Joint, q: &mut [f64]) { + for (i, v) in q.iter_mut().enumerate() { + if let Some(Some((lo, hi))) = j.limits.get(i) { + *v = v.clamp(lo.min(*hi), hi.max(*lo)); + } + } +} + +/// The result of placing occurrences by their joints. +#[derive(Clone, Debug, Default, Serialize)] +pub struct Solution { + /// New world transforms of occurrences the joints place. + #[serde(skip)] + pub world: BTreeMap, + /// Joints that disagree with the placement found through others: (joint id, name, why). + pub conflicts: Vec<(u64, String, String)>, + /// Joints that could not be applied (bad origin…): (joint id, name, why). + pub errors: Vec<(u64, String, String)>, + /// Remaining degrees of freedom per occurrence (6 = free, 0 = fixed). + pub dof: BTreeMap, + /// Occurrences fixed in place (grounded, or the root's). + pub fixed: Vec, +} + +/// How far two placements differ: (translation mm, rotation rad). +fn mismatch(a: &Mat, b: &Mat) -> (f64, f64) { + let t = Vec3::new(a[3][0] - b[3][0], a[3][1] - b[3][1], a[3][2] - b[3][2]).len(); + let mut tr = 0.0; + for i in 0..3 { + for k in 0..3 { + tr += a[i][k] * b[i][k]; + } + } + (t, ((tr - 1.0) / 2.0).clamp(-1.0, 1.0).acos()) +} + +/// Effective values of every joint after motion links and limits. +pub fn joint_values(asm: &Assembly) -> BTreeMap> { + let mut vals: BTreeMap> = asm + .joints + .iter() + .map(|j| { + let mut q = j.values.clone(); + q.resize(j.kind.dofs().len(), 0.0); + (j.id, q) + }) + .collect(); + // Links in order, a few passes for chains. + for _ in 0..4 { + for l in &asm.links { + let Some(src) = vals.get(&l.a).and_then(|q| q.get(l.ia)).copied() else { continue }; + if let Some(v) = vals.get_mut(&l.b).and_then(|q| q.get_mut(l.ib)) { + *v = l.ratio * src + l.offset; + } + } + } + for j in &asm.joints { + if let Some(q) = vals.get_mut(&j.id) { + clamp(j, q); + } + } + vals +} + +/// Place occurrences from the joint graph. +pub fn solve(doc: &Document, st: &ModelState) -> Solution { + let asm = &doc.assembly; + let mut sol = Solution::default(); + let joints: Vec<&Joint> = asm.joints.iter().filter(|j| !j.suppressed).take(MAX_JOINTS).collect(); + let vals = joint_values(asm); + // Joint frames in component coordinates (errors leave the joint out). + let mut frames: BTreeMap = BTreeMap::new(); + for j in &joints { + match (resolve_origin(doc, st, &j.a), resolve_origin(doc, st, &j.b)) { + (Ok(fa), Ok(fb)) => { + frames.insert(j.id, (fa, fb)); + } + (Err(e), _) | (_, Err(e)) => sol.errors.push((j.id, j.name.clone(), e.to_string())), + } + } + // Fixed: the root, grounded occurrences; with neither involved, the first joint's A side. + let mut placed: BTreeMap = BTreeMap::new(); + let mut dof: BTreeMap = BTreeMap::new(); + placed.insert(0, IDENTITY); + dof.insert(0, 0); + for o in doc.occurrences.iter().filter(|o| o.grounded) { + placed.insert(o.id, occurrence_world(doc, o.id)); + dof.insert(o.id, 0); + } + let involved: BTreeSet = joints.iter().flat_map(|j| [j.a.occurrence, j.b.occurrence]).collect(); + if !involved.iter().any(|o| placed.contains_key(o)) + && let Some(j) = joints.first() + { + placed.insert(j.a.occurrence, occurrence_world(doc, j.a.occurrence)); + dof.insert(j.a.occurrence, 0); + } + sol.fixed = placed.keys().copied().collect(); + let flip_m = |j: &Joint| { + let mut m = rot_z(j.angle); + m[3][2] = j.offset; + 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 } + }; + // B's world transform from A's (or A's from B's, going the other way). + let through = |j: &Joint, from_a: bool, t: &Mat| -> Option { + let (fa, fb) = frames.get(&j.id)?; + let q = vals.get(&j.id).cloned().unwrap_or_default(); + let link = mat_mul(&mat_mul(&motion(j.kind, &q), &flip_m(j)), &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)?)) + } + }; + let mut used: BTreeSet = BTreeSet::new(); + let mut queue: VecDeque = placed.keys().copied().collect(); + while let Some(occ) = queue.pop_front() { + let Some(t) = placed.get(&occ).copied() else { continue }; + for j in &joints { + if used.contains(&j.id) || !frames.contains_key(&j.id) { + continue; + } + let (other, from_a) = if j.a.occurrence == occ { + (j.b.occurrence, true) + } else if j.b.occurrence == occ { + (j.a.occurrence, false) + } else { + continue; + }; + if placed.contains_key(&other) { + continue; + } + let Some(m) = through(j, from_a, &t) else { continue }; + used.insert(j.id); + placed.insert(other, m); + dof.insert(other, dof.get(&occ).copied().unwrap_or(0) + j.kind.dofs().len()); + queue.push_back(other); + } + } + // Joints that close loops: do they agree? + for j in &joints { + if used.contains(&j.id) || !frames.contains_key(&j.id) { + continue; + } + let (Some(ta), Some(tb)) = (placed.get(&j.a.occurrence), placed.get(&j.b.occurrence)) else { continue }; + let Some(want) = through(j, true, ta) else { continue }; + let (dt, da) = mismatch(&want, tb); + if dt > 1e-6 || da > 1e-8 { + sol.conflicts.push(( + j.id, + j.name.clone(), + format!("off by {dt:.4} mm and {:.3}° from the placement the other joints give", da.to_degrees()), + )); + } + } + for o in &doc.occurrences { + dof.entry(o.id).or_insert(6); + } + placed.remove(&0); + sol.world = placed; + sol.dof = dof; + sol +} + +impl Document { + /// Apply a solution: occurrence transforms (in their parents) for the occurrences it places. + /// Returns true when something moved. + pub fn apply_joint_solution(&mut self, sol: &Solution) -> bool { + let mut changed = false; + let parents: BTreeMap = self.occurrences.iter().map(|o| (o.id, o.parent)).collect(); + 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) }; + 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) { + let (dt, da) = mismatch(&o.transform, &local); + if dt > 1e-9 || da > 1e-10 { + o.transform = local; + changed = true; + } + } + } + changed + } + + /// A frame for an as-built joint: the world frame `w` seen from an occurrence. + pub fn frame_in_occurrence(&self, occ: u64, w: &Mat) -> Option { + let t = if occ == 0 { IDENTITY } else { occurrence_world(self, occ) }; + let local = mat_mul(&mat_inverse(&t)?, w); + let o = apply_point(&local, Vec3::ZERO); + let z = apply_vector(&local, Vec3::Z); + let x = apply_vector(&local, Vec3::X); + Some(Snap::Frame { origin: o, z, x: Some(x) }) + } + + /// A joint origin's frame in the world (for as-built joints and display). + pub fn origin_world(&self, st: &ModelState, o: &JointOrigin) -> Result { + let f = resolve_origin(self, st, o)?; + let t = if o.occurrence == 0 { IDENTITY } else { occurrence_world(self, o.occurrence) }; + Ok(mat_mul(&t, &f)) + } +} + +/// The frame matrix for an explicit origin / z / x (public for commands). +pub fn frame_of(o: Vec3, z: Vec3, x: Option) -> Option { + frame(o, z, x) +} + +#[cfg(test)] +mod tests { + use super::*; + + fn close(a: &Mat, b: &Mat) -> bool { + let (t, r) = mismatch(a, b); + t < 1e-9 && r < 1e-9 + } + + #[test] + fn motions_compose() { + assert!(close(&motion(JointKind::Revolute, &[std::f64::consts::FRAC_PI_2]), &rot_z(std::f64::consts::FRAC_PI_2))); + let m = motion(JointKind::Cylindrical, &[0.0, 5.0]); + assert!((m[3][2] - 5.0).abs() < 1e-12); + let m = motion(JointKind::PinSlot, &[0.0, 3.0]); + assert!((m[3][0] - 3.0).abs() < 1e-12); + assert_eq!(JointKind::Ball.dofs().len(), 3); + assert!(JointKind::parse("Pin Slot").is_some() && JointKind::parse("nope").is_none()); + let f = frame(Vec3::new(1.0, 2.0, 3.0), Vec3::Z, None).unwrap(); + assert!(close(&mat_mul(&f, &mat_inverse(&f).unwrap()), &IDENTITY)); + } +} diff --git a/crates/doc/src/lib.rs b/crates/doc/src/lib.rs index 0ad015f0..d281188e 100644 --- a/crates/doc/src/lib.rs +++ b/crates/doc/src/lib.rs @@ -10,6 +10,7 @@ mod clipboard; mod document; mod eval; pub mod expr; +pub mod joints; mod params; pub mod project; mod project3d; diff --git a/crates/engine/src/cmd/joints.rs b/crates/engine/src/cmd/joints.rs new file mode 100644 index 00000000..b6db903e --- /dev/null +++ b/crates/engine/src/cmd/joints.rs @@ -0,0 +1,548 @@ +//! Joints between occurrences (`solvecraft_doc::joints`): Joint Origin, Joint, As-built Joint, +//! Rigid Group, Drive Joints, Motion Link, limits, and Interference. Every change re-solves the +//! joint graph and moves the occurrences it places; the session re-solves after any edit too, +//! so joints follow the geometry their origins snap to. + +use serde_json::{Value, json}; +use solvecraft_doc::expr::Kind; +use solvecraft_doc::joints::{self, Joint, JointKind, JointOrigin, MotionLink, NamedOrigin, Snap}; +use solvecraft_geom::Vec3; + +use super::CommandSpec; +use crate::params::{bad, bool_, num, str_, string_list, vec3}; +use crate::{EngineError, Result, Session}; + +pub static COMMANDS: &[CommandSpec] = &[ + CommandSpec::new("JointOrigin", "Joint Origin", joint_origin).at("SOLID", "CREATE").icon("joint").params( + "name?, occurrence?: id|name (default: from the pick), face: [x,y,z] (centre of a planar face) | circle: [x,y,z] (centre of a round end) | point: [x,y,z], z?: [x,y,z]", + ), + CommandSpec::new("JointAssembleCmdNew", "Joint", joint).at("SOLID", "ASSEMBLE").icon("joint").key("J").params( + "type: rigid|revolute|slider|cylindrical|pin_slot|planar|ball; a, b: joint origins ({occurrence, face|circle|point: [x,y,z] in world} | origin name); \ + values?: [angles in deg or expressions, distances]; limits?: [[min, max] | null…]; flip?: bool; offset?; angle?; name?", + ), + CommandSpec::new("JointAsBuiltCmd", "As-Built Joint", as_built).at("SOLID", "ASSEMBLE").icon("joint").params( + "type, a: occurrence, b: occurrence (stay where they are); at?: joint origin (default: b's origin) — the joint's frame", + ), + CommandSpec::new("RigidGroupCmd", "Rigid Group", rigid_group).at("SOLID", "ASSEMBLE").icon("joint").params("occurrences: [ids or names] (move together, as they are)"), + CommandSpec::new("FusionMoveJointsCommand", "Drive Joints", drive).at("SOLID", "ASSEMBLE").icon("joint").params( + "joint: id|name; value | values: [angles in deg or expressions, distances] (clamped to the limits)", + ), + CommandSpec::new("FusionMotionRelationshipCommand", "Motion Link", motion_link) + .at("SOLID", "ASSEMBLE") + .icon("joint") + .params("a, b: joints; ratio (b = ratio · a + offset); offset?; ia?, ib?: value indices (default 0)"), + CommandSpec::new("joint.limits", "Joint Limits", limits).params("joint; index?: value index (default 0); min?, max? (omit both to clear)"), + CommandSpec::new("joint.edit", "Edit Joint", edit).params("joint; type?, flip?, offset?, angle?, suppressed?, name?"), + CommandSpec::new("joint.delete", "Delete Joint", delete).params("joint: id|name (or origin: name, link: index)"), + CommandSpec::new("joint.list", "List Joints", list).noundo().params("→ joints (type, occurrences, values, limits), origins, links, degrees of freedom, conflicts"), + CommandSpec::new("joint.solve", "Solve Joints", solve_cmd).params("re-place the occurrences from the joints → conflicts, errors, dof"), + CommandSpec::new("InterferenceCheckCommand", "Interference", interference).at("SOLID", "INSPECT").icon("measure").noundo().params( + "bodies?: [names] (default: all, in world placement) → pairs that overlap with the overlap volume, area, bounding box and centre", + ), +]; + +/// Re-solve the joints and move the occurrences they place (after any edit). +pub fn resolve(s: &mut Session) -> joints::Solution { + let st = s.model.state(); + let sol = joints::solve(&s.doc, &st); + if !sol.world.is_empty() { + let mut next = (*s.doc).clone(); + if next.apply_joint_solution(&sol) { + *s.doc_mut() = next; + s.revision += 1; + } + } + sol +} + +fn sol_json(sol: &joints::Solution) -> Value { + json!({ + "conflicts": sol.conflicts.iter().map(|(id, n, w)| json!({"joint": id, "name": n, "why": w})).collect::>(), + "errors": sol.errors.iter().map(|(id, n, w)| json!({"joint": id, "name": n, "why": w})).collect::>(), + "dof": sol.dof.iter().map(|(o, d)| json!({"occurrence": o, "dof": d})).collect::>(), + "fixed": sol.fixed, + }) +} + +fn occurrence_id(s: &Session, v: Option<&Value>, cmd: &str) -> Result { + let key = match v { + Some(Value::Number(n)) => n.to_string(), + Some(Value::String(x)) => x.clone(), + _ => return Err(bad(cmd, "an occurrence must be an id or a name")), + }; + if key == "0" || key.eq_ignore_ascii_case("root") { + return Ok(0); + } + if let Some(o) = s.doc.occurrences.iter().find(|o| o.id.to_string() == key || o.name == key) { + return Ok(o.id); + } + // A component name: its first occurrence. + s.doc.find_component(&key).and_then(|c| s.doc.occurrence_of(c)).map(|o| o.id).ok_or_else(|| bad(cmd, format!("no occurrence `{key}`"))) +} + +/// The occurrence whose bodies are nearest a world point (0 = root bodies). +fn occurrence_at(s: &Session, p: Vec3) -> Option { + let st = s.model.state(); + let mut best: Option<(f64, u64)> = None; + for b in &st.bodies { + let comp = s.doc.body_component(&b.name, b.feature); + let occs: Vec = if comp == 0 { vec![0] } else { s.doc.occurrences.iter().filter(|o| o.component == comp).map(|o| o.id).collect() }; + for occ in occs { + let t = if occ == 0 { solvecraft_doc::IDENTITY } else { joints::occurrence_world(&s.doc, occ) }; + let inv = solvecraft_doc::mat_inverse(&t)?; + let q = solvecraft_doc::apply_point(&inv, p); + let bb = b.mesh().bounds(); + let c = Vec3::new(q.x.clamp(bb.min.x, bb.max.x), q.y.clamp(bb.min.y, bb.max.y), q.z.clamp(bb.min.z, bb.max.z)); + let d = c.dist(q); + if best.is_none_or(|(bd, _)| d < bd) { + best = Some((d, occ)); + } + } + } + best.map(|(_, o)| o) +} + +/// A joint origin from parameters: a named origin, or a snap given in world coordinates. +fn origin_param(s: &Session, v: Option<&Value>, cmd: &str) -> Result { + let v = v.ok_or_else(|| bad(cmd, "joint origins `a` and `b` are required"))?; + if let Value::String(name) = v { + return s + .doc + .assembly + .origins + .iter() + .find(|o| &o.name == name) + .map(|o| o.origin.clone()) + .ok_or_else(|| bad(cmd, format!("no joint origin `{name}`"))); + } + let pick = ["face", "circle", "point"].iter().find_map(|k| v.get(*k).and_then(vec3).map(|p| (*k, p))); + let (what, world) = match (pick, v.get("frame")) { + (Some(x), _) => x, + (None, Some(f)) => ("frame", f.get("origin").and_then(vec3).ok_or_else(|| bad(cmd, "`frame` needs origin and z"))?), + _ => return Err(bad(cmd, "a joint origin needs face, circle, point or frame [x, y, z]")), + }; + let occurrence = match v.get("occurrence") { + Some(o) => occurrence_id(s, Some(o), cmd)?, + None => occurrence_at(s, world).ok_or_else(|| bad(cmd, "no body there"))?, + }; + // World → the component's own coordinates. + let t = if occurrence == 0 { solvecraft_doc::IDENTITY } else { joints::occurrence_world(&s.doc, occurrence) }; + let inv = solvecraft_doc::mat_inverse(&t).ok_or_else(|| bad(cmd, "occurrence transform"))?; + let local = |p: Vec3| solvecraft_doc::apply_point(&inv, p); + let local_dir = |d: Vec3| solvecraft_doc::apply_vector(&inv, d); + let snap = match what { + "face" => Snap::FaceCenter { pick: local(world) }, + "circle" => Snap::CircleCenter { pick: local(world) }, + "point" => Snap::Point { point: local(world), z: v.get("z").and_then(vec3).map(local_dir) }, + _ => { + let f = v.get("frame").cloned().unwrap_or_default(); + let z = f.get("z").and_then(vec3).ok_or_else(|| bad(cmd, "`frame` needs a z axis"))?; + Snap::Frame { origin: local(world), z: local_dir(z), x: f.get("x").and_then(vec3).map(local_dir) } + } + }; + let o = JointOrigin { occurrence, snap }; + joints::resolve_origin(&s.doc, &s.model.state(), &o).map_err(|e| bad(cmd, e.to_string()))?; + Ok(o) +} + +fn joint_id(s: &Session, v: Option<&Value>, cmd: &str) -> Result { + let key = match v { + Some(Value::Number(n)) => n.to_string(), + Some(Value::String(x)) => x.clone(), + _ => return Err(bad(cmd, "`joint` must be an id or a name")), + }; + s.doc.assembly.joints.iter().find(|j| j.id.to_string() == key || j.name == key).map(|j| j.id).ok_or_else(|| bad(cmd, format!("no joint `{key}`"))) +} + +/// A joint value: numbers are degrees for angles and mm for distances; text is an expression. +fn value(s: &Session, v: &Value, angle: bool, cmd: &str) -> Result { + let kind = if angle { Kind::Angle } else { Kind::Length }; + let e = match v { + Value::Number(n) => n.as_f64().filter(|x| x.is_finite() && x.abs() < 1e9).map(|x| x.to_string()), + Value::String(t) => Some(t.clone()), + _ => None, + } + .ok_or_else(|| bad(cmd, "values must be numbers or expressions"))?; + s.doc.eval(&e, kind).map_err(|e| bad(cmd, e.to_string())) +} + +fn values_param(s: &Session, p: &Value, kind: JointKind, cmd: &str) -> Result> { + let n = kind.dofs().len(); + let list: Vec = match (p.get("values"), p.get("value")) { + (Some(Value::Array(a)), _) => a.clone(), + (_, Some(v)) => vec![v.clone()], + _ => Vec::new(), + }; + if list.len() > n { + return Err(bad(cmd, format!("a {kind:?} joint has {n} values ({})", kind.dofs().join(", ")))); + } + let mut out = vec![0.0; n]; + for (i, v) in list.iter().enumerate() { + if let Some(slot) = out.get_mut(i) { + *slot = value(s, v, kind.is_angle(i), cmd)?; + } + } + Ok(out) +} + +fn limits_param(s: &Session, p: &Value, kind: JointKind, cmd: &str) -> Result>> { + let n = kind.dofs().len(); + let mut out = vec![None; n]; + if let Some(Value::Array(a)) = p.get("limits") { + if a.len() > n { + return Err(bad(cmd, "more limits than values")); + } + for (i, l) in a.iter().enumerate() { + if let (Some(lo), Some(hi)) = (l.get(0), l.get(1)) + && let Some(slot) = out.get_mut(i) + { + *slot = Some((value(s, lo, kind.is_angle(i), cmd)?, value(s, hi, kind.is_angle(i), cmd)?)); + } + } + } + Ok(out) +} + +fn next_joint_id(s: &Session) -> u64 { + s.doc.assembly.joints.iter().map(|j| j.id).max().unwrap_or(0) + 1 +} + +fn add_joint(s: &mut Session, j: Joint) -> Result { + if s.doc.assembly.joints.len() >= 10_000 { + return Err(EngineError::Other("too many joints".into())); + } + if j.a.occurrence == j.b.occurrence { + return Err(bad("JointAssembleCmdNew", "a joint needs two different occurrences")); + } + let id = j.id; + let name = j.name.clone(); + s.doc_mut().assembly.joints.push(j); + let sol = resolve(s); + let mut out = sol_json(&sol); + out["joint"] = json!(id); + out["name"] = json!(name); + Ok(out) +} + +fn joint_origin(s: &mut Session, p: &Value) -> Result { + let cmd = "JointOrigin"; + let origin = origin_param(s, Some(p), cmd)?; + let name = match str_(p, "name").map(str::trim).filter(|n| !n.is_empty()) { + Some(n) => n.to_string(), + None => (1..).map(|k| format!("Joint Origin{k}")).find(|n| !s.doc.assembly.origins.iter().any(|o| &o.name == n)).unwrap_or_default(), + }; + if s.doc.assembly.origins.iter().any(|o| o.name == name) { + return Err(bad(cmd, format!("there is already a joint origin `{name}`"))); + } + s.doc_mut().assembly.origins.push(NamedOrigin { name: name.clone(), origin }); + Ok(json!({"origin": name})) +} + +fn kind_param(p: &Value, cmd: &str) -> Result { + str_(p, "type") + .and_then(JointKind::parse) + .ok_or_else(|| bad(cmd, "`type` must be rigid, revolute, slider, cylindrical, pin_slot, planar or ball")) +} + +fn joint(s: &mut Session, p: &Value) -> Result { + let cmd = "JointAssembleCmdNew"; + let kind = kind_param(p, cmd)?; + let a = origin_param(s, p.get("a"), cmd)?; + let b = origin_param(s, p.get("b"), cmd)?; + let id = next_joint_id(s); + let name = str_(p, "name").map(str::to_string).unwrap_or_else(|| format!("Joint{id}")); + let offset = match p.get("offset") { + Some(v) => value(s, v, false, cmd)?, + None => 0.0, + }; + let angle = match p.get("angle") { + Some(v) => value(s, v, true, cmd)?, + None => 0.0, + }; + let j = Joint { + id, + name, + kind, + a, + b, + values: values_param(s, p, kind, cmd)?, + limits: limits_param(s, p, kind, cmd)?, + flip: bool_(p, "flip").unwrap_or(false), + offset, + angle, + suppressed: false, + }; + add_joint(s, j) +} + +fn as_built(s: &mut Session, p: &Value) -> Result { + let cmd = "JointAsBuiltCmd"; + let kind = kind_param(p, cmd)?; + let (oa, ob) = (occurrence_id(s, p.get("a"), cmd)?, occurrence_id(s, p.get("b"), cmd)?); + // The joint's frame in the world: the given origin, or B's component origin. + let w = match p.get("at") { + Some(v) => { + let o = origin_param(s, Some(v), cmd)?; + s.doc.origin_world(&s.model.state(), &o).map_err(|e| bad(cmd, e.to_string()))? + } + None => joints::occurrence_world(&s.doc, ob), + }; + let fa = s.doc.frame_in_occurrence(oa, &w).ok_or_else(|| bad(cmd, "occurrence transform"))?; + let fb = s.doc.frame_in_occurrence(ob, &w).ok_or_else(|| bad(cmd, "occurrence transform"))?; + let id = next_joint_id(s); + let name = str_(p, "name").map(str::to_string).unwrap_or_else(|| format!("Joint{id}")); + let n = kind.dofs().len(); + let j = Joint { + id, + name, + kind, + a: JointOrigin { occurrence: oa, snap: fa }, + b: JointOrigin { occurrence: ob, snap: fb }, + values: vec![0.0; n], + limits: limits_param(s, p, kind, cmd)?, + flip: false, + offset: 0.0, + angle: 0.0, + suppressed: false, + }; + add_joint(s, j) +} + +fn rigid_group(s: &mut Session, p: &Value) -> Result { + let cmd = "RigidGroupCmd"; + let list = p.get("occurrences").and_then(Value::as_array).cloned().unwrap_or_default(); + if list.len() < 2 || list.len() > 1000 { + return Err(bad(cmd, "`occurrences` must list 2…1000 occurrences")); + } + let ids: Vec = list.iter().map(|v| occurrence_id(s, Some(v), cmd)).collect::>()?; + let first = ids.first().copied().unwrap_or(0); + let mut made = Vec::new(); + for other in ids.iter().skip(1) { + let r = as_built(s, &json!({"type": "rigid", "a": first, "b": other, "name": format!("Rigid Group {first}:{other}")}))?; + made.push(r["joint"].clone()); + } + Ok(json!({"joints": made})) +} + +fn drive(s: &mut Session, p: &Value) -> Result { + let cmd = "FusionMoveJointsCommand"; + let id = joint_id(s, p.get("joint"), cmd)?; + let kind = s.doc.assembly.joints.iter().find(|j| j.id == id).map(|j| j.kind).ok_or_else(|| bad(cmd, "joint"))?; + if kind == JointKind::Rigid { + return Err(bad(cmd, "a rigid joint does not move")); + } + let mut q = values_param(s, p, kind, cmd)?; + if p.get("values").is_none() && p.get("value").is_none() { + return Err(bad(cmd, "give `value` or `values`")); + } + let doc = s.doc_mut(); + let Some(j) = doc.assembly.joints.iter_mut().find(|j| j.id == id) else { return Err(bad(cmd, "joint")) }; + // Values not given keep theirs. + if let Some(Value::Array(a)) = p.get("values") { + for (i, v) in q.iter_mut().enumerate().skip(a.len()) { + *v = j.values.get(i).copied().unwrap_or(*v); + } + } + joints::clamp(j, &mut q); + j.values = q.clone(); + let sol = resolve(s); + let mut out = sol_json(&sol); + out["joint"] = json!(id); + out["values"] = json!(q); + Ok(out) +} + +fn motion_link(s: &mut Session, p: &Value) -> Result { + let cmd = "FusionMotionRelationshipCommand"; + let (a, b) = (joint_id(s, p.get("a"), cmd)?, joint_id(s, p.get("b"), cmd)?); + if a == b { + return Err(bad(cmd, "link two different joints")); + } + let ratio = num(p, "ratio").ok_or_else(|| bad(cmd, "`ratio` must be a number"))?; + let (ia, ib) = (num(p, "ia").unwrap_or(0.0).max(0.0) as usize, num(p, "ib").unwrap_or(0.0).max(0.0) as usize); + let dofs = |id: u64| s.doc.assembly.joints.iter().find(|j| j.id == id).map(|j| j.kind.dofs().len()).unwrap_or(0); + if ia >= dofs(a) || ib >= dofs(b) { + return Err(bad(cmd, "the linked joints need moving values at those indices")); + } + let offset = num(p, "offset").unwrap_or(0.0); + s.doc_mut().assembly.links.push(MotionLink { a, ia, b, ib, ratio, offset }); + let sol = resolve(s); + Ok(sol_json(&sol)) +} + +fn limits(s: &mut Session, p: &Value) -> Result { + let cmd = "joint.limits"; + let id = joint_id(s, p.get("joint"), cmd)?; + let i = num(p, "index").unwrap_or(0.0).max(0.0) as usize; + let kind = s.doc.assembly.joints.iter().find(|j| j.id == id).map(|j| j.kind).ok_or_else(|| bad(cmd, "joint"))?; + if i >= kind.dofs().len() { + return Err(bad(cmd, "no such value")); + } + let lim = match (p.get("min"), p.get("max")) { + (Some(lo), Some(hi)) => Some((value(s, lo, kind.is_angle(i), cmd)?, value(s, hi, kind.is_angle(i), cmd)?)), + (None, None) => None, + _ => return Err(bad(cmd, "give both `min` and `max`, or neither to clear")), + }; + let doc = s.doc_mut(); + let Some(j) = doc.assembly.joints.iter_mut().find(|j| j.id == id) else { return Err(bad(cmd, "joint")) }; + j.limits.resize(kind.dofs().len(), None); + if let Some(slot) = j.limits.get_mut(i) { + *slot = lim; + } + let mut q = j.values.clone(); + q.resize(kind.dofs().len(), 0.0); + joints::clamp(j, &mut q); + j.values = q; + let sol = resolve(s); + Ok(sol_json(&sol)) +} + +fn edit(s: &mut Session, p: &Value) -> Result { + let cmd = "joint.edit"; + let id = joint_id(s, p.get("joint"), cmd)?; + let kind = match str_(p, "type") { + Some(t) => Some(JointKind::parse(t).ok_or_else(|| bad(cmd, "unknown joint type"))?), + None => None, + }; + let offset = match p.get("offset") { + Some(v) => Some(value(s, v, false, cmd)?), + None => None, + }; + let angle = match p.get("angle") { + Some(v) => Some(value(s, v, true, cmd)?), + None => None, + }; + let doc = s.doc_mut(); + let Some(j) = doc.assembly.joints.iter_mut().find(|j| j.id == id) else { return Err(bad(cmd, "joint")) }; + if let Some(k) = kind { + j.kind = k; + j.values.resize(k.dofs().len(), 0.0); + j.limits.resize(k.dofs().len(), None); + } + if let Some(o) = offset { + j.offset = o; + } + if let Some(a) = angle { + j.angle = a; + } + if let Some(f) = bool_(p, "flip") { + j.flip = f; + } + if let Some(x) = bool_(p, "suppressed") { + j.suppressed = x; + } + if let Some(n) = str_(p, "name").map(str::trim).filter(|n| !n.is_empty() && n.len() <= 128) { + j.name = n.to_string(); + } + let sol = resolve(s); + Ok(sol_json(&sol)) +} + +fn delete(s: &mut Session, p: &Value) -> Result { + let cmd = "joint.delete"; + if let Some(name) = str_(p, "origin") { + let used = s.doc.assembly.joints.len(); + let doc = s.doc_mut(); + let before = doc.assembly.origins.len(); + doc.assembly.origins.retain(|o| o.name != name); + if doc.assembly.origins.len() == before { + return Err(bad(cmd, format!("no joint origin `{name}`"))); + } + return Ok(json!({"deleted_origin": name, "joints": used})); + } + if let Some(i) = num(p, "link") { + let i = i.max(0.0) as usize; + let doc = s.doc_mut(); + if i >= doc.assembly.links.len() { + return Err(bad(cmd, "no such motion link")); + } + doc.assembly.links.remove(i); + return Ok(json!({"deleted_link": i})); + } + let id = joint_id(s, p.get("joint"), cmd)?; + let doc = s.doc_mut(); + doc.assembly.joints.retain(|j| j.id != id); + doc.assembly.links.retain(|l| l.a != id && l.b != id); + let sol = resolve(s); + let mut out = sol_json(&sol); + out["deleted"] = json!(id); + Ok(out) +} + +fn list(s: &mut Session, _p: &Value) -> Result { + let st = s.model.state(); + let sol = joints::solve(&s.doc, &st); + let vals = joints::joint_values(&s.doc.assembly); + let js: Vec = s + .doc + .assembly + .joints + .iter() + .map(|j| { + let q = vals.get(&j.id).cloned().unwrap_or_default(); + let named: Vec = j + .kind + .dofs() + .iter() + .enumerate() + .map(|(i, n)| { + let v = q.get(i).copied().unwrap_or(0.0); + json!({"name": n, "value": if j.kind.is_angle(i) { v.to_degrees() } else { v }, "unit": if j.kind.is_angle(i) { "deg" } else { "mm" }, + "limits": j.limits.get(i).copied().flatten().map(|(a, b)| if j.kind.is_angle(i) { [a.to_degrees(), b.to_degrees()] } else { [a, b] })}) + }) + .collect(); + let world = |o: &JointOrigin| s.doc.origin_world(&st, o).ok().map(|m| json!({"origin": [m[3][0], m[3][1], m[3][2]], "z": [m[2][0], m[2][1], m[2][2]]})); + json!({"id": j.id, "name": j.name, "type": j.kind, "a": j.a.occurrence, "b": j.b.occurrence, "values": named, + "flip": j.flip, "offset": j.offset, "suppressed": j.suppressed, "frame_a": world(&j.a), "frame_b": world(&j.b)}) + }) + .collect(); + let links: Vec = + s.doc.assembly.links.iter().map(|l| json!({"a": l.a, "ia": l.ia, "b": l.b, "ib": l.ib, "ratio": l.ratio, "offset": l.offset})).collect(); + let origins: Vec = s.doc.assembly.origins.iter().map(|o| json!({"name": o.name, "occurrence": o.origin.occurrence})).collect(); + let mut out = sol_json(&sol); + out["joints"] = json!(js); + out["links"] = json!(links); + out["origins"] = json!(origins); + Ok(out) +} + +fn solve_cmd(s: &mut Session, _p: &Value) -> Result { + let sol = resolve(s); + Ok(sol_json(&sol)) +} + +fn interference(s: &mut Session, p: &Value) -> Result { + let cmd = "InterferenceCheckCommand"; + let want = string_list(p, "bodies"); + let st = s.world_state(); + let bodies: Vec<&solvecraft_doc::ModelBody> = + st.bodies.iter().filter(|b| want.is_empty() || want.contains(&b.name)).filter(|b| !b.body.is_mesh()).collect(); + if bodies.len() > 200 { + return Err(bad(cmd, "too many bodies (200 at most)")); + } + let mut hits = Vec::new(); + for (i, a) in bodies.iter().enumerate() { + for b in bodies.iter().skip(i + 1) { + let (ba, bb) = (a.mesh().bounds(), b.mesh().bounds()); + let apart = ba.max.x < bb.min.x + || bb.max.x < ba.min.x + || ba.max.y < bb.min.y + || bb.max.y < ba.min.y + || ba.max.z < bb.min.z + || bb.max.z < ba.min.z; + if apart { + continue; + } + let Ok(Some(o)) = solvecraft_kernel::boolean(&a.body, &b.body, solvecraft_kernel::BoolOp::Intersect) else { continue }; + let Ok(m) = solvecraft_kernel::measure(&o) else { continue }; + if m.volume > 1e-9 * a.body.size().max(1.0).powi(3) { + hits.push(json!({"a": a.name, "b": b.name, "volume_mm3": m.volume, "area_mm2": m.area, "center": m.centroid, "bbox": {"min": m.bbox.min, "max": m.bbox.max}})); + } + } + } + Ok(json!({"interferences": hits, "count": hits.len(), "checked": bodies.len()})) +} + +#[cfg(test)] +#[path = "joints_tests.rs"] +mod tests; diff --git a/crates/engine/src/cmd/joints_tests.rs b/crates/engine/src/cmd/joints_tests.rs new file mode 100644 index 00000000..2c6f3e10 --- /dev/null +++ b/crates/engine/src/cmd/joints_tests.rs @@ -0,0 +1,188 @@ +use std::f64::consts::FRAC_PI_2; + +use serde_json::{Value, json}; +use solvecraft_geom::Vec3; + +use crate::Session; + +fn run(s: &mut Session, id: &str, p: Value) -> Value { + match s.execute(id, &p) { + Ok(v) => v, + Err(e) => panic!("{id} {p}: {e}"), + } +} + +fn occ(s: &Session, component: &str) -> u64 { + let c = s.doc.find_component(component).unwrap_or(0); + s.doc.occurrence_of(c).map(|o| o.id).unwrap_or(0) +} + +fn place(s: &Session, component: &str, p: Vec3) -> Vec3 { + solvecraft_doc::apply_point(&solvecraft_doc::joints::occurrence_world(&s.doc, occ(s, component)), p) +} + +fn near(a: Vec3, b: Vec3) -> bool { + a.dist(b) < 1e-6 +} + +/// A hinge leaf (oracle 63's knuckle geometry): a 40 x 30 x 3 plate and an r4 knuckle along X +/// at y = 33, z = 4. +fn leaf(s: &mut Session, name: &str) { + run(s, "component.activate", json!({"component": "root"})); + run(s, "FusionCreateNewComponentCommand", json!({"name": name})); + run(s, "PrimitiveBox", json!({"length": 40, "width": 30, "height": 3, "corner": [0, 0, 0]})); + run(s, "PrimitiveCylinder", json!({"base": [0, 33, 4], "axis": [1, 0, 0], "radius": 4, "height": 40, "operation": "new"})); +} + +#[test] +fn revolute_hinge_turns_about_the_knuckle() { + let mut s = Session::default(); + leaf(&mut s, "LeafA"); + leaf(&mut s, "LeafB"); + let o = occ(&s, "LeafA"); + run(&mut s, "occurrence.ground", json!({"occurrence": o, "grounded": true})); + let r = run( + &mut s, + "JointAssembleCmdNew", + json!({ + "type": "revolute", "a": {"occurrence": "LeafA", "circle": [1, 37, 4]}, "b": {"occurrence": "LeafB", "circle": [1, 37, 4]}, "name": "Hinge"}), + ); + assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}"); + // Same frames: B stays put at angle 0. + assert!(near(place(&s, "LeafB", Vec3::new(20.0, 0.0, 0.0)), Vec3::new(20.0, 0.0, 0.0))); + let l = run(&mut s, "joint.list", json!({})); + let dof = l["dof"].as_array().unwrap().iter().find(|d| d["occurrence"] == occ(&s, "LeafB")).unwrap()["dof"].clone(); + assert_eq!(dof, 1, "{l}"); + // Drive 90°: B turns about the knuckle axis (through (0, 33, 4), along the frame's z = −X). + run(&mut s, "FusionMoveJointsCommand", json!({"joint": "Hinge", "value": 90})); + let want = solvecraft_doc::rigid(Vec3::ZERO, Vec3::new(0.0, 33.0, 4.0), Vec3::new(-1.0, 0.0, 0.0), FRAC_PI_2); + let p = Vec3::new(20.0, 0.0, 0.0); + assert!(near(place(&s, "LeafB", p), solvecraft_doc::apply_point(&want, p)), "{:?}", place(&s, "LeafB", p)); + assert!(near(place(&s, "LeafB", p), Vec3::new(20.0, 29.0, 37.0))); + // LeafA did not move; the knuckle axis is fixed. + assert!(near(place(&s, "LeafA", p), p)); + assert!(near(place(&s, "LeafB", Vec3::new(10.0, 33.0, 4.0)), Vec3::new(10.0, 33.0, 4.0))); + // Expressions drive too, and undo puts it back. + run(&mut s, "FusionMoveJointsCommand", json!({"joint": "Hinge", "value": "pi rad"})); + assert!(near(place(&s, "LeafB", p), Vec3::new(20.0, 66.0, 8.0))); + run(&mut s, "UndoCommand", json!({})); + assert!(near(place(&s, "LeafB", p), Vec3::new(20.0, 29.0, 37.0))); +} + +/// Two 10 mm cubes in components A (grounded) and B. +fn two_cubes() -> Session { + let mut s = Session::default(); + for (name, x) in [("A", 0), ("B", 20)] { + run(&mut s, "component.activate", json!({"component": "root"})); + run(&mut s, "FusionCreateNewComponentCommand", json!({"name": name})); + run(&mut s, "PrimitiveBox", json!({"length": 10, "width": 10, "height": 10, "corner": [x, 0, 0]})); + } + let o = occ(&s, "A"); + run(&mut s, "occurrence.ground", json!({"occurrence": o, "grounded": true})); + s +} + +#[test] +fn slider_limits_clamp() { + let mut s = two_cubes(); + // B's bottom face onto A's top face (faces meet), sliding along their common normal. + let r = run( + &mut s, + "JointAssembleCmdNew", + json!({"type": "slider", "a": {"face": [5, 5, 10]}, "b": {"face": [25, 5, 0]}, "flip": true, "limits": [[0, 10]], "name": "Rail"}), + ); + assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}"); + // B now sits on A. + assert!(near(place(&s, "B", Vec3::new(25.0, 5.0, 0.0)), Vec3::new(5.0, 5.0, 10.0)), "{:?}", place(&s, "B", Vec3::new(25.0, 5.0, 0.0))); + let r = run(&mut s, "FusionMoveJointsCommand", json!({"joint": "Rail", "value": 25})); + assert_eq!(r["values"], json!([10.0]), "{r}"); + assert!(near(place(&s, "B", Vec3::new(25.0, 5.0, 0.0)), Vec3::new(5.0, 5.0, 20.0))); + run(&mut s, "FusionMoveJointsCommand", json!({"joint": "Rail", "value": -5})); + assert!(near(place(&s, "B", Vec3::new(25.0, 5.0, 0.0)), Vec3::new(5.0, 5.0, 10.0))); + // Changing the limits clamps the current value. + run(&mut s, "FusionMoveJointsCommand", json!({"joint": "Rail", "value": 8})); + run(&mut s, "joint.limits", json!({"joint": "Rail", "min": 0, "max": 4})); + assert!(near(place(&s, "B", Vec3::new(25.0, 5.0, 0.0)), Vec3::new(5.0, 5.0, 14.0))); + // Joints follow edits: A gets taller, B rides on its new top. + let h = s + .doc + .features + .iter() + .find(|f| matches!(f.kind, solvecraft_doc::FeatureKind::Box { .. })) + .and_then(|f| f.param_names.get(2).cloned()) + .unwrap_or_default(); + run(&mut s, "ChangeParameterCommand", json!({"name": h, "expression": "15"})); + assert!(near(place(&s, "B", Vec3::new(25.0, 5.0, 0.0)), Vec3::new(5.0, 5.0, 19.0)), "{:?}", place(&s, "B", Vec3::new(25.0, 5.0, 0.0))); +} + +#[test] +fn rigid_cycles_solve_and_conflicts_report() { + let mut s = two_cubes(); + run(&mut s, "component.activate", json!({"component": "root"})); + run(&mut s, "FusionCreateNewComponentCommand", json!({"name": "C"})); + run(&mut s, "PrimitiveBox", json!({"length": 10, "width": 10, "height": 10, "corner": [40, 0, 0]})); + run(&mut s, "component.activate", json!({"component": "root"})); + // A rigid group and a closing as-built joint: a consistent loop. + run(&mut s, "RigidGroupCmd", json!({"occurrences": ["A", "B", "C"]})); + let r = run(&mut s, "JointAsBuiltCmd", json!({"type": "rigid", "a": "B", "b": "C"})); + assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}"); + assert!(near(place(&s, "C", Vec3::new(40.0, 0.0, 0.0)), Vec3::new(40.0, 0.0, 0.0))); + let l = run(&mut s, "joint.list", json!({})); + assert!(l["dof"].as_array().unwrap().iter().all(|d| d["dof"] == 0), "{l}"); + // A joint that wants C elsewhere conflicts and says so. + let r = run( + &mut s, + "JointAssembleCmdNew", + json!({"type": "rigid", "a": {"occurrence": "A", "point": [0, 0, 0]}, "b": {"occurrence": "C", "point": [40, 0, 0]}, "name": "Wrong"}), + ); + let c = r["conflicts"].as_array().cloned().unwrap_or_default(); + assert_eq!(c.len(), 1, "{r}"); + assert!(c[0]["why"].as_str().unwrap_or("").contains("off by"), "{r}"); + // Suppressing it clears the conflict. + let r = run(&mut s, "joint.edit", json!({"joint": "Wrong", "suppressed": true})); + assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0)); + assert!( + s.execute( + "JointAssembleCmdNew", + &json!({"type": "rigid", "a": {"occurrence": "A", "point": [0, 0, 0]}, "b": {"occurrence": "A", "point": [1, 0, 0]}}) + ) + .is_err() + ); + assert!(s.execute("JointAssembleCmdNew", &json!({"type": "hinge"})).is_err()); +} + +#[test] +fn motion_links_and_interference() { + let mut s = two_cubes(); + run( + &mut s, + "JointAssembleCmdNew", + json!({"type": "cylindrical", "a": {"face": [5, 5, 10]}, "b": {"face": [25, 5, 0]}, "flip": true, "name": "Screw"}), + ); + run(&mut s, "FusionMoveJointsCommand", json!({"joint": "Screw", "values": [0, 0]})); + // No overlap while B sits on A; then push B 4 mm into A. + let i = run(&mut s, "InterferenceCheckCommand", json!({})); + assert_eq!(i["count"], 0, "{i}"); + run(&mut s, "FusionMoveJointsCommand", json!({"joint": "Screw", "values": [0, -4]})); + let i = run(&mut s, "InterferenceCheckCommand", json!({})); + assert_eq!(i["count"], 1, "{i}"); + assert!((i["interferences"][0]["volume_mm3"].as_f64().unwrap_or(0.0) - 400.0).abs() < 1e-3, "{i}"); + // A motion link: a second joint's value follows. + run(&mut s, "component.activate", json!({"component": "root"})); + run(&mut s, "FusionCreateNewComponentCommand", json!({"name": "D"})); + run(&mut s, "PrimitiveBox", json!({"length": 10, "width": 10, "height": 10, "corner": [60, 0, 0]})); + run( + &mut s, + "JointAssembleCmdNew", + json!({"type": "slider", "a": {"occurrence": "A", "face": [5, 5, 10]}, "b": {"occurrence": "D", "face": [65, 5, 0]}, "flip": true, "name": "Follower"}), + ); + run(&mut s, "FusionMotionRelationshipCommand", json!({"a": "Screw", "ia": 1, "b": "Follower", "ratio": 2})); + run(&mut s, "FusionMoveJointsCommand", json!({"joint": "Screw", "values": [0, 3]})); + assert!(near(place(&s, "D", Vec3::new(65.0, 5.0, 0.0)), Vec3::new(5.0, 5.0, 16.0)), "{:?}", place(&s, "D", Vec3::new(65.0, 5.0, 0.0))); + // Saved and reopened, joints come back. + let back = solvecraft_doc::Document::from_json(&s.doc.to_json()).unwrap(); + assert_eq!(back.assembly, s.doc.assembly); + let mut t = Session::new(back); + let l = run(&mut t, "joint.list", json!({})); + assert_eq!(l["joints"].as_array().map(Vec::len), Some(2)); +} diff --git a/crates/engine/src/cmd/mod.rs b/crates/engine/src/cmd/mod.rs index 6c0b44fd..96a06f68 100644 --- a/crates/engine/src/cmd/mod.rs +++ b/crates/engine/src/cmd/mod.rs @@ -12,6 +12,7 @@ mod features_more; pub use features::auto_operation; mod file; mod inspect; +pub(crate) mod joints; mod measure_sel; mod section; pub(crate) use measure_sel::measure_items; @@ -126,6 +127,7 @@ pub fn command_specs() -> Vec<&'static CommandSpec> { v.extend(features::COMMANDS.iter()); v.extend(features_more::COMMANDS.iter()); v.extend(component::COMMANDS.iter()); + v.extend(joints::COMMANDS.iter()); v.extend(browser::COMMANDS.iter()); v.extend(edit::COMMANDS.iter()); v.extend(clipboard::COMMANDS.iter()); diff --git a/crates/engine/src/lib.rs b/crates/engine/src/lib.rs index 006556cd..130ba8c4 100644 --- a/crates/engine/src/lib.rs +++ b/crates/engine/src/lib.rs @@ -249,6 +249,10 @@ impl Session { pub fn refresh(&mut self) { self.model.evaluate(&self.doc); self.revision += 1; + // Joints follow the geometry their origins snap to. + if !self.doc.assembly.joints.is_empty() { + cmd::joints::resolve(self); + } if let Some(id) = self.active_sketch && self.doc.feature(id).is_none() {