Joints: relations that tie part of a placement (#2497)

Joints can tie part of a placement: parallel, angle, point on plane and more (#2496).
This commit is contained in:
NOWN-DEV
2026-10-19 22:08:23 +02:00
committed by GitHub
parent e1268c6ad9
commit b0b59480eb
9 changed files with 580 additions and 25 deletions
@@ -0,0 +1,6 @@
//! A joint can be a relation that ties only some directions or points of its two parts:
//! `parallel`, `perpendicular`, `angle` (its `angle` is then the tilt between the two z axes),
//! `point_on_plane`, `point_on_line` and `line_on_plane`, each with the values its free motion
//! needs. Several relations between one pair of parts close loops the solver meets together.
//! Older designs need no transformation; the last release asks for an update instead of meeting
//! a joint type it can't read.
+107 -11
View File
@@ -1,11 +1,13 @@
//! 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 (gears, racks and screws are
//! planar, ball, and relations that tie only part of a placement, `joints_relations.rs`) with
//! their motion values and limits, motion links (gears, racks and screws are
//! links whose ratio comes from radii or a pitch; a cam's follower follows a table worked out from
//! its profile, `joints_cam.rs`), 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`
//! `T_B · F_b = T_A · F_a · M(q) · Flip · Tail(q)`, so `T_B = T_A · F_a · M(q) · Flip · Tail(q) · F_b⁻¹`
//! (`Tail` is a relation's tilt and turns after the mate; the identity for the others), 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 (a four-bar
@@ -203,6 +205,20 @@ pub enum JointKind {
PinSlot,
Planar,
Ball,
/// B's z stays parallel to A's (against it unless flipped); B moves and turns about z freely.
Parallel,
/// B's z stays square to A's; B moves freely and turns about either z.
Perpendicular,
/// B's z stays at the joint's `angle` from mated (about their common x); B moves freely and
/// turns about either z.
Angle,
/// B's origin stays on A's xy plane (`offset` off it); B slides on it and turns any way.
PointOnPlane,
/// B's origin stays on A's z axis; B slides along it and turns any way.
PointOnLine,
/// B's z axis lies in A's xy plane (`offset` off it: a cylinder's radius makes it tangent);
/// B slides on the plane, turns about A's z and about its own axis.
LineOnPlane,
}
impl JointKind {
@@ -215,6 +231,12 @@ impl JointKind {
"pinslot" => JointKind::PinSlot,
"planar" => JointKind::Planar,
"ball" => JointKind::Ball,
"parallel" => JointKind::Parallel,
"perpendicular" | "square" => JointKind::Perpendicular,
"angle" => JointKind::Angle,
"pointonplane" | "pointonface" => JointKind::PointOnPlane,
"pointonline" | "pointonaxis" => JointKind::PointOnLine,
"lineonplane" | "lineonface" => JointKind::LineOnPlane,
_ => return None,
})
}
@@ -228,10 +250,29 @@ impl JointKind {
JointKind::PinSlot => &["angle", "slide"],
JointKind::Planar => &["x", "y", "angle"],
JointKind::Ball => &["yaw", "pitch", "roll"],
JointKind::Parallel => &["x", "y", "z", "angle"],
JointKind::Perpendicular | JointKind::Angle => &["x", "y", "z", "turn", "spin"],
JointKind::PointOnPlane => &["x", "y", "yaw", "pitch", "roll"],
JointKind::PointOnLine => &["slide", "yaw", "pitch", "roll"],
JointKind::LineOnPlane => &["x", "y", "turn", "spin"],
}
}
pub fn is_angle(self, i: usize) -> bool {
matches!(self.dofs().get(i), Some(&("angle" | "yaw" | "pitch" | "roll")))
matches!(self.dofs().get(i), Some(&("angle" | "yaw" | "pitch" | "roll" | "turn" | "spin")))
}
/// A relation that only ties some directions or points (parallel, on a plane…): made where
/// the parts are, its values start as the nearest that meet it, so B moves no more than it
/// must.
pub fn is_relation(self) -> bool {
matches!(
self,
JointKind::Parallel
| JointKind::Perpendicular
| JointKind::Angle
| JointKind::PointOnPlane
| JointKind::PointOnLine
| JointKind::LineOnPlane
)
}
}
@@ -599,6 +640,10 @@ 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_x(a: f64) -> Mat {
let (s, c) = a.sin_cos();
[[1.0, 0.0, 0.0, 0.0], [0.0, c, s, 0.0], [0.0, -s, c, 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]]
@@ -620,9 +665,40 @@ pub fn motion(kind: JointKind, q: &[f64]) -> Mat {
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))),
JointKind::Parallel => mat_mul(&shift(v(0), v(1), v(2)), &rot_z(v(3))),
JointKind::Perpendicular | JointKind::Angle => mat_mul(&shift(v(0), v(1), v(2)), &rot_z(v(3))),
// Their turn is about B's origin, after the offset (`tail`).
JointKind::PointOnPlane => shift(v(0), v(1), 0.0),
JointKind::PointOnLine => shift(0.0, 0.0, v(0)),
JointKind::LineOnPlane => mat_mul(&shift(v(0), v(1), 0.0), &rot_z(v(2))),
}
}
fn turn_zyx(yaw: f64, pitch: f64, roll: f64) -> Mat {
mat_mul(&mat_mul(&rot_z(yaw), &rot_y(pitch)), &rot_x(roll))
}
/// What follows the mate (`flip_of`) in a joint's link: for a relation that tilts B's z, the tilt
/// and B's turn about its own z (`spin`); for a point relation, B's turn about its origin (yaw,
/// pitch and roll about z, y and x: free of gimbal lock at the start, all 0); nothing for the
/// other joints.
fn tail(j: &Joint, q: &[f64]) -> Mat {
let v = |i: usize| q.get(i).copied().unwrap_or(0.0);
match j.kind {
JointKind::Perpendicular => mat_mul(&rot_x(std::f64::consts::FRAC_PI_2), &rot_z(v(4))),
JointKind::Angle => mat_mul(&rot_x(j.angle), &rot_z(v(4))),
JointKind::LineOnPlane => mat_mul(&rot_y(std::f64::consts::FRAC_PI_2), &rot_z(v(3))),
JointKind::PointOnPlane => turn_zyx(v(2), v(3), v(4)),
JointKind::PointOnLine => turn_zyx(v(1), v(2), v(3)),
_ => IDENTITY,
}
}
/// B's joint frame in A's for the joint's values: `M(q) · Flip · tail`.
fn link_of(j: &Joint, q: &[f64]) -> Mat {
mat_mul(&mat_mul(&motion(j.kind, q), &flip_of(j)), &tail(j, q))
}
/// 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.model_body_component(b) == component).collect()
@@ -854,14 +930,16 @@ pub fn joint_values(asm: &Assembly) -> BTreeMap<u64, Vec<f64>> {
/// The turn and offset from A's joint frame to B's: mated (B's z against A's, as faces meet)
/// unless flipped.
fn flip_of(j: &Joint) -> Mat {
let mut m = rot_z(j.angle);
// An angle relation's `angle` is its tilt (`tail`); a perpendicular one has none.
let turn = if matches!(j.kind, JointKind::Angle | JointKind::Perpendicular) { 0.0 } else { j.angle };
let mut m = rot_z(turn);
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) for the joint's values.
fn through(j: &Joint, (fa, fb): &(Mat, Mat), q: &[f64], from_a: bool, t: &Mat) -> Option<Mat> {
let link = mat_mul(&mat_mul(&motion(j.kind, q), &flip_of(j)), &mat_inverse(fb)?);
let link = mat_mul(&link_of(j, q), &mat_inverse(fb)?);
if from_a {
Some(mat_mul(&mat_mul(t, fa), &link))
} else {
@@ -928,6 +1006,10 @@ impl Tree<'_> {
/// on at most: larger ones are only checked.
const MAX_LOOP_VALUES: usize = 64;
const MAX_LOOPS: usize = 100;
/// Loop-closing joints one solve takes on at most, over all its groups: a part placed by several
/// relations closes a loop with each one after the first, so a panel of parts has many small
/// groups.
const MAX_CLOSING: usize = 5_000;
/// Residual evaluations one solve may spend on all its loops.
const MAX_LOOP_WORK: usize = 200_000;
@@ -984,13 +1066,13 @@ fn held_moves(asm: &Assembly, held: &Held, touches: &BTreeSet<(u64, usize)>, val
/// moves; a group that can't close without moving is left to the conflict check).
fn close_loops(tree: &Tree, closing: &[u64], asm: &Assembly, held: &Held, vals: &mut Values) -> (Values, BTreeSet<u64>) {
let (mut changed, mut stuck) = (BTreeMap::new(), BTreeSet::new());
if closing.len() > MAX_LOOPS {
if closing.len() > MAX_CLOSING {
return (Values::new(), stuck);
}
let linked = set_values(asm);
let groups = loop_groups(tree, closing, asm, &linked);
let spent = std::cell::Cell::new(0usize);
for (group, around, touches) in &groups {
for (group, around, touches) in groups.iter().filter(|(c, _, _)| c.len() <= MAX_LOOPS) {
let moves = held_moves(asm, held, touches, vals);
match close_group(tree, group, around, &moves, asm, held, &linked, &spent, vals) {
Some(m) => changed.extend(m),
@@ -1179,11 +1261,12 @@ fn nearby_pose(tree: &Tree, groups: &[Group], asm: &Assembly, linked: &BTreeSet<
/// pose alone counts 0. The tree's count stands where a loop group is over the solver's caps or
/// the work runs out.
fn loop_dof(tree: &Tree, closing: &[u64], asm: &Assembly, vals: &Values, dof: &mut BTreeMap<u64, usize>) {
if closing.len() > MAX_LOOPS {
if closing.len() > MAX_CLOSING {
return;
}
let linked = set_values(asm);
let groups = loop_groups(tree, closing, asm, &linked);
let mut groups = loop_groups(tree, closing, asm, &linked);
groups.retain(|(c, _, _)| c.len() <= MAX_LOOPS);
// The joints loops depend on: those around them, those whose values they read, and those a
// motion link carries any of these values into.
let mut relevant: BTreeSet<u64> = groups.iter().flat_map(|(_, a, t)| a.iter().copied().chain(t.iter().map(|(j, _)| *j))).collect();
@@ -1592,8 +1675,18 @@ struct Built<'a> {
fn build<'a>(doc: &'a Document, st: &ModelState, vals: &Values, sol: &mut Solution) -> Built<'a> {
let asm = &doc.assembly;
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).
// Joint frames in component coordinates (errors leave the joint out). An origin several joints
// share (a panel face every part sits on) is found once.
let mut frames: BTreeMap<u64, (Mat, Mat)> = BTreeMap::new();
let mut found: BTreeMap<(u64, String), std::result::Result<Mat, String>> = BTreeMap::new();
let mut resolve = |o: &JointOrigin| -> Result<Mat> {
let Ok(comp) = component_of(doc, o.occurrence) else { return resolve_origin(doc, st, o) };
found
.entry((comp, format!("{:?}", o.snap)))
.or_insert_with(|| resolve_origin(doc, st, o).map_err(|e| e.to_string()))
.clone()
.map_err(DocError::Invalid)
};
for j in &joints {
// A joint whose expressions can't be worked out (a parameter gone) is left out rather
// than placed by numbers that no longer say what it was defined by.
@@ -1603,7 +1696,7 @@ fn build<'a>(doc: &'a Document, st: &ModelState, vals: &Values, sol: &mut Soluti
sol.errors.push((j.id, j.name.clone(), e.to_string()));
continue;
}
match (resolve_origin(doc, st, &j.a), resolve_origin(doc, st, &j.b)) {
match (resolve(&j.a), resolve(&j.b)) {
(Ok(fa), Ok(fb)) => {
frames.insert(j.id, (fa, fb));
}
@@ -1735,7 +1828,10 @@ mod reach;
#[path = "joints_face.rs"]
mod face;
pub use face::{end_faces, face_name_at, named_face_spot, pin_end, round_end_at};
#[path = "joints_relations.rs"]
mod relations;
pub use reach::{REACH_TOL, Reach, Reached, reach};
pub use relations::relation_values;
/// The rest angle that turns B least as joint `j` is made, as B sits now: none or a half turn
/// about the joint's z. Origins whose x is chosen for them (a face, a circle, a point without
+130
View File
@@ -0,0 +1,130 @@
//! Relations: joints that tie only some directions or points of two parts (B parallel to A, B's
//! point on A's plane…), so several of them between one pair of parts build up the placement the
//! way a designer thinks of it: on that face, square to that edge, 40 mm from that end. The
//! solver treats them as joints with more values; a second relation between the same parts closes
//! a loop, and the loop solver meets every relation at once (or reports the one that can't be).
//!
//! A relation is made where the parts are: its values start as the nearest that meet it, so B
//! moves no further than the relation needs.
use super::{Joint, component_bodies, component_of, least_squares, link_of, occurrence_world, resolve_origin};
use crate::{Document, IDENTITY, Mat, ModelState, mat_inverse, mat_mul};
/// Starting turns tried for each angle value: the nearest values may lie a half turn away.
const TURNS: [f64; 4] = [0.0, std::f64::consts::FRAC_PI_2, std::f64::consts::PI, -std::f64::consts::FRAC_PI_2];
/// Starts tried at most (angle values beyond the first three start at 0).
const MAX_STARTS: usize = 64;
/// The values of relation `j` that move B least from where it is now: B's joint frame as it sits
/// in A's, met as nearly as the relation allows (its origin as near as it can be to where it is,
/// and its axes, each weighed as a point a part's size along it). `None` for a joint that is not a relation, or whose
/// origins can't be found.
pub fn relation_values(doc: &Document, st: &ModelState, j: &Joint) -> Option<Vec<f64>> {
if !j.kind.is_relation() {
return None;
}
let world = |occ: u64| if occ == 0 { IDENTITY } else { occurrence_world(doc, occ) };
let fa = mat_mul(&world(j.a.occurrence), &resolve_origin(doc, st, &j.a).ok()?);
let fb = mat_mul(&world(j.b.occurrence), &resolve_origin(doc, st, &j.b).ok()?);
// B's joint frame in A's as it is.
let now = mat_mul(&mat_inverse(&fa)?, &fb);
// A turn weighs as far as it moves a point B's size away.
let size = component_of(doc, j.b.occurrence).map(|c| component_bodies(doc, st, c).iter().map(|b| b.body.size()).fold(0.0, f64::max));
let scale = size.unwrap_or(0.0).clamp(1.0, 1e5);
let n = j.kind.dofs().len();
let angles: Vec<usize> = (0..n).filter(|i| j.kind.is_angle(*i)).collect();
let mut gaps = |q: &[f64]| -> Option<Vec<f64>> {
let m: Mat = link_of(j, q);
let mut out = Vec::with_capacity(12);
for (k, (row, want)) in m.iter().zip(&now).enumerate() {
let w = if k == 3 { 1.0 } else { scale };
out.extend(row.iter().zip(want).take(3).map(|(a, b)| (a - b) * w));
}
out.iter().all(|v| v.is_finite()).then_some(out)
};
let cost = |r: &[f64]| r.iter().map(|v| v * v).sum::<f64>();
let bounds = vec![(f64::NEG_INFINITY, f64::INFINITY); n];
let mut best: Option<(f64, Vec<f64>)> = None;
let tried = angles.len().min(3);
for k in 0..TURNS.len().pow(tried as u32).min(MAX_STARTS) {
let mut q = vec![0.0; n];
let mut code = k;
for i in angles.iter().take(tried) {
if let (Some(slot), Some(t)) = (q.get_mut(*i), TURNS.get(code % TURNS.len())) {
*slot = *t;
}
code /= TURNS.len();
}
// Tolerance 0: as near as it gets; `q` keeps the best values reached.
least_squares(&mut gaps, &mut q, &bounds, 0.0);
let Some(c) = gaps(&q).map(|r| cost(&r)) else { continue };
if best.as_ref().is_none_or(|(b, _)| c < *b - 1e-12) {
best = Some((c, q));
}
}
// Angles within a half turn of 0.
best.map(|(_, mut q)| {
for i in &angles {
if let Some(v) = q.get_mut(*i) {
*v = (*v + std::f64::consts::PI).rem_euclid(std::f64::consts::TAU) - std::f64::consts::PI;
}
}
q
})
}
#[cfg(test)]
mod tests {
#![allow(clippy::unwrap_used)]
use super::*;
use crate::joints::{JointKind, JointOrigin, Snap};
use solidcraft_geom::Vec3;
fn joint(kind: JointKind) -> Joint {
let at = |occurrence| JointOrigin { occurrence, snap: Snap::Point { point: Vec3::ZERO, z: None } };
Joint {
id: 1,
name: "R".into(),
kind,
a: at(0),
b: at(1),
values: Vec::new(),
limits: Vec::new(),
flip: false,
offset: 0.0,
angle: 0.6,
suppressed: false,
exprs: Default::default(),
}
}
fn z_of(m: &Mat) -> Vec3 {
Vec3::new(m[2][0], m[2][1], m[2][2])
}
/// Whatever its values, each relation keeps what it ties.
#[test]
fn relations_keep_what_they_tie() {
let q = [13.0, -7.0, 21.0, 0.7, -1.9];
let at = |kind| {
let j = joint(kind);
let n = kind.dofs().len();
link_of(&j, q.get(..n).unwrap())
};
// Parallel: z against A's.
assert!((z_of(&at(JointKind::Parallel)).z + 1.0).abs() < 1e-12);
// Perpendicular: z square to A's.
assert!(z_of(&at(JointKind::Perpendicular)).z.abs() < 1e-12);
// Angle: 0.6 from mated.
assert!((z_of(&at(JointKind::Angle)).z + 0.6f64.cos()).abs() < 1e-12);
// Point on plane: the origin on A's plane.
assert!(at(JointKind::PointOnPlane)[3][2].abs() < 1e-12);
// Point on line: on A's z axis.
let m = at(JointKind::PointOnLine);
assert!(m[3][0].abs() < 1e-12 && m[3][1].abs() < 1e-12);
// Line on plane: the origin on the plane and z along it.
let m = at(JointKind::LineOnPlane);
assert!(m[3][2].abs() < 1e-12 && z_of(&m).z.abs() < 1e-12);
}
}
+28 -6
View File
@@ -20,11 +20,13 @@ pub static COMMANDS: &[CommandSpec] = &[
"name?, occurrence?: id|name (default: from the pick, or the named face's component), face: [x,y,z] | \"<name>\" (centre of a planar face) | circle: [x,y,z] | \"<name>\" (centre of a round end: a point on its wall or rim, the end's centre, or the round face's name as model.faces lists it) | edge: [x,y,z] (a point on an edge of a planar face: the edge's middle, z out of the face, y into it; on?: [x,y,z] a point on the face (default: the larger planar face beside the edge); corner?: [x,y,z] near one end: the corner there, x and y along its two edges into the face; found again by name, so it follows the face as it changes size) | point: [x,y,z], z?: [x,y,z]; body?: the named face's body; end?: \"<name>\" (a circle's end: the face across its rim there; default the end nearer the pick); x?: [x,y,z] (a direction, world) | \"<face name>\" | {face: [x,y,z]} (the frame's x: a direction, a plane's normal, or the way to another hole's axis; keeps a round origin's turn). Faces are kept by name: the origin follows its face, and a circle its end, when the design changes",
),
CommandSpec::new("joint.create", "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|edge|point: [x,y,z] in world, or face|circle: \"<face name>\"; end?, x? as joint.origin} | origin name); \
"type: rigid|revolute|slider|cylindrical|pin_slot|planar|ball, or a relation: parallel|perpendicular|angle|point_on_plane|point_on_line|line_on_plane \
(ties only some directions or points, made where the parts are; several between two parts add up); a, b: joint origins ({occurrence, face|circle|edge|point: [x,y,z] in world, or face|circle: \"<face name>\"; end?, x? as joint.origin} | origin name); \
values?: [angles in deg or expressions, distances]; limits?: [[min, max] | null…]; flip?: bool (default false: the origins are mated, \
b's z against a's as two faces meet; true: b's z along a's, so two origins given at one point with the same z stay as they are); \
offset?: mm or an expression; angle?: deg or an expression (default: none, or a half turn where that turns b less, so faces that \
already meet stay as they are); name?; quiet?: bool (leave out dof and fixed). An expression (text) is \
offset?: mm or an expression; angle?: deg or an expression (an angle relation's tilt from mated; for other joints the default \
is none, or a half turn where that turns b less, so faces that already meet stay as they are); name?; quiet?: bool (leave \
out dof and fixed). An expression (text) is \
kept with the joint and worked out again whenever the design changes, so the joint follows the parameters it names; a joint whose \
expression no longer works out (a parameter deleted) is left out of the solve with the reason in its errors",
),
@@ -615,7 +617,7 @@ fn joint_origin(s: &mut Session, p: &Value) -> Result<Value> {
fn kind_param(p: &Value, cmd: &str) -> Result<JointKind> {
str_(p, "type")
.and_then(JointKind::parse)
.ok_or_else(|| bad(cmd, "`type` must be rigid, revolute, slider, cylindrical, pin_slot, planar or ball"))
.ok_or_else(|| bad(cmd, "`type` must be rigid, revolute, slider, cylindrical, pin_slot, planar, ball, parallel, perpendicular, angle, point_on_plane, point_on_line or line_on_plane"))
}
fn joint(s: &mut Session, p: &Value) -> Result<Value> {
@@ -647,9 +649,16 @@ fn joint(s: &mut Session, p: &Value) -> Result<Value> {
suppressed: false,
exprs: JointExprs { offset: typed(p.get("offset")), angle: typed(p.get("angle")), values: value_exprs(p) },
};
// A relation given no values starts from where its parts are: B moves no further than it must.
if p.get("values").is_none()
&& p.get("value").is_none()
&& let Some(q) = joints::relation_values(&s.doc, &s.model.state(), &j)
{
j.values = q;
}
// No angle given: B keeps its turn about the joint's axis where a half turn would otherwise
// come only from how its origin's x was chosen.
if angle.is_none() {
// come only from how its origin's x was chosen. (An angle relation's angle is its tilt.)
if angle.is_none() && !j.kind.is_relation() {
j.angle = joints::least_turn_angle(&s.doc, &s.model.state(), &j);
}
add_joint(s, j, p)
@@ -1224,6 +1233,16 @@ fn edit(s: &mut Session, p: &Value) -> Result<Value> {
if let Some(n) = str_(p, "name").map(str::trim).filter(|n| !n.is_empty() && n.len() <= 128) {
j.name = n.to_string();
}
// A relation whose type, offset, tilt or flip changes starts again from where its parts are.
let reseat = kind.is_some() || offset.is_some() || angle.is_some() || p.get("flip").is_some();
if reseat {
let edited = s.doc.assembly.joints.iter().find(|j| j.id == id).cloned();
if let Some(q) = edited.and_then(|j| joints::relation_values(&s.doc, &s.model.state(), &j))
&& let Some(j) = s.doc_mut().assembly.joints.iter_mut().find(|j| j.id == id)
{
j.values = q;
}
}
let sol = resolve(s);
Ok(sol_json(&sol, p))
}
@@ -1367,6 +1386,9 @@ mod frames_tests;
#[path = "joints_reach_tests.rs"]
mod reach_tests;
#[cfg(test)]
#[path = "joints_relations_tests.rs"]
mod relations_tests;
#[cfg(test)]
#[path = "joints_tests.rs"]
mod tests;
#[cfg(test)]
@@ -0,0 +1,238 @@
//! Relations (parallel, perpendicular, angle, point on plane, point on line, line on plane): each
//! ties only part of a placement, several between two parts add up, they start from where the
//! parts are, and they keep holding as the parts change size.
use serde_json::{Value, json};
use solidcraft_geom::Vec3;
use super::tests::{occ, place, run};
use crate::Session;
/// A 2 mm panel `W` x 300 (W = 400) in component Panel and a 20 x 10 x 6 bracket in component
/// Bracket off to the side, with named origins on both made where they are; then the bracket is
/// lifted, turned and tilted, as a part dropped in anywhere.
fn panel() -> Session {
let mut s = Session::default();
run(&mut s, "parameters.add", json!({"name": "W", "expression": "400 mm"}));
run(&mut s, "component.create", json!({"name": "Panel"}));
run(&mut s, "solid.box", json!({"length": "W", "width": 300, "height": 2, "corner": [0, 0, 0]}));
run(&mut s, "component.activate", json!({"component": "root"}));
run(&mut s, "component.create", json!({"name": "Bracket"}));
run(&mut s, "solid.box", json!({"length": 20, "width": 10, "height": 6, "corner": [0, 500, 0]}));
run(&mut s, "component.activate", json!({"component": "root"}));
let panel = occ(&s, "Panel");
run(&mut s, "occurrence.ground", json!({"occurrence": panel, "grounded": true}));
for (name, o, k, at) in [
("Top", "Panel", "face", [100.0, 100.0, 2.0]),
("Front", "Panel", "face", [100.0, 0.0, 1.0]),
("Right", "Panel", "face", [400.0, 100.0, 1.0]),
("Bottom", "Bracket", "face", [10.0, 505.0, 0.0]),
("Face", "Bracket", "face", [10.0, 500.0, 3.0]),
("Corner", "Bracket", "point", [20.0, 500.0, 0.0]),
] {
run(&mut s, "joint.origin", json!({"name": name, "occurrence": o, k: at}));
}
let b = occ(&s, "Bracket");
run(&mut s, "occurrence.move", json!({"occurrence": b, "translate": [30, -120, 50]}));
run(&mut s, "occurrence.move", json!({"occurrence": b, "axis": [0, 0, 1], "angle": "25 deg", "origin": [40, 385, 50]}));
run(&mut s, "occurrence.move", json!({"occurrence": b, "axis": [1, 0, 0], "angle": "10 deg", "origin": [40, 385, 50]}));
s
}
fn relation(s: &mut Session, kind: &str, a: &str, b: &str, extra: Value) -> Value {
let mut p = json!({"type": kind, "a": a, "b": b, "name": format!("{kind} {a}-{b}")});
if let (Some(p), Some(extra)) = (p.as_object_mut(), extra.as_object()) {
p.extend(extra.clone());
}
run(s, "joint.create", p)
}
fn dof(r: &Value, s: &Session) -> Option<u64> {
let b = occ(s, "Bracket");
r["dof"].as_array()?.iter().find(|d| d["occurrence"] == json!(b))?["dof"].as_u64()
}
fn near(a: Vec3, b: Vec3, tol: f64) -> bool {
a.dist(b) < tol
}
fn clean(r: &Value) -> bool {
r["conflicts"] == json!([]) && r["errors"] == json!([])
}
#[test]
fn stacked_relations_place_a_part_and_follow_the_panel() {
let mut s = panel();
// Flat on the top (parallel, and a corner on it), its face along the panel's front edge, the
// corner 40 mm in from the front and the right end.
let r = relation(&mut s, "parallel", "Top", "Bottom", json!({}));
assert_eq!(dof(&r, &s), Some(4), "{r}");
let r = relation(&mut s, "point_on_plane", "Top", "Corner", json!({}));
assert_eq!(dof(&r, &s), Some(3), "{r}");
let r = relation(&mut s, "parallel", "Front", "Face", json!({"flip": true}));
assert_eq!(dof(&r, &s), Some(2), "{r}");
let r = relation(&mut s, "point_on_plane", "Front", "Corner", json!({"offset": -40}));
assert_eq!(dof(&r, &s), Some(1), "{r}");
let r = relation(&mut s, "point_on_plane", "Right", "Corner", json!({"offset": -40}));
assert!(clean(&r), "{r}");
assert_eq!(dof(&r, &s), Some(0), "{r}");
for w in [400.0, 650.0, 250.0, 1000.0] {
run(&mut s, "parameters.change", json!({"name": "W", "expression": format!("{w} mm")}));
let r = run(&mut s, "joint.solve", json!({}));
assert!(clean(&r), "W {w}: {r}");
// The corner, the bracket's other bottom corner along its face, and its top: flat, square
// and 40 mm in.
assert!(near(place(&s, "Bracket", Vec3::new(20.0, 500.0, 0.0)), Vec3::new(w - 40.0, 40.0, 2.0), 1e-6), "W {w}");
assert!(near(place(&s, "Bracket", Vec3::new(0.0, 500.0, 0.0)), Vec3::new(w - 60.0, 40.0, 2.0), 1e-6), "W {w}");
assert!(near(place(&s, "Bracket", Vec3::new(20.0, 510.0, 6.0)), Vec3::new(w - 40.0, 50.0, 8.0), 1e-6), "W {w}");
}
}
#[test]
fn a_relation_moves_the_part_no_further_than_it_must() {
let mut s = panel();
let corner = |s: &Session| place(s, "Bracket", Vec3::new(20.0, 500.0, 0.0));
let along = |s: &Session| place(s, "Bracket", Vec3::new(0.0, 510.0, 6.0)) - corner(s);
let (before, turn) = (corner(&s), along(&s));
let r = relation(&mut s, "point_on_plane", "Top", "Corner", json!({}));
assert!(clean(&r), "{r}");
assert_eq!(dof(&r, &s), Some(5), "{r}");
// Straight down onto the top, turned as it was.
assert!(near(corner(&s), Vec3::new(before.x, before.y, 2.0), 1e-6), "{:?} from {before:?}", corner(&s));
assert!(near(along(&s), turn, 1e-6), "{:?} from {turn:?}", along(&s));
// Parallel to the top: it only turns level, about the corner it keeps.
let r = relation(&mut s, "parallel", "Top", "Bottom", json!({}));
assert!(clean(&r), "{r}");
let up = place(&s, "Bracket", Vec3::new(20.0, 500.0, 6.0)) - corner(&s);
assert!(near(up, Vec3::new(0.0, 0.0, 6.0), 1e-6), "{up:?}");
assert!((corner(&s).z - 2.0).abs() < 1e-6, "{:?}", corner(&s));
}
#[test]
fn an_angle_relation_holds_its_tilt_and_follows_an_edit() {
let mut s = panel();
relation(&mut s, "point_on_plane", "Top", "Corner", json!({}));
let tilt = |s: &Session| {
let n = place(s, "Bracket", Vec3::new(10.0, 505.0, 0.0)) - place(s, "Bracket", Vec3::new(10.0, 505.0, 1.0));
n.normalized().map_or(f64::NAN, |n| n.dot(Vec3::Z).acos().to_degrees())
};
// 0 deg is mated: the bracket's bottom against the top, its outward normal straight down.
let r = relation(&mut s, "angle", "Top", "Bottom", json!({"angle": "30 deg"}));
assert!(clean(&r), "{r}");
assert!((tilt(&s) - 150.0).abs() < 1e-6, "{}", tilt(&s));
run(&mut s, "joint.edit", json!({"joint": "angle Top-Bottom", "angle": "60 deg"}));
assert!((tilt(&s) - 120.0).abs() < 1e-6, "{}", tilt(&s));
run(&mut s, "joint.edit", json!({"joint": "angle Top-Bottom", "type": "perpendicular"}));
assert!((tilt(&s) - 90.0).abs() < 1e-6, "{}", tilt(&s));
// The corner stayed on the top through every change.
assert!((place(&s, "Bracket", Vec3::new(20.0, 500.0, 0.0)).z - 2.0).abs() < 1e-6);
}
#[test]
fn a_rod_lies_tangent_on_the_panel_and_rolls_along_it() {
let mut s = panel();
run(&mut s, "component.create", json!({"name": "Rod"}));
run(&mut s, "solid.cylinder", json!({"base": [0, 700, 40], "axis": [1, 0, 0], "radius": 5, "height": 80, "operation": "new"}));
run(&mut s, "component.activate", json!({"component": "root"}));
run(&mut s, "joint.origin", json!({"name": "Axis", "occurrence": "Rod", "circle": [0, 700, 40]}));
let r = relation(&mut s, "line_on_plane", "Top", "Axis", json!({"offset": 5}));
assert!(clean(&r), "{r}");
let rod = occ(&s, "Rod");
let d = r["dof"].as_array().and_then(|a| a.iter().find(|d| d["occurrence"] == json!(rod))).and_then(|d| d["dof"].as_u64());
assert_eq!(d, Some(4), "{r}");
// Its axis level, 5 mm over the top: it touches it along a line.
let (a, b) = (place(&s, "Rod", Vec3::new(0.0, 700.0, 40.0)), place(&s, "Rod", Vec3::new(80.0, 700.0, 40.0)));
assert!((a.z - 7.0).abs() < 1e-6 && (b.z - 7.0).abs() < 1e-6, "{a:?} {b:?}");
// Rolled along: still tangent.
run(&mut s, "joint.drive", json!({"joint": "line_on_plane Top-Axis", "values": [100, 50, "30 deg", "90 deg"]}));
let (a, b) = (place(&s, "Rod", Vec3::new(0.0, 700.0, 40.0)), place(&s, "Rod", Vec3::new(80.0, 700.0, 40.0)));
assert!((a.z - 7.0).abs() < 1e-6 && (b.z - 7.0).abs() < 1e-6, "{a:?} {b:?}");
}
#[test]
fn relations_that_disagree_are_reported_not_forced() {
let mut s = panel();
relation(&mut s, "point_on_plane", "Right", "Corner", json!({"offset": -40}));
let r = relation(&mut s, "point_on_plane", "Right", "Corner", json!({"offset": -60}));
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(1), "{r}");
// A point on the panel's front plane's line: still placed, every number finite.
let r = relation(&mut s, "point_on_line", "Front", "Corner", json!({}));
assert!(r["dof"].is_array(), "{r}");
let p = place(&s, "Bracket", Vec3::new(20.0, 500.0, 0.0));
assert!(p.is_finite(), "{p:?}");
// Saved and read back, the relations are the same.
let saved = serde_json::to_string(&s.doc.assembly.joints).unwrap_or_default();
let back: Vec<solidcraft_doc::joints::Joint> = serde_json::from_str(&saved).unwrap_or_default();
assert_eq!(back, s.doc.assembly.joints);
assert!(saved.contains("\"point_on_plane\"") && saved.contains("\"point_on_line\""), "{saved}");
}
/// More parts placed by relations than one group of loops may hold (each relation after a part's
/// first closes a loop): every group is small, so every part is placed.
#[test]
fn a_panel_full_of_parts_placed_by_relations_stays_solved() {
let mut s = Session::default();
run(&mut s, "parameters.add", json!({"name": "W", "expression": "1500 mm"}));
run(&mut s, "component.create", json!({"name": "Panel"}));
run(&mut s, "solid.box", json!({"length": "W", "width": 300, "height": 2, "corner": [0, 0, 0]}));
run(&mut s, "component.activate", json!({"component": "root"}));
let panel = occ(&s, "Panel");
run(&mut s, "occurrence.ground", json!({"occurrence": panel, "grounded": true}));
for (name, at) in [("Top", [100.0, 100.0, 2.0]), ("Front", [100.0, 0.0, 1.0]), ("Right", [1500.0, 100.0, 1.0])] {
run(&mut s, "joint.origin", json!({"name": name, "occurrence": "Panel", "face": at}));
}
// 30 brackets, 5 relations each: 120 loops, more than the 100 one group may hold. The joints
// go straight into the design and are solved once (made one by one, each would solve all).
let n = 30usize;
for k in 0..n {
let (c, y) = (format!("B{k}"), 500.0 + 30.0 * k as f64);
run(&mut s, "component.activate", json!({"component": "root"}));
run(&mut s, "component.create", json!({"name": c}));
run(&mut s, "solid.box", json!({"length": 20, "width": 10, "height": 6, "corner": [0, y, 0]}));
run(&mut s, "component.activate", json!({"component": "root"}));
for (o, k2, at) in [("b", "face", [10.0, y + 5.0, 0.0]), ("f", "face", [10.0, y, 3.0]), ("c", "point", [20.0, y, 0.0])] {
run(&mut s, "joint.origin", json!({"name": format!("{c}{o}"), "occurrence": c, k2: at}));
}
let x = 40.0 * (k + 1) as f64;
for (kind, a, b, flip, offset) in [
("parallel", "Top", "b", false, 0.0),
("point_on_plane", "Top", "c", false, 0.0),
("parallel", "Front", "f", true, 0.0),
("point_on_plane", "Front", "c", false, -40.0),
("point_on_plane", "Right", "c", false, -x),
] {
let origin = |n: &str| s.doc.assembly.origins.iter().find(|o| o.name == n).map(|o| o.origin.clone());
let (Some(a), Some(b)) = (origin(a), origin(&format!("{c}{b}"))) else { panic!("origins") };
let id = s.doc.assembly.joints.iter().map(|j| j.id).max().unwrap_or(0) + 1;
let kind = solidcraft_doc::joints::JointKind::parse(kind).unwrap_or(solidcraft_doc::joints::JointKind::Rigid);
let mut j = solidcraft_doc::joints::Joint {
id,
name: format!("R{id}"),
kind,
a,
b,
values: vec![0.0; kind.dofs().len()],
limits: vec![None; kind.dofs().len()],
flip,
offset,
angle: 0.0,
suppressed: false,
exprs: Default::default(),
};
if let Some(q) = solidcraft_doc::joints::relation_values(&s.doc, &s.model.state(), &j) {
j.values = q;
}
s.doc_mut().assembly.joints.push(j);
}
}
for w in [1500.0, 1800.0] {
run(&mut s, "parameters.change", json!({"name": "W", "expression": format!("{w} mm")}));
let r = run(&mut s, "joint.solve", json!({}));
assert!(clean(&r), "W {w}: {r}");
for k in [0, n / 2, n - 1] {
let (c, y) = (format!("B{k}"), 500.0 + 30.0 * k as f64);
let at = place(&s, &c, Vec3::new(20.0, y, 0.0));
assert!(near(at, Vec3::new(w - 40.0 * (k + 1) as f64, 40.0, 2.0), 1e-6), "W {w} {c}: {at:?}");
}
}
}
+16 -5
View File
@@ -148,9 +148,11 @@ fn rotation(u: Vec3, a: f64) -> Mat {
}
/// The joint's motion as URDF steps, in order from A's frame: (kind, axis, index of the value).
fn steps(kind: JointKind) -> Vec<(Step, Vec3, usize)> {
/// `None` for a relation whose turns follow a fixed step (a tilt, or the offset off a plane or
/// line): URDF has no fixed step inside a joint's chain.
fn steps(kind: JointKind) -> Option<Vec<(Step, Vec3, usize)>> {
use Step::{Prismatic as P, Revolute as R};
match kind {
Some(match kind {
JointKind::Rigid => vec![],
JointKind::Revolute => vec![(R, Vec3::Z, 0)],
JointKind::Slider => vec![(P, Vec3::Z, 0)],
@@ -158,7 +160,9 @@ fn steps(kind: JointKind) -> Vec<(Step, Vec3, usize)> {
JointKind::PinSlot => vec![(P, Vec3::X, 1), (R, Vec3::Z, 0)],
JointKind::Planar => vec![(P, Vec3::X, 0), (P, Vec3::Y, 1), (R, Vec3::Z, 2)],
JointKind::Ball => vec![(R, Vec3::Z, 0), (R, Vec3::Y, 1), (R, Vec3::Z, 2)],
}
JointKind::Parallel => vec![(P, Vec3::X, 0), (P, Vec3::Y, 1), (P, Vec3::Z, 2), (R, Vec3::Z, 3)],
JointKind::Perpendicular | JointKind::Angle | JointKind::PointOnPlane | JointKind::PointOnLine | JointKind::LineOnPlane => return None,
})
}
/// `raw` with every character but `[A-Za-z0-9_]` (and `-` in file names) replaced by `_`, so it
@@ -378,7 +382,14 @@ impl<'a> Tree<'a> {
/// `child` on joint `j` from `p` (already in the tree): the joint's chain and the child's link.
fn attach(&mut self, j: &Joint, fa: &Mat, p: u64, child: u64, q: &[f64]) -> Option<()> {
let m = joints::motion(j.kind, q);
// A relation URDF can't write is fixed where it is: the child link's frame is the joint's.
let (chain, m) = match steps(j.kind) {
Some(chain) => (chain, joints::motion(j.kind, q)),
None => {
self.warnings.push(format!("joint `{}` is a {:?} relation, which URDF has no joint for: written fixed as it is", j.name, j.kind));
(Vec::new(), IDENTITY)
}
};
let child_on_b = j.a.occurrence == p;
// (parent-side joint frame, child link frame) in the world.
let (jf, lf) = if child_on_b {
@@ -390,7 +401,7 @@ impl<'a> Tree<'a> {
};
let (parent_index, parent_frame) = self.placed.get(&p).copied()?;
let origin = mat_mul(&mat_inverse(&parent_frame)?, &jf);
let mut chain = steps(j.kind);
let mut chain = chain;
if !child_on_b {
chain.reverse();
for s in &mut chain {
+30 -2
View File
@@ -26,8 +26,36 @@ mod trace;
pub use trace::{TraceForm, picked_point};
/// Joint types in the order the Type box lists them (command names).
pub const TYPES: [&str; 7] = ["rigid", "revolute", "slider", "cylindrical", "pin_slot", "planar", "ball"];
const TYPE_LABELS: [&str; 7] = ["Rigid", "Revolute", "Slider", "Cylindrical", "Pin-slot", "Planar", "Ball"];
pub const TYPES: [&str; 13] = [
"rigid",
"revolute",
"slider",
"cylindrical",
"pin_slot",
"planar",
"ball",
"parallel",
"perpendicular",
"angle",
"point_on_plane",
"point_on_line",
"line_on_plane",
];
const TYPE_LABELS: [&str; 13] = [
"Rigid",
"Revolute",
"Slider",
"Cylindrical",
"Pin-slot",
"Planar",
"Ball",
"Parallel",
"Perpendicular",
"Angle",
"Point on plane",
"Point on line",
"Line on plane",
];
/// What a joint origin input accepts (snaps on faces, edges and vertices).
pub const SNAPS: u16 = FACES | EDGES | VERTICES;
/// Commands whose effect is where occurrences sit (previewed on the placed model).
+23 -1
View File
@@ -46,6 +46,28 @@ expression stops working out (say a parameter becomes an angle), the joint is le
solve and `joint.solve` gives the reason, instead of placing the part from old numbers. Typing a
plain number, or driving the value, drops that expression.
**Relations** are joints that tie only part of a placement, so a part is placed the way a
designer describes it, one relation at a time: `parallel` (z along z, free to move and turn
about it), `perpendicular` and `angle` (z square to z, or at the joint's `angle` from mated),
`point_on_plane` (a point on a face's plane, `offset` off it), `point_on_line` (a point on an
axis) and `line_on_plane` (an axis on a face's plane: a cylinder's radius as `offset` lays it
tangent). A relation is made where the parts are: B moves no further than the relation needs.
Several relations between the same two parts add up; the solver meets them together, each one's
degrees of freedom come off what is left, and one that can't be met is reported, not forced:
```json
{"command": "joint.create", "params": {"type": "parallel", "a": "Panel top", "b": "Bracket bottom"}}
{"command": "joint.create", "params": {"type": "point_on_plane", "a": "Panel top", "b": "Bracket corner"}}
{"command": "joint.create", "params": {"type": "parallel", "a": "Panel front", "b": "Bracket face", "flip": true}}
{"command": "joint.create", "params": {"type": "point_on_plane", "a": "Panel front", "b": "Bracket corner", "offset": -40}}
{"command": "joint.create", "params": {"type": "point_on_plane", "a": "Panel end", "b": "Bracket corner", "offset": -40}}
```
leaves the bracket flat on the panel, square to its front, its corner 40 mm in from the front and
the end, wherever the panel's size takes them. Coincident faces are a `planar` joint, a distance
between faces is `planar` with an `offset`, concentric is `cylindrical` and two points that meet
are `ball`.
The check behind it, `inspect.clearance`, does all of that in one call for any number of bodies,
which suits scripts and agents checking a whole assembly:
@@ -169,7 +191,7 @@ it where it is), and it moves.
with their units and limits, and `moved` says whether any part moved. A joint's origins are mated
by default: B's z axis is turned against A's, as two faces meet. `flip: true` lines them up
instead, so two origins given at one point with the same z stay as they are. Without an
`angle`, a new joint takes no turn or a half turn about its axis, whichever turns B less: two
`angle`, a new joint (other than a relation, which starts where its parts are) takes no turn or a half turn about its axis, whichever turns B less: two
faces that already meet, a plate under a panel, stay as they are instead of B spinning about
their shared normal. For scripts that drive many steps, `quiet: true` leaves the per-part
degrees of freedom out of joint answers, and
+2
View File
@@ -68,6 +68,8 @@ defaults are written and a warning says so. A value that is not a positive numbe
| Pin-slot | `prismatic` x, then `revolute` z |
| Planar | `prismatic` x, `prismatic` y, then `revolute` z |
| Ball | `revolute` z, `revolute` y, then `revolute` z |
| Parallel | `prismatic` x, y and z, then `revolute` z |
| Perpendicular, angle, point on plane, point on line, line on plane | `fixed` where it is, with a warning: their turns follow a fixed tilt or offset, which a URDF chain can't hold |
- **Chains:** a joint with several values becomes one URDF joint per value. Between them sit
links named `<joint>_link1`, `<joint>_link2`. The steps before the last are named