Joint sweep: drive a joint until parts touch, and report where (#1981)

Joint sweeps can stop where parts touch and report the contact and remaining room.
This commit is contained in:
NOWN-DEV
2026-10-19 22:07:44 +02:00
committed by GitHub
parent d3fa22e987
commit 539c17b425
17 changed files with 914 additions and 23 deletions
+1
View File
@@ -246,6 +246,7 @@
"cmd.joint_rigid_group": "Starrgruppe",
"cmd.joint_screw": "Schraube",
"cmd.joint_solve": "Gelenke lösen",
"cmd.joint_sweep": "Gelenk bis zum Kontakt durchfahren",
"cmd.library_apply": "Lokalen Bibliothekseintrag anwenden",
"cmd.library_export": "Designbibliothek exportieren",
"cmd.library_inspect": "Lokale Bibliothek prüfen",
+1
View File
@@ -454,6 +454,7 @@
"cmd.joint_rigid_group": "Rigid Group",
"cmd.joint_screw": "Screw",
"cmd.joint_solve": "Solve Joints",
"cmd.joint_sweep": "Sweep Joint",
"cmd.library_apply": "Apply Local Library Entry",
"cmd.library_export": "Export Design Library",
"cmd.library_inspect": "Inspect Local Library",
+1
View File
@@ -448,6 +448,7 @@
"cmd.joint_rigid_group": "Groupe rigide",
"cmd.joint_screw": "Vis",
"cmd.joint_solve": "Résoudre les liaisons",
"cmd.joint_sweep": "Balayer une liaison",
"cmd.library_apply": "Appliquer une entrée de la bibliothèque locale",
"cmd.library_export": "Exporter la bibliothèque de la conception",
"cmd.library_inspect": "Inspecter la bibliothèque locale",
+1
View File
@@ -454,6 +454,7 @@
"cmd.joint_rigid_group": "リジッド グループ",
"cmd.joint_screw": "ねじ",
"cmd.joint_solve": "ジョイントを解決",
"cmd.joint_sweep": "ジョイントをスイープ",
"cmd.library_apply": "ローカルライブラリ項目を適用",
"cmd.library_export": "デザインライブラリを書き出す",
"cmd.library_inspect": "ローカルライブラリを確認",
+1
View File
@@ -246,6 +246,7 @@
"cmd.joint_rigid_group": "강체 그룹",
"cmd.joint_screw": "나사",
"cmd.joint_solve": "관절 해석",
"cmd.joint_sweep": "관절 스윕",
"cmd.library_apply": "로컬 라이브러리 항목 적용",
"cmd.library_export": "설계 라이브러리 내보내기",
"cmd.library_inspect": "로컬 라이브러리 확인",
+1
View File
@@ -397,6 +397,7 @@
"cmd.joint_rigid_group": "Жёсткая группа",
"cmd.joint_screw": "Винт",
"cmd.joint_solve": "Решить сопряжения",
"cmd.joint_sweep": "Прогон сопряжения",
"cmd.material_assign": "Физический материал",
"cmd.material_create": "Новый физический материал",
"cmd.material_delete": "Удалить физический материал",
+1
View File
@@ -407,6 +407,7 @@
"cmd.joint_origin": "Bağlantı orijini",
"cmd.joint_rigid_group": "Rijit grup",
"cmd.joint_solve": "Bağlantıları çöz",
"cmd.joint_sweep": "Bağlantıyı tara",
"cmd.library_apply": "Yerel kitaplık girişini uygula",
"cmd.library_export": "Tasarım kitaplığını dışa aktar",
"cmd.library_inspect": "Yerel kitaplığı incele",
+1
View File
@@ -441,6 +441,7 @@
"cmd.joint_rigid_group": "Жорстка група",
"cmd.joint_screw": "Гвинт",
"cmd.joint_solve": "Розв'язати з'єднання",
"cmd.joint_sweep": "Прогін з'єднання",
"cmd.library_apply": "Застосувати елемент локальної бібліотеки",
"cmd.library_export": "Експортувати бібліотеку проєкту",
"cmd.library_inspect": "Перевірити локальну бібліотеку",
+1
View File
@@ -454,6 +454,7 @@
"cmd.joint_rigid_group": "刚性组",
"cmd.joint_screw": "螺旋",
"cmd.joint_solve": "解算接头",
"cmd.joint_sweep": "扫掠接头",
"cmd.library_apply": "应用本地库条目",
"cmd.library_export": "导出设计库",
"cmd.library_inspect": "检查本地库",
+304 -19
View File
@@ -1,11 +1,12 @@
//! Bounded, chronological checks of a joint trajectory. This samples tessellated geometry;
//! it is deliberately not advertised as exact continuous collision detection.
use std::collections::{BTreeSet, HashMap};
use std::cell::Cell;
use std::collections::{BTreeMap, BTreeSet, HashMap};
use std::sync::Arc;
use solidcraft_doc::joints;
use solidcraft_doc::{IDENTITY, Mat, apply_point, mat_mul};
use solidcraft_geom::{Aabb3, Mesh, Vec3};
use solidcraft_doc::{Document, IDENTITY, Mat, apply_point, mat_mul};
use solidcraft_geom::{Aabb3, Bvh, Mesh, Vec3};
use crate::params::bad;
use crate::{Result, Session};
@@ -20,11 +21,15 @@ fn limit(message: &str) -> crate::EngineError {
bad("contact.sweep", message)
}
#[derive(Clone)]
struct CachedBody {
path: Vec<u64>,
offset: Mat,
mesh: Arc<Mesh>,
size: f64,
/// How far its mesh may stray from its faces: 0 when every face is flat (the mesh is the
/// shape), else the chord tolerance it was tessellated to.
chord: f64,
}
/// A member of a contact set: (occurrence id, name).
@@ -56,7 +61,7 @@ impl ContactGeometry {
return Ok(out);
}
let state = s.model.state();
let mut meshes: HashMap<String, Arc<Mesh>> = HashMap::new();
let mut meshes: HashMap<String, (Arc<Mesh>, f64)> = HashMap::new();
let mut stack = vec![(0, Vec::<u64>::new())];
let mut count = 0;
while let Some((component, path)) = stack.pop() {
@@ -82,10 +87,11 @@ impl ContactGeometry {
if out.bodies.len() >= MAX_BODIES {
return Err(limit("too many bodies for a contact sweep"));
}
let mesh = if let Some(mesh) = meshes.get(&body.name) {
mesh.clone()
let (mesh, chord) = if let Some(cached) = meshes.get(&body.name) {
cached.clone()
} else {
let mesh = body.body.tessellate((body.body.size() * 1e-3).max(1e-3))?;
let tolerance = (body.body.size() * 1e-3).max(1e-3);
let mesh = body.body.tessellate(tolerance)?;
if mesh.triangles.len() > MAX_TRIANGLES || mesh.positions.len().saturating_add(mesh.triangles.len().saturating_mul(4)) > 200_000 {
return Err(limit("body exceeds contact sweep tessellation budget"));
}
@@ -118,14 +124,20 @@ impl ContactGeometry {
}
out.step = out.step.min(thickness * 0.25);
let mesh = Arc::new(mesh);
meshes.insert(body.name.clone(), mesh.clone());
mesh
// Flat faces tessellate exactly; only planes known from how the face was
// made count (an unknown face is taken as curved).
let faces = body.body.face_count();
let flat = faces <= 1024 && (0..faces).all(|i| body.body.face_plane(i).is_some());
let chord = if flat { 0.0 } else { tolerance };
meshes.insert(body.name.clone(), (mesh.clone(), chord));
(mesh, chord)
};
out.bodies.push(CachedBody {
path: path.clone(),
offset: s.doc.model_body_offset(body).copied().unwrap_or(IDENTITY),
mesh,
size: body.body.size(),
chord,
});
}
}
@@ -155,6 +167,14 @@ impl ContactGeometry {
self.pairs.retain(|(_, _, a, b)| keep(a.0, b.0));
}
/// Move the pairs of set members `take` picks into a geometry of their own (with the same
/// bodies), leaving the rest here.
pub(super) fn take_pairs(&mut self, take: impl Fn(u64, u64) -> bool) -> ContactGeometry {
let (taken, kept) = std::mem::take(&mut self.pairs).into_iter().partition(|(_, _, a, b)| take(a.0, b.0));
self.pairs = kept;
ContactGeometry { bodies: self.bodies.clone(), pairs: taken, step: self.step }
}
/// How many mesh points are placed for each check.
pub(super) fn vertex_count(&self) -> usize {
self.bodies.iter().map(|b| b.mesh.positions.len()).fold(0, usize::saturating_add)
@@ -166,9 +186,13 @@ impl ContactGeometry {
}
fn frame(body: &CachedBody, s: &Session) -> Result<Mat> {
Self::frame_in(body, &s.doc)
}
fn frame_in(body: &CachedBody, doc: &Document) -> Result<Mat> {
let mut frame = IDENTITY;
for id in &body.path {
let occurrence = s.doc.occurrences.iter().find(|o| o.id == *id).ok_or_else(|| limit("contact occurrence disappeared"))?;
let occurrence = doc.occurrences.iter().find(|o| o.id == *id).ok_or_else(|| limit("contact occurrence disappeared"))?;
frame = mat_mul(&frame, &occurrence.transform);
}
Ok(mat_mul(&frame, &body.offset))
@@ -229,6 +253,18 @@ impl ContactGeometry {
Ok(required as usize)
}
/// Each body's mesh placed where `s` has it.
fn placed(&self, s: &Session) -> Result<Vec<Mesh>> {
self.bodies
.iter()
.map(|body| {
let frame = Self::frame(body, s)?;
let positions = body.mesh.positions.iter().map(|p| apply_point(&frame, *p)).collect();
Ok(Mesh { positions, triangles: body.mesh.triangles.clone(), ..Mesh::default() })
})
.collect()
}
pub(super) fn first_hit(&self, s: &Session, work: &mut usize) -> Result<Option<(String, String, f64)>> {
Ok(self.hits(s, work, MAX_WORK, false)?.into_iter().next().map(|(a, b, depth)| (a.1, b.1, depth)))
}
@@ -240,15 +276,7 @@ impl ContactGeometry {
let mut out: Vec<(Member, Member, f64)> = Vec::new();
// The member pairs found so far (lower id first).
let mut found: BTreeSet<(u64, u64)> = BTreeSet::new();
let meshes: Vec<Mesh> = self
.bodies
.iter()
.map(|body| {
let frame = Self::frame(body, s)?;
let positions = body.mesh.positions.iter().map(|p| apply_point(&frame, *p)).collect();
Ok(Mesh { positions, triangles: body.mesh.triangles.clone(), ..Mesh::default() })
})
.collect::<Result<_>>()?;
let meshes = self.placed(s)?;
// Each mesh's box once, not once per pair it is in.
let boxes: Vec<Aabb3> = meshes.iter().map(Mesh::bounds).collect();
for (a, b, oa, ob) in &self.pairs {
@@ -293,6 +321,263 @@ impl ContactGeometry {
}
}
/// How near two set members come: the bodies under them nearest each other.
pub(super) struct Gap {
pub(super) a: Member,
pub(super) b: Member,
/// 0 where they touch or pass into each other.
pub(super) mm: f64,
/// The nearest points (world), on a's body and on b's.
pub(super) at: (Vec3, Vec3),
}
impl ContactGeometry {
/// The gap between each pair of set members whose bodies come within `cutoff` of each
/// other in `s` (all pairs with an infinite cutoff), nearest first. Mesh distances: exact
/// for flat faces, within [`ContactGeometry::chord`] for curved ones. `work` adds up the
/// cost, refused past `budget`.
pub(super) fn gaps(&self, s: &Session, cutoff: f64, work: &mut usize, budget: usize) -> Result<Vec<Gap>> {
let meshes = self.placed(s)?;
let tris: Vec<Vec<[Vec3; 3]>> = meshes.iter().map(|m| m.triangles.iter().filter_map(|t| m.tri(t)).collect()).collect();
let boxes: Vec<Aabb3> = meshes.iter().map(Mesh::bounds).collect();
let mut trees: Vec<Option<Bvh>> = vec![None; meshes.len()];
let mut nearest: BTreeMap<(u64, u64), Gap> = BTreeMap::new();
for (a, b, oa, ob) in &self.pairs {
let key = (oa.0.min(ob.0), oa.0.max(ob.0));
let limit = nearest.get(&key).map_or(cutoff, |g| g.mm.min(cutoff));
let (Some(ta), Some(tb), Some(ba), Some(bb)) = (tris.get(*a), tris.get(*b), boxes.get(*a), boxes.get(*b)) else {
return Err(limit_error("invalid contact geometry pair"));
};
if box_distance(ba, bb) > limit {
continue;
}
if trees.get(*b).is_some_and(Option::is_none)
&& let Some(slot) = trees.get_mut(*b)
{
*work = work.saturating_add(tb.len());
*slot = Some(Bvh::new(&tb.iter().map(tri_box).collect::<Vec<_>>()));
}
let Some(Some(tree)) = trees.get(*b) else { return Err(limit_error("invalid contact geometry pair")) };
let Some((mm, pa, pb)) = mesh_gap(ta, tb, tree, ba, bb, limit, work, budget)? else { continue };
nearest.insert(key, Gap { a: oa.clone(), b: ob.clone(), mm, at: (pa, pb) });
}
let mut out: Vec<Gap> = nearest.into_values().collect();
out.sort_by(|x, y| x.mm.total_cmp(&y.mm));
Ok(out)
}
/// The most the gaps can be off: the largest chord tolerance of a curved body (0 when every
/// body is flat-faced, and the mesh distances are exact).
pub(super) fn chord(&self) -> f64 {
self.bodies.iter().map(|b| b.chord).fold(0.0, f64::max)
}
/// The size of the largest body (at least 1 mm): what tolerances scale with.
pub(super) fn scale(&self) -> f64 {
self.bodies.iter().map(|b| b.size).fold(1.0, f64::max)
}
/// The furthest any point of the bodies moves between `from` and `to` (bounded by the
/// corners of each body's box, which a rigid move takes furthest).
pub(super) fn travel(&self, from: &Document, to: &Document) -> Result<f64> {
let mut most = 0.0_f64;
for body in &self.bodies {
let (fa, fb) = (Self::frame_in(body, from)?, Self::frame_in(body, to)?);
let local = body.mesh.bounds();
for c in 0..8 {
let pick = |bit: usize, lo: f64, hi: f64| if c & bit == 0 { lo } else { hi };
let corner = Vec3::new(pick(1, local.min.x, local.max.x), pick(2, local.min.y, local.max.y), pick(4, local.min.z, local.max.z));
most = most.max(apply_point(&fa, corner).dist(apply_point(&fb, corner)));
}
}
Ok(most)
}
}
fn limit_error(message: &str) -> crate::EngineError {
limit(message)
}
fn tri_box(t: &[Vec3; 3]) -> Aabb3 {
let mut b = Aabb3::EMPTY;
for p in t {
b.add(*p);
}
b
}
/// The distance between two boxes (0 where they overlap).
fn box_distance(a: &Aabb3, b: &Aabb3) -> f64 {
let axis = |a0: f64, a1: f64, b0: f64, b1: f64| (b0 - a1).max(a0 - b1).max(0.0);
let (x, y, z) = (axis(a.min.x, a.max.x, b.min.x, b.max.x), axis(a.min.y, a.max.y, b.min.y, b.max.y), axis(a.min.z, a.max.z, b.min.z, b.max.z));
(x * x + y * y + z * z).sqrt()
}
/// The gap between meshes `a` and `b` (their triangles; `tree` over b's) and their nearest
/// points, when within `limit` (None: farther). 0 where their surfaces cross, and where one
/// lies wholly inside the other.
#[allow(clippy::too_many_arguments)]
fn mesh_gap(
a: &[[Vec3; 3]],
b: &[[Vec3; 3]],
tree: &Bvh,
ba: &Aabb3,
bb: &Aabb3,
limit: f64,
work: &mut usize,
budget: usize,
) -> Result<Option<(f64, Vec3, Vec3)>> {
let mut best: Option<(f64, Vec3, Vec3)> = None;
// An upper bound to prune with when there is no limit: a vertex of each near the other.
let mut bound = limit;
if !bound.is_finite() {
let centre = bb.center();
let near = |list: &[[Vec3; 3]], to: Vec3| list.iter().map(|t| t[0]).min_by(|p, q| p.dist(to).total_cmp(&q.dist(to)));
if let Some(pa) = near(a, centre)
&& let Some(pb) = near(b, pa)
{
*work = work.saturating_add(a.len() + b.len());
bound = pa.dist(pb);
best = Some((bound, pa, pb));
}
}
for ta in a {
let box_a = tri_box(ta);
if box_distance(&box_a, bb) > bound {
continue;
}
// Both callbacks read the bound the other tightens.
let (visits, shared) = (Cell::new(0usize), Cell::new(bound));
tree.visit(
|node| {
visits.set(visits.get() + 1);
box_distance(node, &box_a) <= shared.get()
},
|j| {
let Some(tb) = b.get(j as usize) else { return };
visits.set(visits.get() + 8);
let (d, pa, pb) = tri_distance(ta, tb);
if d <= shared.get() && best.is_none_or(|(x, _, _)| d < x) {
shared.set(d);
best = Some((d, pa, pb));
}
},
);
bound = shared.get();
let visits = visits.get();
*work = work.saturating_add(visits);
if *work > budget {
return Err(limit_error("gaps exceed the work budget; check fewer parts or leave out report_gaps"));
}
if bound <= 0.0 {
return Ok(best);
}
}
// Wholly inside, no surface crossing: a vertex of one inside the other.
let overlap =
ba.min.x <= bb.max.x && bb.min.x <= ba.max.x && ba.min.y <= bb.max.y && bb.min.y <= ba.max.y && ba.min.z <= bb.max.z && bb.min.z <= ba.max.z;
if overlap {
*work = work.saturating_add(a.len() + b.len());
for (p, other) in [(a.first().map(|t| t[0]), b), (b.first().map(|t| t[0]), a)] {
if let Some(p) = p
&& super::inside(p, other)
{
return Ok(Some((0.0, p, p)));
}
}
}
Ok(best.filter(|(d, _, _)| *d <= limit))
}
/// The distance between triangles `a` and `b`, with their nearest points (0 where they cross).
fn tri_distance(a: &[Vec3; 3], b: &[Vec3; 3]) -> (f64, Vec3, Vec3) {
for (x, y) in [(a, b), (b, a)] {
for i in 0..3 {
if let (Some(p), Some(q)) = (x.get(i), x.get((i + 1) % 3))
&& let Some(hit) = segment_hits_triangle(*p, *q, y)
{
return (0.0, hit, hit);
}
}
}
let mut best = (f64::INFINITY, a[0], b[0]);
let mut keep = |pa: Vec3, pb: Vec3| {
let d = pa.dist(pb);
if d < best.0 {
best = (d, pa, pb);
}
};
for p in a {
keep(*p, super::closest(*p, b[0], b[1], b[2]));
}
for q in b {
keep(super::closest(*q, a[0], a[1], a[2]), *q);
}
for i in 0..3 {
for j in 0..3 {
let (pa, pb) = segment_closest(a[i], a[(i + 1) % 3], b[j], b[(j + 1) % 3]);
keep(pa, pb);
}
}
best
}
/// Where segment pq passes through triangle t (None: it misses, or lies in its plane).
fn segment_hits_triangle(p: Vec3, q: Vec3, t: &[Vec3; 3]) -> Option<Vec3> {
let d = q - p;
let (e1, e2) = (t[1] - t[0], t[2] - t[0]);
let h = d.cross(e2);
let det = e1.dot(h);
// Parallel (or degenerate): the vertex and edge distances answer it.
if det.abs() < 1e-12 * e1.len().max(1e-300) * e2.len().max(1e-300) * d.len().max(1e-300) {
return None;
}
let f = 1.0 / det;
let sv = p - t[0];
let u = f * sv.dot(h);
if !(0.0..=1.0).contains(&u) {
return None;
}
let qv = sv.cross(e1);
let v = f * d.dot(qv);
if v < 0.0 || u + v > 1.0 {
return None;
}
let along = f * e2.dot(qv);
(0.0..=1.0).contains(&along).then(|| p + d * along)
}
/// The nearest points of segments p1q1 and p2q2.
fn segment_closest(p1: Vec3, q1: Vec3, p2: Vec3, q2: Vec3) -> (Vec3, Vec3) {
let (d1, d2, r) = (q1 - p1, q2 - p2, p1 - p2);
let (a, e, f) = (d1.dot(d1), d2.dot(d2), d2.dot(r));
let eps = 1e-24;
let (s, t) = if a <= eps && e <= eps {
(0.0, 0.0)
} else if a <= eps {
(0.0, (f / e).clamp(0.0, 1.0))
} else {
let c = d1.dot(r);
if e <= eps {
((-c / a).clamp(0.0, 1.0), 0.0)
} else {
let b = d1.dot(d2);
let denom = a * e - b * b;
let mut s = if denom > eps * a * e { ((b * f - c * e) / denom).clamp(0.0, 1.0) } else { 0.0 };
let mut t = (b * s + f) / e;
if t < 0.0 {
t = 0.0;
s = (-c / a).clamp(0.0, 1.0);
} else if t > 1.0 {
t = 1.0;
s = ((b - c) / a).clamp(0.0, 1.0);
}
(s, t)
}
};
(p1 + d1 * s, p2 + d2 * t)
}
#[cfg(test)]
#[path = "contact_sweep_tests.rs"]
mod tests;
+4
View File
@@ -1453,6 +1453,10 @@ mod cam;
#[path = "motion_analysis_tests.rs"]
mod motion_analysis;
// Joints swept until parts touch: a door, the four-bar above and a Watt six-bar.
#[path = "motion_sweep_tests.rs"]
mod motion_sweep;
/// A pin through a hole of its own radius whose axis is a hair off the hole's (parts placed from
/// rounded coordinates): the walls are all but tangent along two lines, which the boolean can't
/// intersect. Each offset gets a checked answer: clear on the axis and within the kernel's
+3
View File
@@ -20,6 +20,8 @@ mod analysis;
mod animation;
#[path = "contact_sweep.rs"]
mod contact_sweep;
#[path = "motion_sweep.rs"]
mod sweep;
macro_rules! trace_doc {
() => {
@@ -79,6 +81,7 @@ pub static COMMANDS: &[CommandSpec] = &[
)),
CommandSpec::new("motion.plot", "Plot Motion Study", analysis::plot).icon("graph").noundo().params(analysis::PLOT_DOC),
CommandSpec::new("motion.interference", "Motion Interference", analysis::interference).icon("interference").noundo().params(analysis::INTERFERENCE_DOC),
CommandSpec::new("joint.sweep", "Sweep Joint", sweep::sweep).icon("interference").params(sweep::SWEEP_DOC),
CommandSpec::new("motion.list", "List Motion Studies", list_studies).icon("list").noundo(),
CommandSpec::new("motion.delete", "Delete Motion Study", delete_study).icon("delete").params("study"),
CommandSpec::new("explode.create", "Exploded View", explode_create).at("SOLID", "ASSEMBLE").icon("explode").params(
+12 -4
View File
@@ -310,10 +310,10 @@ fn ranges(m: &MotionStudy, samples: &[usize], n: usize) -> Vec<[f64; 2]> {
out.into_iter().map(|(a, b)| [step_of(m, a, n), step_of(m, b, n)]).collect()
}
pub(super) fn interference(s: &mut Session, p: &Value) -> Result<Value> {
let cmd = "motion.interference";
let m = s.doc.assembly.studies.get(study_index(s, p, cmd)?).cloned().ok_or_else(|| bad(cmd, "study"))?;
let n = samples(p, &m, 2, cmd)?;
/// The pairs of parts a check looks at, as `motion.interference` and `joint.sweep` take them:
/// `occurrences` against each other (or each only `against` others), the design's contact sets
/// with `contact_sets`, else every occurrence.
pub(super) fn checked_pairs(s: &Session, p: &Value, cmd: &str) -> Result<ContactGeometry> {
let everything = || vec![s.doc.occurrences.iter().map(|o| o.id).collect::<Vec<u64>>()];
// Each of `occurrences` only against these.
let against = match p.get("against") {
@@ -354,6 +354,14 @@ pub(super) fn interference(s: &mut Session, p: &Value) -> Result<Value> {
let a = occurrences(s, p, cmd)?;
geometry.keep_pairs(|x, y| (a.contains(&x) && b.contains(&y)) || (a.contains(&y) && b.contains(&x)));
}
Ok(geometry)
}
pub(super) fn interference(s: &mut Session, p: &Value) -> Result<Value> {
let cmd = "motion.interference";
let m = s.doc.assembly.studies.get(study_index(s, p, cmd)?).cloned().ok_or_else(|| bad(cmd, "study"))?;
let n = samples(p, &m, 2, cmd)?;
let geometry = checked_pairs(s, p, cmd)?;
let mut found: BTreeMap<(u64, u64), Collision> = BTreeMap::new();
let (mut work, mut checked, mut truncated) = (0usize, 0usize, false);
let stop = play(s, &m, n, false, |t, k| {
+261
View File
@@ -0,0 +1,261 @@
//! Sweeping a joint until something touches: `joint.sweep` drives one joint value from `from`
//! toward `to` on a copy of the design (closed loops following on their branch), measures the
//! gaps between parts at each sample and bisects the first contact down to `refine`, so a hinge,
//! door or linkage answers how far it opens, what stops it and how much room each part has.
use std::collections::{BTreeMap, BTreeSet};
use std::sync::Arc;
use serde_json::{Value, json};
use solidcraft_doc::Document;
use solidcraft_geom::Vec3;
use super::analysis::checked_pairs;
use super::contact_sweep::{ContactGeometry, Gap};
use super::{drive_with_contact, joint_id, value};
use crate::params::{bad, bool_};
use crate::{Result, Session};
pub const SWEEP_DOC: &str = "joint: id|name; from, to: its value the sweep runs between (degrees for angles, mm for distances, \
or expressions); index? (which value, default 0); step? (between samples, in the joint's unit; default: fine enough that \
no part passes through another between two); refine?: mm (the first contact is bisected until no part moves more than \
this between the last clear value and the touching one; default 0.01); occurrences?: [ids or names] (checked against \
each other; default every occurrence), with against?: [ids or names] (the parts that stay: each of `occurrences` only \
against these), or contact_sets?: true (the pairs of the design's contact sets); report_gaps?: bool (the nearest gap at \
every sample, default false); apply?: bool (leave the design where the sweep stopped; default false: the design is not \
changed) → stopped_at (the last value clear of contact, or where a loop or limit stopped it; null: it reached `to`), \
stopped_by: {a, b, at: [x,y,z] (where they touch, world)} | \"loop\" | \"limit\" | null, samples: [{value, min_gap_mm?, \
pair?}], min_gap? (report_gaps: {part: {mm, at_value, to}}, the nearest each part came to another), unit, gap_method \
(\"mesh\": distances between the tessellations), gap_tolerance_mm (how far those can be off: 0 when every face is flat), \
pairs (checked), touching_at_start: [[a, b]] (pairs already touching at `from`, such as parts joined face to face or a door \
on its frame: these stop it only once they pass into each other deeper than they start), applied. The parts are tested at the samples and through the bisection; the motion is on a copy (closed loops \
follow, contact sets do not stop it)";
/// Most samples a sweep takes before its bisection: each is a solve.
const MAX_SAMPLES: usize = 4096;
/// Most bisection steps: each halves the bracket around the first contact.
const MAX_BISECTIONS: usize = 64;
/// Most gap work over a whole sweep (see `ContactGeometry::gaps`).
const MAX_WORK: usize = 500_000_000;
/// The nearest a part came to another along the sweep.
struct Nearest {
mm: f64,
at: f64,
to: String,
}
pub(super) fn sweep(s: &mut Session, p: &Value) -> Result<Value> {
let cmd = "joint.sweep";
let id = joint_id(s, p.get("joint"), cmd)?;
let joint = s.doc.assembly.joints.iter().find(|j| j.id == id).ok_or_else(|| bad(cmd, "joint"))?;
let (kind, name) = (joint.kind, joint.name.clone());
let dofs = kind.dofs().len();
if dofs == 0 {
return Err(bad(cmd, "a rigid joint does not move"));
}
let index = match p.get("index") {
None | Some(Value::Null) => 0,
Some(v) => v
.as_u64()
.and_then(|i| usize::try_from(i).ok())
.filter(|i| *i < dofs)
.ok_or_else(|| bad(cmd, format!("`index` must be below {dofs}, the joint's value count")))?,
};
let angle = kind.is_angle(index);
// Shown in degrees for angles, mm for distances.
let shown = |v: f64| if angle { v.to_degrees() } else { v };
let end = |key: &str| match p.get(key) {
Some(v) => value(s, v, angle, cmd),
None => Err(bad(cmd, format!("give `from` and `to`: the joint's values the sweep runs between (no `{key}`)"))),
};
let (from, to) = (end("from")?, end("to")?);
let positive = |key: &str, max: f64| -> Result<Option<f64>> {
match p.get(key) {
None | Some(Value::Null) => Ok(None),
Some(v) => v
.as_f64()
.filter(|x| x.is_finite() && *x > 0.0 && *x <= max)
.map(Some)
.ok_or_else(|| bad(cmd, format!("`{key}` must be a number above 0, at most {max}"))),
}
};
let refine = positive("refine", 1e3)?.unwrap_or(0.01);
let step = positive("step", 1e9)?.map(|x| if angle { x.to_radians() } else { x });
let report_gaps = bool_(p, "report_gaps").unwrap_or(false);
let apply = bool_(p, "apply").unwrap_or(false);
let mut geometry = checked_pairs(s, p, cmd)?;
if !geometry.has_pairs() {
return Err(bad(cmd, "no pairs of parts to check: list two or more `occurrences` with bodies, or give `against`"));
}
// On a copy, played from where the parts are placed; contact sets do not stop it.
let mut t = s.scratch();
t.pending_moves.clear();
t.doc_mut().assembly.contact_enabled = false;
let given = BTreeSet::from([(id, index)]);
let drive_to = |t: &mut Session, v: f64| -> Result<Value> {
let mut q = t.doc.assembly.joints.iter().find(|j| j.id == id).map(|j| j.values.clone()).unwrap_or_default();
q.resize(q.len().max(dofs), 0.0);
if let Some(x) = q.get_mut(index) {
*x = v;
}
Ok(drive_with_contact(t, &[(id, q)], &given)?.0)
};
let reached = |t: &Session| t.doc.assembly.joints.iter().find(|j| j.id == id).and_then(|j| j.values.get(index).copied()).unwrap_or(0.0);
let r = drive_to(&mut t, from)?;
if r["stopped_by_loop"].is_object() {
return Err(bad(cmd, "the joint cannot reach `from`: a closed loop it is in does not close there"));
}
if (reached(&t) - from).abs() > 1e-9 {
return Err(bad(cmd, "`from` is outside the joint's limits"));
}
let scale = geometry.scale();
// Nearer than this is touching (well below any `refine`).
let touch = 1e-6 * scale;
let mut work = 0usize;
let key = |x: u64, y: u64| (x.min(y), x.max(y));
// Pairs touching where the sweep starts (parts joined face to face, a door on its frame)
// stop it only when they pass into each other, deeper than they start (by more than their
// meshes can be off): a door resting on its frame cannot sweep through it.
let start: Vec<Gap> = geometry.gaps(&t, touch, &mut work, MAX_WORK)?;
let touching: BTreeSet<(u64, u64)> = start.iter().map(|g| key(g.a.0, g.b.0)).collect();
let touching_names: Vec<Value> = start.iter().map(|g| json!([g.a.1, g.b.1])).collect();
let resting = geometry.take_pairs(|x, y| touching.contains(&key(x, y)));
let rest_depth: BTreeMap<(u64, u64), f64> = if resting.has_pairs() {
resting.hits(&t, &mut work, MAX_WORK, true)?.into_iter().map(|(a, b, d)| (key(a.0, b.0), d)).collect()
} else {
BTreeMap::new()
};
let margin = touch.max(2.0 * resting.chord());
// The samples run to where the joint gets: a limit or a loop can stop it short of `to`, and
// the default count is for the motion up to there. One more drive toward `to` then finds
// what stops it.
let (n, sample_to) = match step {
Some(h) => {
let n = ((to - from).abs() / h).ceil();
if !n.is_finite() || n > MAX_SAMPLES as f64 {
return Err(bad(cmd, format!("`step` makes more than {MAX_SAMPLES} samples: take a larger step")));
}
(n as usize, to)
}
None => {
let mut end = t.scratch();
drive_to(&mut end, to)?;
let n = geometry.samples(&t, &end).map_err(|e| match e {
crate::EngineError::BadParams { msg, .. } => bad(cmd, format!("{msg} (or give a `step`)")),
e => e,
})?;
let got = reached(&end);
(n, if (got - to).abs() > 1e-9 && got.is_finite() { got } else { to })
}
};
let n = n.max(1);
let targets: Vec<f64> =
(1..=n).map(|k| from + (sample_to - from) * k as f64 / n as f64).chain(((sample_to - to).abs() > 1e-9).then_some(to)).collect();
let mut samples: Vec<Value> = Vec::new();
let mut nearest: BTreeMap<String, Nearest> = BTreeMap::new();
let mut record = |t: &Session, v: f64, work: &mut usize, geometry: &ContactGeometry| -> Result<()> {
if !report_gaps {
samples.push(json!({"value": shown(v)}));
return Ok(());
}
let gaps = geometry.gaps(t, f64::INFINITY, work, MAX_WORK)?;
match gaps.first() {
Some(g) => samples.push(json!({"value": shown(v), "min_gap_mm": g.mm, "pair": [g.a.1, g.b.1]})),
None => samples.push(json!({"value": shown(v), "min_gap_mm": null, "pair": null})),
}
for g in &gaps {
for (me, other) in [(&g.a.1, &g.b.1), (&g.b.1, &g.a.1)] {
if nearest.get(me).is_none_or(|x| g.mm < x.mm) {
nearest.insert(me.clone(), Nearest { mm: g.mm, at: shown(v), to: other.clone() });
}
}
}
Ok(())
};
let contact = |t: &Session, work: &mut usize, geometry: &ContactGeometry| -> Result<Option<Gap>> {
if let Some(g) = geometry.gaps(t, touch, work, MAX_WORK)?.into_iter().next() {
return Ok(Some(g));
}
if !resting.has_pairs() {
return Ok(None);
}
let deeper =
resting.hits(t, work, MAX_WORK, true)?.into_iter().find(|(a, b, d)| *d > rest_depth.get(&key(a.0, b.0)).copied().unwrap_or(0.0) + margin);
let Some((a, b, _)) = deeper else { return Ok(None) };
// Where: the nearest points of that pair (passing into each other, at no gap).
let k = key(a.0, b.0);
let at = resting.gaps(t, touch, work, MAX_WORK)?.into_iter().find(|g| key(g.a.0, g.b.0) == k).map_or((Vec3::ZERO, Vec3::ZERO), |g| g.at);
Ok(Some(Gap { a, b, mm: 0.0, at }))
};
record(&t, from, &mut work, &geometry)?;
let (mut lo_doc, mut lo_v): (Arc<Document>, f64) = (t.doc.clone(), from);
let mut stop: Option<(f64, Value)> = None;
for target in targets {
let r = drive_to(&mut t, target)?;
let v = reached(&t);
if let Some(mut hit) = contact(&t, &mut work, &geometry)? {
// Halve the bracket from the last clear value until no part moves more than
// `refine` across it; each try is driven from the clear side, so loops keep their branch.
let (mut hi_doc, mut hi_v) = (t.doc.clone(), v);
for _ in 0..MAX_BISECTIONS {
if geometry.travel(&lo_doc, &hi_doc)? <= refine {
break;
}
let mid = (lo_v + hi_v) / 2.0;
t.doc = lo_doc.clone();
drive_to(&mut t, mid)?;
match contact(&t, &mut work, &geometry)? {
Some(h) => (hi_doc, hi_v, hit) = (t.doc.clone(), mid, h),
None => (lo_doc, lo_v) = (t.doc.clone(), mid),
}
}
t.doc = lo_doc.clone();
let at = (hit.at.0 + hit.at.1) * 0.5;
stop = Some((lo_v, json!({"a": hit.a.1, "b": hit.b.1, "at": [at.x, at.y, at.z]})));
break;
}
// (The drive on toward `to` past a limit stays where the last sample was.)
if (v - lo_v).abs() > 1e-12 {
record(&t, v, &mut work, &geometry)?;
}
if r["stopped_by_loop"].is_object() {
stop = Some((v, json!("loop")));
break;
}
if (v - target).abs() > 1e-9 {
stop = Some((v, json!("limit")));
break;
}
(lo_doc, lo_v) = (t.doc.clone(), v);
}
let applied = apply && !Arc::ptr_eq(&t.doc, &s.doc);
if applied {
let mut doc = (*t.doc).clone();
doc.assembly.contact_enabled = s.doc.assembly.contact_enabled;
*s.doc_mut() = doc;
}
let mut out = json!({
"joint": id, "name": name, "index": index, "unit": if angle { "deg" } else { "mm" },
"from": shown(from), "to": shown(to),
"stopped_at": stop.as_ref().map(|(v, _)| shown(*v)),
"stopped_by": stop.map(|(_, by)| by),
"samples": samples,
"refine_mm": refine,
"gap_method": "mesh", "gap_tolerance_mm": geometry.chord(),
"pairs": geometry.pair_count() + resting.pair_count(),
"touching_at_start": touching_names,
"applied": applied,
});
if report_gaps {
out["min_gap"] =
nearest.into_iter().map(|(k, g)| (k, json!({"mm": g.mm, "at_value": g.at, "to": g.to}))).collect::<serde_json::Map<_, _>>().into();
}
Ok(out)
}
+307
View File
@@ -0,0 +1,307 @@
//! `joint.sweep` against stops worked out by hand: a door swinging into a block, the four-bar's
//! rocker meeting a stop and a Watt six-bar's output link meeting one, each checked against the
//! linkage solved with circle intersections and a separating-axis test of the parts' outlines.
use serde_json::{Value, json};
use solidcraft_geom::Vec3;
use super::{four_bar, meet, occ, part, pin, run};
use crate::Session;
/// A part made of one box (`size` from `corner`) in a new component.
fn block(s: &mut Session, name: &str, corner: [f64; 3], size: [f64; 3]) {
run(s, "component.activate", json!({"component": "root"}));
run(s, "component.create", json!({"name": name}));
run(s, "solid.box", json!({"length": size[0], "width": size[1], "height": size[2], "corner": corner}));
}
/// A 100 × 4 × 50 door hinged about Z at the origin, and a 20 × 20 block from (60, 30) in its
/// way.
fn door() -> Session {
let mut s = Session::default();
block(&mut s, "Door", [0.0, 0.0, 0.0], [100.0, 4.0, 50.0]);
block(&mut s, "Block", [60.0, 30.0, 0.0], [20.0, 20.0, 50.0]);
let r = run(
&mut s,
"joint.create",
json!({"type": "revolute", "a": {"occurrence": 0, "point": [0, 0, 0]}, "b": {"occurrence": "Door", "point": [0, 0, 0]}, "flip": true, "name": "Hinge"}),
);
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
s
}
fn num(v: &Value) -> f64 {
v.as_f64().unwrap_or(f64::NAN)
}
fn hinge(s: &Session) -> f64 {
s.doc.assembly.joints.iter().find(|j| j.name == "Hinge").and_then(|j| j.values.first().copied()).unwrap_or(f64::NAN)
}
/// The door's face 4 mm off its hinge line meets a corner of the block at polar (r, φ) when
/// r·sin(φ − θ) = 4, so at θ = φ − asin(4 / r); the corner (80, 30) is the first.
#[test]
fn a_door_swings_until_it_meets_the_block() {
let mut s = door();
let corner = |x: f64, y: f64| y.atan2(x) - (4.0 / x.hypot(y)).asin();
let expected =
[(60.0, 30.0), (80.0, 30.0), (60.0, 50.0), (80.0, 50.0)].iter().map(|(x, y)| corner(*x, *y)).fold(f64::INFINITY, f64::min).to_degrees();
let before = s.doc.clone();
let r = run(&mut s, "joint.sweep", json!({"joint": "Hinge", "from": 0, "to": 90, "report_gaps": true}));
assert_eq!(*s.doc, *before, "a sweep leaves the design as it was");
assert_eq!(r["applied"], false);
let at = num(&r["stopped_at"]);
assert!((at - expected).abs() < 0.01 && at <= expected + 1e-9, "stopped at {at}°, the block is met at {expected}°: {r}");
let by = &r["stopped_by"];
let names = [by["a"].as_str(), by["b"].as_str()];
assert!(names.contains(&Some("Door:1")) && names.contains(&Some("Block:1")), "{by}");
let touch = Vec3::new(num(&by["at"][0]), num(&by["at"][1]), num(&by["at"][2]));
assert!((touch.x - 80.0).abs() < 0.05 && (touch.y - 30.0).abs() < 0.05 && (0.0..=50.0).contains(&touch.z), "{touch:?}");
// Flat faces: the gaps are the boxes' own.
assert_eq!((r["gap_method"].as_str(), num(&r["gap_tolerance_mm"])), (Some("mesh"), 0.0));
let samples = r["samples"].as_array().cloned().unwrap_or_default();
assert!(samples.len() > 2, "{r}");
assert!((num(&samples[0]["min_gap_mm"]) - 26.0).abs() < 1e-9, "the door starts 26 mm below the block: {}", samples[0]);
for w in samples.windows(2) {
assert!(num(&w[1]["min_gap_mm"]) < num(&w[0]["min_gap_mm"]) && num(&w[1]["value"]) > num(&w[0]["value"]), "{w:?}");
}
let nearest = &r["min_gap"]["Door:1"];
assert!(nearest["to"] == "Block:1" && num(&nearest["mm"]) < 2.0 && num(&nearest["at_value"]) < expected, "{}", r["min_gap"]);
// Kept where it stopped on request.
let r = run(&mut s, "joint.sweep", json!({"joint": "Hinge", "from": 0, "to": 90, "refine": 0.001, "apply": true}));
assert_eq!(r["applied"], true);
assert!((hinge(&s).to_degrees() - expected).abs() < 0.002, "{}° wants {expected}°", hinge(&s).to_degrees());
run(&mut s, "contact.create", json!({"occurrences": ["Door", "Block"]}));
let r = run(&mut s, "contact.check", json!({}));
assert!(r["collisions"].as_array().is_some_and(Vec::is_empty), "kept clear of the block: {r}");
}
/// Swung the other way, nothing is in the way; a limit stops it, and a sweep starting there
/// does not move.
#[test]
fn a_door_swings_free_until_its_limit() {
let mut s = door();
let r = run(&mut s, "joint.sweep", json!({"joint": "Hinge", "from": 0, "to": -90, "step": 5}));
assert!(r["stopped_at"].is_null() && r["stopped_by"].is_null(), "{r}");
assert_eq!(r["samples"].as_array().map(Vec::len), Some(19), "{r}");
run(&mut s, "joint.limits", json!({"joint": "Hinge", "min": -30, "max": 10}));
let r = run(&mut s, "joint.sweep", json!({"joint": "Hinge", "from": 0, "to": -90}));
assert_eq!((r["stopped_by"].as_str(), num(&r["stopped_at"]).round()), (Some("limit"), -30.0), "{r}");
assert!(s.execute("joint.sweep", &json!({"joint": "Hinge", "from": 20, "to": 0})).is_err(), "`from` is past the limit");
}
/// Parts already touching where the sweep starts (a door resting on its frame) do not stop it
/// while they move apart, but they do once they pass into each other: the door cannot sweep
/// through its frame.
#[test]
fn parts_touching_at_the_start_stop_it_only_passing_into_each_other() {
let mut s = door();
block(&mut s, "Frame", [0.0, -10.0, 0.0], [100.0, 10.0, 50.0]);
let r = run(&mut s, "joint.sweep", json!({"joint": "Hinge", "from": 0, "to": 90}));
let touching = r["touching_at_start"].as_array().cloned().unwrap_or_default();
assert_eq!(touching.len(), 1, "{r}");
let by = &r["stopped_by"];
assert!(by.is_object() && [&by["a"], &by["b"]].contains(&&json!("Block:1")), "the block still stops it: {r}");
// Swung open, away from the frame: nothing stops it.
let only = json!({"joint": "Hinge", "from": 0, "to": 90, "occurrences": ["Door"], "against": ["Frame"]});
let r = run(&mut s, "joint.sweep", only);
assert!(r["stopped_by"].is_null() && r["touching_at_start"].as_array().map(Vec::len) == Some(1), "{r}");
// Swung into the frame: it stops where it starts.
let into = json!({"joint": "Hinge", "from": 0, "to": -90, "occurrences": ["Door"], "against": ["Frame"]});
let r = run(&mut s, "joint.sweep", into);
let by = &r["stopped_by"];
assert!(by.is_object() && [&by["a"], &by["b"]].contains(&&json!("Frame:1")), "{r}");
assert!(num(&r["stopped_at"]).abs() < 0.01, "{r}");
}
/// A limit stops the door well short of `to`: the samples are still as fine as the motion up to
/// the limit needs, so a thin plate just inside the limit is met, not stepped over.
#[test]
fn a_thin_part_just_inside_a_limit_is_met() {
let mut s = Session::default();
block(&mut s, "Door", [0.0, 0.0, 0.0], [100.0, 1.0, 50.0]);
block(&mut s, "Plate", [80.0, -32.0, 0.0], [0.5, 6.0, 50.0]);
run(
&mut s,
"joint.create",
json!({"type": "revolute", "a": {"occurrence": 0, "point": [0, 0, 0]}, "b": {"occurrence": "Door", "point": [0, 0, 0]}, "flip": true, "name": "Hinge"}),
);
run(&mut s, "joint.limits", json!({"joint": "Hinge", "min": -30, "max": 10}));
// The door's face through the hinge meets the plate's far upper corner (80.5, -26).
let expected = (-26.0f64).atan2(80.5).to_degrees();
let r = run(&mut s, "joint.sweep", json!({"joint": "Hinge", "from": 0, "to": -1800}));
let by = &r["stopped_by"];
assert!(by.is_object() && [&by["a"], &by["b"]].contains(&&json!("Plate:1")), "the plate stops it, not the limit: {r}");
let at = num(&r["stopped_at"]);
assert!((at - expected).abs() < 0.01 && at >= expected - 1e-9, "stopped at {at}°, the plate is met at {expected}°: {r}");
// The samples are as fine as the motion up to the limit needs (spread over `to`, they were
// sixty times coarser).
let values: Vec<f64> = r["samples"].as_array().unwrap().iter().map(|v| num(&v["value"])).collect();
assert!(values.len() > 100, "{r}");
for w in values.windows(2) {
assert!((w[1] - w[0]).abs() < 0.5, "samples {}° apart: {r}", (w[1] - w[0]).abs());
}
}
#[test]
fn a_sweep_refuses_what_it_cannot_do() {
let mut s = door();
for p in [
json!({"joint": "Hinge", "to": 90}),
json!({"joint": "Hinge", "from": 0}),
json!({"joint": "Nope", "from": 0, "to": 90}),
json!({"joint": "Hinge", "from": 0, "to": 90, "index": 3}),
json!({"joint": "Hinge", "from": 0, "to": 90, "refine": 0}),
json!({"joint": "Hinge", "from": 0, "to": 90, "refine": -1}),
json!({"joint": "Hinge", "from": 0, "to": 90, "step": 0}),
json!({"joint": "Hinge", "from": 0, "to": 1e8, "step": 1e-6}),
json!({"joint": "Hinge", "from": "nan", "to": 90}),
json!({"joint": "Hinge", "from": 0, "to": 90, "occurrences": ["Door"]}),
json!({"joint": "Hinge", "from": 0, "to": 90, "against": ["Block"]}),
] {
assert!(s.execute("joint.sweep", &p).is_err(), "{p}");
}
run(&mut s, "joint.as_built", json!({"type": "rigid", "a": "Block", "b": "Door", "name": "Lock"}));
assert!(s.execute("joint.sweep", &json!({"joint": "Lock", "from": 0, "to": 1})).is_err(), "a rigid joint does not move");
}
/// A point turned about `c` by `a` radians in the XY plane.
fn turn(p: Vec3, c: Vec3, a: f64) -> Vec3 {
let (sin, cos) = a.sin_cos();
let d = p - c;
Vec3::new(c.x + d.x * cos - d.y * sin, c.y + d.x * sin + d.y * cos, p.z)
}
/// The XY outline of a 2 × 2 part box from `corner`, turned about `c` by `a`.
fn outline(corner: Vec3, size: f64, c: Vec3, a: f64) -> [Vec3; 4] {
[(0.0, 0.0), (size, 0.0), (size, size), (0.0, size)].map(|(x, y)| turn(corner + Vec3::new(x, y, 0.0), c, a))
}
/// Whether two convex outlines overlap (no edge normal of either separates them).
fn overlap(a: &[Vec3; 4], b: &[Vec3; 4]) -> bool {
for poly in [a, b] {
for i in 0..4 {
let e = poly[(i + 1) % 4] - poly[i];
let n = Vec3::new(-e.y, e.x, 0.0);
let span = |p: &[Vec3; 4]| p.iter().map(|q| q.dot(n)).fold((f64::INFINITY, f64::NEG_INFINITY), |(lo, hi), x| (lo.min(x), hi.max(x)));
let ((a0, a1), (b0, b1)) = (span(a), span(b));
if a1 <= b0 || b1 <= a0 {
return false;
}
}
}
true
}
/// The crank value (degrees, 0…`hi`) where `clear` first fails, bisected to 1e-9°.
fn first_contact(hi: f64, clear: impl Fn(f64) -> bool) -> f64 {
let (mut lo, mut hi) = (0.0, hi);
assert!(clear(lo) && !clear(hi), "the stop must lie between 0° and {hi}°");
while hi - lo > 1e-9 {
let mid = (lo + hi) / 2.0;
if clear(mid) { lo = mid } else { hi = mid }
}
lo
}
/// A part box 3 mm square about `c` (in XY), deep enough for the linkage's boxes (z −5…−3).
fn stop_at(s: &mut Session, c: Vec3) {
block(s, "Stop", [c.x - 1.5, c.y - 1.5, -6.0], [3.0, 3.0, 4.0]);
let id = occ(s, "Stop");
run(s, "occurrence.ground", json!({"occurrence": id, "grounded": true}));
}
const O4: Vec3 = Vec3::new(45.0, 0.0, 0.0);
/// The four-bar's crank driven to `v`°, followed a quarter degree at a time from where it was
/// assembled: the crank pin and the rocker pin.
fn four_bar_at(v: f64, q0: Vec3) -> (Vec3, Vec3) {
let n = ((v.abs() * 4.0).ceil() as usize).max(1);
let (mut p, mut q) = (Vec3::new(0.0, 20.0, 0.0), q0);
for k in 1..=n {
let th = (90.0 + v * k as f64 / n as f64).to_radians();
p = Vec3::new(20.0 * th.cos(), 20.0 * th.sin(), 0.0);
let [m1, m2] = meet(p, 50.0, O4, 40.0);
q = if m1.dist(q) <= m2.dist(q) { m1 } else { m2 };
}
(p, q)
}
fn angle(d: Vec3) -> f64 {
d.y.atan2(d.x)
}
/// The four-bar's rocker swings into a stop: the crank stops where the rocker's box first
/// overlaps it.
#[test]
fn a_four_bar_rocker_meets_a_stop() {
let (mut s, _, q0) = four_bar();
let rocker = |v: f64| angle(four_bar_at(v, q0).1 - O4) - angle(q0 - O4);
let corner = Vec3::new(q0.x, q0.y, 0.0);
let centre = turn(corner + Vec3::new(1.0, 1.0, 0.0), O4, rocker(60.0));
stop_at(&mut s, centre);
let stop = [(-1.5, -1.5), (1.5, -1.5), (1.5, 1.5), (-1.5, 1.5)].map(|(x, y)| centre + Vec3::new(x, y, 0.0));
let expected = first_contact(60.0, |v| !overlap(&outline(corner, 2.0, O4, rocker(v)), &stop));
let r =
run(&mut s, "joint.sweep", json!({"joint": "Crank", "from": 0, "to": 90, "refine": 0.001, "occurrences": ["Rocker"], "against": ["Stop"]}));
let at = num(&r["stopped_at"]);
assert!((at - expected).abs() < 0.01, "stopped at {at}°, the rocker meets the stop at {expected}°: {r}");
let names = [r["stopped_by"]["a"].as_str(), r["stopped_by"]["b"].as_str()];
assert!(names.contains(&Some("Rocker:1")) && names.contains(&Some("Stop:1")), "{r}");
assert_eq!(r["pairs"], 1);
}
/// Driving the four-bar's rocker past its toggle stops where the loop can no longer close
/// (|Q| = 30: about 139.2° from the ground line).
#[test]
fn a_four_bar_sweep_stops_where_its_loop_cannot_close() {
let (mut s, _, q0) = four_bar();
let limit = ((900.0f64 - 3625.0) / 3600.0).acos().to_degrees() - q0.y.atan2(q0.x - 45.0).to_degrees();
let r = run(&mut s, "joint.sweep", json!({"joint": "Rocker", "from": 0, "to": 100, "occurrences": ["Rocker"], "against": ["Ground"]}));
assert_eq!(r["stopped_by"].as_str(), Some("loop"), "{r}");
let at = num(&r["stopped_at"]);
assert!(at > limit - 1.0 && at <= limit + 1e-6, "stopped at {at}°, the loop closes up to {limit}°");
}
/// A Watt six-bar: the four-bar above, its rocker a ternary link carrying a pin R0 = Q0 + 15 x,
/// and a second loop R–S–O6 (link 40, output 35, O6 = (90, 0)). The output link's box swings
/// into a stop.
#[test]
fn a_watt_six_bar_output_meets_a_stop() {
let (mut s, _, q0) = four_bar();
let o6 = Vec3::new(90.0, 0.0, 0.0);
let r0 = q0 + Vec3::new(15.0, 0.0, 0.0);
let [s0, _] = meet(r0, 40.0, o6, 35.0);
for (name, c) in [("Link", (r0 + s0) * 0.5), ("Output", s0)] {
part(&mut s, name, [c.x, c.y, -5.0]);
}
pin(&mut s, "revolute", "Rocker arm", "Rocker", "Link", r0, Vec3::Z);
pin(&mut s, "revolute", "Output pin", "Link", "Output", s0, Vec3::Z);
pin(&mut s, "revolute", "Output", "Ground", "Output", o6, Vec3::Z);
// The output's turn with the crank at `v`°, both loops followed a quarter degree at a time.
let output = |v: f64| {
let n = ((v.abs() * 4.0).ceil() as usize).max(1);
let mut sp = s0;
for k in 1..=n {
let (_, q) = four_bar_at(v * k as f64 / n as f64, q0);
let r = turn(r0, O4, angle(q - O4) - angle(q0 - O4));
let [m1, m2] = meet(r, 40.0, o6, 35.0);
sp = if m1.dist(sp) <= m2.dist(sp) { m1 } else { m2 };
}
angle(sp - o6) - angle(s0 - o6)
};
// Where the solver puts the output at 30° is where the circles say.
let check = run(&mut s.scratch(), "joint.drive", json!({"joint": "Crank", "value": 30}));
assert!(check["stopped_by_loop"].is_null(), "{check}");
let corner = Vec3::new(s0.x, s0.y, 0.0);
let centre = turn(corner + Vec3::new(1.0, 1.0, 0.0), o6, output(60.0));
stop_at(&mut s, centre);
let stop = [(-1.5, -1.5), (1.5, -1.5), (1.5, 1.5), (-1.5, 1.5)].map(|(x, y)| centre + Vec3::new(x, y, 0.0));
let expected = first_contact(60.0, |v| !overlap(&outline(corner, 2.0, o6, output(v)), &stop));
let r =
run(&mut s, "joint.sweep", json!({"joint": "Crank", "from": 0, "to": 90, "refine": 0.001, "occurrences": ["Output"], "against": ["Stop"]}));
let at = num(&r["stopped_at"]);
assert!((at - expected).abs() < 0.01, "stopped at {at}°, the output meets the stop at {expected}°: {r}");
let names = [r["stopped_by"]["a"].as_str(), r["stopped_by"]["b"].as_str()];
assert!(names.contains(&Some("Output:1")) && names.contains(&Some("Stop:1")), "{r}");
}
+13
View File
@@ -240,6 +240,19 @@ sets tell them apart: an occurrence inside a sub-assembly placed twice is one pa
copies are not checked against each other. A check takes at most 256 bodies; on a large design,
list the `occurrences` to check (an arm, with `against` the parts it must miss). When the work
runs out it says `truncated` and the step it `checked_to`.
`joint.sweep {joint, from, to, index?, step?, refine?, occurrences?, against?, contact_sets?,
report_gaps?, apply?}` drives one joint value from `from` toward `to` and answers how far it gets
before parts touch: `stopped_at` (the last value clear of contact), `stopped_by` (the two parts
and the point `at` which they touch, or `"loop"` where a closed loop can no longer close, or
`"limit"`), and with `report_gaps` the nearest gap at every sample and the nearest each part came
to another (`min_gap`). The samples are fine enough that no part passes through another between
two (or `step` apart), and the first contact is bisected until no part moves more than `refine`
(0.01 mm by default) across it. Gaps are distances between the parts' tessellations
(`gap_method: "mesh"`): exact for flat faces, within `gap_tolerance_mm` for curved ones. Pairs
already touching at `from`, such as parts joined face to face or a door resting on its frame, are
listed in `touching_at_start` and stop the sweep only once they pass into each other, deeper than
they start. The sweep runs on a copy (closed loops follow, contact sets don't stop it) and
leaves the design as it was unless `apply` is true.
**Insert Design** (`component.insert_design {path, translate?}`, on the **ASSEMBLE** panel and
in the **File** menu) adds a component linked to another saved design, so each part can live in
+1
View File
@@ -59,6 +59,7 @@ Written against SolidCraft `main` after 0.11.1, which counts the degrees of free
- **Trace:** `motion.trace {"occurrence":..., "point":[x,y,z], "joint":..., "from":0, "to":360}` (or `"study":...` instead of joint, from and to) answers the world path of a point of a part, such as a coupler curve, without changing the design. It ends at `stopped_at` where a loop or contact stops the motion. `motion.trace.keep` with the same params keeps it as a 3D sketch spline.
- **Plot:** `motion.plot {"study":..., "joints":[...], "duration":seconds}` answers each joint value's `value`, `velocity` and `acceleration` per sample (finite differences; per step, or per second with a duration), and with `"occurrence"` and `"point"` a point's `position`, `velocity`, `acceleration`, `speed`. Check a follower's rate against the analytic one this way, without changing the design.
- **Collisions:** `motion.interference {"study":..., "occurrences":[...], "against":[...]}` plays the study (contact sets don't stop it) and lists each colliding pair with `first_step` and `steps` ranges; `clear: true` when none. It takes at most 256 bodies: on a big design list the moving part as `occurrences` and what it must miss as `against`. Parts joined at a knuckle often overlap by design and show up at every step.
- **Sweep until contact:** `joint.sweep {"joint":..., "from":0, "to":120, "against":[...], "report_gaps":true}` answers how far a hinge, door or linkage opens: `stopped_at` (last clear value, bisected to `refine` mm, default 0.01), `stopped_by` `{a, b, at}` (or `"loop"`, `"limit"`), per-sample `min_gap_mm` and the nearest each part came (`min_gap`). It runs on a copy; `"apply":true` leaves the design at the stop. Pairs touching at `from` (`touching_at_start`) stop it only once they pass into each other.
## Verify
- **Each joint:** `execute occurrence.properties {"occurrence":"Wing:1"}` (under 1 KB) gives the world bbox and centre of mass of one occurrence. Joining must not move them.