Files
solidcraft/crates/doc/src/joints_relations.rs
T
af436493a2 Joints: a point origin keeps the x it is given (#2597) (#2598)
* Joints: a point origin keeps the x it is given

`joint.origin` and `joint.create` take `x` for a point origin (a
direction, laid square to z) and the point snap saves it. Two origins
given at one point with the same z and x now meet as they are: a part
turned a quarter turn about z no longer turns back (and moves) when
rigid-joined there. A point origin without x takes its x from z alone,
as before. An x along z, or of no length, is refused.

Co-Authored-By: Claude Opus 5.5 (1M context) <noreply@anthropic.com>

* Joint relations test: a point origin has an x too

Co-Authored-By: Claude Opus 5.5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Aac7mP9rthK3WsPpPRvCsF

---------

Co-authored-by: Claude Opus 5.5 (1M context) <noreply@anthropic.com>
Co-authored-by: bherbruck <brennen.herbruck@gmail.com>
2026-10-19 23:56:32 +00:00

131 lines
5.6 KiB
Rust

//! 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, x: 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);
}
}