diff --git a/assets/locales/de.json b/assets/locales/de.json index 8570b54c6..4562094e6 100644 --- a/assets/locales/de.json +++ b/assets/locales/de.json @@ -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", diff --git a/assets/locales/en.json b/assets/locales/en.json index 26538886b..a948a4e06 100644 --- a/assets/locales/en.json +++ b/assets/locales/en.json @@ -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", diff --git a/assets/locales/fr.json b/assets/locales/fr.json index 319456076..803fba008 100644 --- a/assets/locales/fr.json +++ b/assets/locales/fr.json @@ -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", diff --git a/assets/locales/ja.json b/assets/locales/ja.json index 02c5e371d..ff55a1600 100644 --- a/assets/locales/ja.json +++ b/assets/locales/ja.json @@ -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": "ローカルライブラリを確認", diff --git a/assets/locales/ko.json b/assets/locales/ko.json index 21198a398..d1bb0d9a6 100644 --- a/assets/locales/ko.json +++ b/assets/locales/ko.json @@ -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": "로컬 라이브러리 확인", diff --git a/assets/locales/ru.json b/assets/locales/ru.json index 9cd04228a..894a42681 100644 --- a/assets/locales/ru.json +++ b/assets/locales/ru.json @@ -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": "Удалить физический материал", diff --git a/assets/locales/tr.json b/assets/locales/tr.json index f22c0478b..c6d91825a 100644 --- a/assets/locales/tr.json +++ b/assets/locales/tr.json @@ -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", diff --git a/assets/locales/uk.json b/assets/locales/uk.json index ad83c4703..da63577d1 100644 --- a/assets/locales/uk.json +++ b/assets/locales/uk.json @@ -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": "Перевірити локальну бібліотеку", diff --git a/assets/locales/zh.json b/assets/locales/zh.json index f36707f37..1a4fd6a17 100644 --- a/assets/locales/zh.json +++ b/assets/locales/zh.json @@ -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": "检查本地库", diff --git a/crates/engine/src/cmd/contact_sweep.rs b/crates/engine/src/cmd/contact_sweep.rs index 19a0e4cac..7004e9a7e 100644 --- a/crates/engine/src/cmd/contact_sweep.rs +++ b/crates/engine/src/cmd/contact_sweep.rs @@ -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, offset: Mat, mesh: Arc, 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> = HashMap::new(); + let mut meshes: HashMap, f64)> = HashMap::new(); let mut stack = vec![(0, Vec::::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 { + Self::frame_in(body, &s.doc) + } + + fn frame_in(body: &CachedBody, doc: &Document) -> Result { 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> { + 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> { 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 = 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::>()?; + let meshes = self.placed(s)?; // Each mesh's box once, not once per pair it is in. let boxes: Vec = 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> { + let meshes = self.placed(s)?; + let tris: Vec> = meshes.iter().map(|m| m.triangles.iter().filter_map(|t| m.tri(t)).collect()).collect(); + let boxes: Vec = meshes.iter().map(Mesh::bounds).collect(); + let mut trees: Vec> = 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::>())); + } + 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 = 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 { + 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> { + 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 { + 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; diff --git a/crates/engine/src/cmd/joints_tests.rs b/crates/engine/src/cmd/joints_tests.rs index adf3df2fc..87a415b06 100644 --- a/crates/engine/src/cmd/joints_tests.rs +++ b/crates/engine/src/cmd/joints_tests.rs @@ -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 diff --git a/crates/engine/src/cmd/motion.rs b/crates/engine/src/cmd/motion.rs index 9cb31e417..d84ccb75b 100644 --- a/crates/engine/src/cmd/motion.rs +++ b/crates/engine/src/cmd/motion.rs @@ -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( diff --git a/crates/engine/src/cmd/motion_analysis.rs b/crates/engine/src/cmd/motion_analysis.rs index d842d9c9c..ab3c51d3f 100644 --- a/crates/engine/src/cmd/motion_analysis.rs +++ b/crates/engine/src/cmd/motion_analysis.rs @@ -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 { - 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 { let everything = || vec![s.doc.occurrences.iter().map(|o| o.id).collect::>()]; // 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 { 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 { + 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| { diff --git a/crates/engine/src/cmd/motion_sweep.rs b/crates/engine/src/cmd/motion_sweep.rs new file mode 100644 index 000000000..faca1f5c8 --- /dev/null +++ b/crates/engine/src/cmd/motion_sweep.rs @@ -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 { + 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> { + 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 { + 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 = 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 = 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 = + (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 = Vec::new(); + let mut nearest: BTreeMap = 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> { + 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, 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::>().into(); + } + Ok(out) +} diff --git a/crates/engine/src/cmd/motion_sweep_tests.rs b/crates/engine/src/cmd/motion_sweep_tests.rs new file mode 100644 index 000000000..d5e767d7d --- /dev/null +++ b/crates/engine/src/cmd/motion_sweep_tests.rs @@ -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 = 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}"); +} diff --git a/docs/book/src/guide/assemblies.md b/docs/book/src/guide/assemblies.md index 033451123..39862cff8 100644 --- a/docs/book/src/guide/assemblies.md +++ b/docs/book/src/guide/assemblies.md @@ -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 diff --git a/skills/solidcraft-mechanisms/SKILL.md b/skills/solidcraft-mechanisms/SKILL.md index a2e3616b4..7f6d7639a 100644 --- a/skills/solidcraft-mechanisms/SKILL.md +++ b/skills/solidcraft-mechanisms/SKILL.md @@ -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.