Merge pull request #1712 from SychPL/feat/motion-analysis
Motion analysis: chart a study's joints and points, and find the parts that collide along it
This commit is contained in:
23 files changed
+1588
-51
No files matched your search
@@ -27,9 +27,13 @@ struct CachedBody {
|
||||
size: f64,
|
||||
}
|
||||
|
||||
/// A member of a contact set: (occurrence id, name).
|
||||
pub(super) type Member = (u64, String);
|
||||
|
||||
pub(super) struct ContactGeometry {
|
||||
bodies: Vec<CachedBody>,
|
||||
pairs: Vec<(usize, usize, String, String)>,
|
||||
/// Body indices, and the set members (occurrence id, name) they are under.
|
||||
pairs: Vec<(usize, usize, Member, Member)>,
|
||||
step: f64,
|
||||
}
|
||||
|
||||
@@ -41,6 +45,12 @@ impl ContactGeometry {
|
||||
} else {
|
||||
assembly.contact_sets.iter().filter(|set| !set.suppressed).map(|set| set.occurrences.clone()).collect()
|
||||
};
|
||||
Self::of_sets(s, &sets)
|
||||
}
|
||||
|
||||
/// The geometry of the bodies under `sets` (occurrence ids), paired as contact sets pair
|
||||
/// them: bodies under different members of one set.
|
||||
pub(super) fn of_sets(s: &Session, sets: &[Vec<u64>]) -> Result<Self> {
|
||||
let mut out = Self { bodies: Vec::new(), pairs: Vec::new(), step: f64::INFINITY };
|
||||
if sets.is_empty() {
|
||||
return Ok(out);
|
||||
@@ -120,7 +130,7 @@ impl ContactGeometry {
|
||||
}
|
||||
}
|
||||
let mut pairs = BTreeSet::new();
|
||||
for set in &sets {
|
||||
for set in sets {
|
||||
let tagged: Vec<(usize, u64)> =
|
||||
out.bodies.iter().enumerate().filter_map(|(i, b)| b.path.iter().rev().find(|id| set.contains(id)).map(|id| (i, *id))).collect();
|
||||
for (i, (a, oa)) in tagged.iter().enumerate() {
|
||||
@@ -132,7 +142,7 @@ impl ContactGeometry {
|
||||
}
|
||||
}
|
||||
let name = |id| s.doc.occurrences.iter().find(|o| o.id == id).map(|o| o.name.clone()).unwrap_or_default();
|
||||
out.pairs = pairs.into_iter().map(|(a, b, oa, ob)| (a, b, name(oa), name(ob))).collect();
|
||||
out.pairs = pairs.into_iter().map(|(a, b, oa, ob)| (a, b, (oa, name(oa)), (ob, name(ob)))).collect();
|
||||
Ok(out)
|
||||
}
|
||||
|
||||
@@ -140,6 +150,21 @@ impl ContactGeometry {
|
||||
!self.pairs.is_empty()
|
||||
}
|
||||
|
||||
/// Keeps only the pairs of set members (occurrence ids) `keep` takes.
|
||||
pub(super) fn keep_pairs(&mut self, keep: impl Fn(u64, u64) -> bool) {
|
||||
self.pairs.retain(|(_, _, a, b)| keep(a.0, b.0));
|
||||
}
|
||||
|
||||
/// 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)
|
||||
}
|
||||
|
||||
/// How many pairs of set members are checked against each other.
|
||||
pub(super) fn pair_count(&self) -> usize {
|
||||
self.pairs.iter().map(|(_, _, a, b)| (a.0.min(b.0), a.0.max(b.0))).collect::<BTreeSet<_>>().len()
|
||||
}
|
||||
|
||||
fn frame(body: &CachedBody, s: &Session) -> Result<Mat> {
|
||||
let mut frame = IDENTITY;
|
||||
for id in &body.path {
|
||||
@@ -205,6 +230,16 @@ impl ContactGeometry {
|
||||
}
|
||||
|
||||
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)))
|
||||
}
|
||||
|
||||
/// The pairs of set members whose bodies pass through each other in `s`, with how deep the
|
||||
/// first two bodies found go (every pair, or the first found when `all` is false). `work`
|
||||
/// adds up the cost, refused past `budget`.
|
||||
pub(super) fn hits(&self, s: &Session, work: &mut usize, budget: usize, all: bool) -> Result<Vec<(Member, Member, f64)>> {
|
||||
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()
|
||||
@@ -214,11 +249,19 @@ impl ContactGeometry {
|
||||
Ok(Mesh { positions, triangles: body.mesh.triangles.clone(), ..Mesh::default() })
|
||||
})
|
||||
.collect::<Result<_>>()?;
|
||||
for (a, b, aname, bname) in &self.pairs {
|
||||
// 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 {
|
||||
// Members already found colliding need no more of their bodies tested.
|
||||
if found.contains(&(oa.0.min(ob.0), oa.0.max(ob.0))) {
|
||||
continue;
|
||||
}
|
||||
let (Some(ma), Some(mb), Some(ba), Some(bb)) = (meshes.get(*a), meshes.get(*b), self.bodies.get(*a), self.bodies.get(*b)) else {
|
||||
return Err(limit("invalid contact geometry pair"));
|
||||
};
|
||||
let (aabb, bbb) = (ma.bounds(), mb.bounds());
|
||||
let (Some(aabb), Some(bbb)) = (boxes.get(*a), boxes.get(*b)) else {
|
||||
return Err(limit("invalid contact geometry pair"));
|
||||
};
|
||||
if aabb.max.x < bbb.min.x
|
||||
|| bbb.max.x < aabb.min.x
|
||||
|| aabb.max.y < bbb.min.y
|
||||
@@ -235,14 +278,18 @@ impl ContactGeometry {
|
||||
.saturating_mul(mb.triangles.len())
|
||||
.saturating_add(mb.positions.len().saturating_add(mb.triangles.len().saturating_mul(4)).saturating_mul(ma.triangles.len()));
|
||||
*work = work.saturating_add(cost);
|
||||
if *work > MAX_WORK {
|
||||
if *work > budget {
|
||||
return Err(limit("motion exceeds contact sweep collision budget; simplify the contact set or use smaller increments"));
|
||||
}
|
||||
if let Some(depth) = super::mesh_overlap(ma, mb, 1e-4 * ba.size.max(bb.size).max(1.0)) {
|
||||
return Ok(Some((aname.clone(), bname.clone(), depth)));
|
||||
found.insert((oa.0.min(ob.0), oa.0.max(ob.0)));
|
||||
out.push((oa.clone(), ob.clone(), depth));
|
||||
if !all {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
Ok(None)
|
||||
Ok(out)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -1448,3 +1448,7 @@ mod coupling;
|
||||
|
||||
#[path = "joints_cam_tests.rs"]
|
||||
mod cam;
|
||||
|
||||
// Plots and interference of motion studies, on the four-bar and slider-crank above.
|
||||
#[path = "motion_analysis_tests.rs"]
|
||||
mod motion_analysis;
|
||||
@@ -14,6 +14,8 @@ use super::joints::{cannot_move, effective_values, joint_id, occurrence_id, reso
|
||||
use crate::params::{bad, bool_, str_, vec3};
|
||||
use crate::{EngineError, Result, Session};
|
||||
|
||||
#[path = "motion_analysis.rs"]
|
||||
mod analysis;
|
||||
#[path = "motion_animation.rs"]
|
||||
mod animation;
|
||||
#[path = "contact_sweep.rs"]
|
||||
@@ -69,6 +71,8 @@ pub static COMMANDS: &[CommandSpec] = &[
|
||||
"; name? (default Motion TraceN) - keeps the trace as a 3D sketch spline through its points (a new sketch with 3D Sketch \
|
||||
on, in the root component, where no joint moves it; a fixed curve, it does not follow later changes) → sketch, name, curves, points"
|
||||
)),
|
||||
CommandSpec::new("motion.plot", "Plot Motion Study", analysis::plot).noundo().params(analysis::PLOT_DOC),
|
||||
CommandSpec::new("motion.interference", "Motion Interference", analysis::interference).noundo().params(analysis::INTERFERENCE_DOC),
|
||||
CommandSpec::new("motion.list", "List Motion Studies", list_studies).noundo(),
|
||||
CommandSpec::new("motion.delete", "Delete Motion Study", delete_study).params("study"),
|
||||
CommandSpec::new("explode.create", "Exploded View", explode_create).at("SOLID", "ASSEMBLE").icon("joint").params(
|
||||
@@ -472,12 +476,15 @@ fn check(s: &mut Session, _p: &Value) -> Result<Value> {
|
||||
// ---- Motion studies ----
|
||||
|
||||
fn study_index(s: &Session, p: &Value, cmd: &str) -> Result<usize> {
|
||||
let key = match p.get("study") {
|
||||
Some(Value::Number(n)) => n.to_string(),
|
||||
Some(Value::String(x)) => x.clone(),
|
||||
// A number is an id first, text a name first: a study may be named like another's id.
|
||||
let (key, id_first) = match p.get("study") {
|
||||
Some(Value::Number(n)) => (n.to_string(), true),
|
||||
Some(Value::String(x)) => (x.clone(), false),
|
||||
_ => return Err(bad(cmd, "`study` must be an id or a name")),
|
||||
};
|
||||
s.doc.assembly.studies.iter().position(|m| m.id.to_string() == key || m.name == key).ok_or_else(|| bad(cmd, format!("no motion study `{key}`")))
|
||||
let studies = &s.doc.assembly.studies;
|
||||
let (by_id, by_name) = (studies.iter().position(|m| m.id.to_string() == key), studies.iter().position(|m| m.name == key));
|
||||
if id_first { by_id.or(by_name) } else { by_name.or(by_id) }.ok_or_else(|| bad(cmd, format!("no motion study `{key}`")))
|
||||
}
|
||||
|
||||
fn study(s: &mut Session, p: &Value) -> Result<Value> {
|
||||
@@ -688,13 +695,11 @@ const MAX_TRACE: usize = solidcraft_sketch::MAX_FIT_POINTS;
|
||||
/// The joint values a trace drives to at sample `k` of `n`.
|
||||
type Plan = Box<dyn Fn(&Session, usize, usize) -> Vec<(u64, Vec<f64>)>>;
|
||||
|
||||
/// Where a point of an occurrence goes as a study plays or a joint is driven: its world
|
||||
/// positions, sampled on a copy (the design is not changed). Each sample is driven from the one
|
||||
/// before, so closed loops follow on their branch and contact sets stop them; the trace ends at
|
||||
/// the sample where the motion stops short. Returns the points and the answer without them.
|
||||
fn sample_trace(s: &Session, p: &Value, cmd: &str) -> Result<(Vec<Vec3>, Value)> {
|
||||
// The placement traced: an occurrence path through nested sub-assemblies (one instance of
|
||||
// each), or an occurrence at its first placement (none: the root).
|
||||
/// The point `p` names (`occurrence` and `point`): the occurrence path placing it and the point
|
||||
/// in that placement's frame. The occurrence is an occurrence path through nested sub-assemblies
|
||||
/// (one instance of each), or an occurrence at its first placement (none: the root). The point
|
||||
/// is a world point where the parts are placed (moves only shown are left out).
|
||||
fn traced_point(s: &Session, p: &Value, cmd: &str) -> Result<(Vec<u64>, Vec3)> {
|
||||
let path: Vec<u64> = match p.get("occurrence") {
|
||||
Some(Value::Array(ids)) if ids.len() <= 64 => {
|
||||
ids.iter().map(|v| v.as_u64().ok_or_else(|| bad(cmd, "an occurrence path lists occurrence ids"))).collect::<Result<_>>()?
|
||||
@@ -710,19 +715,29 @@ fn sample_trace(s: &Session, p: &Value, cmd: &str) -> Result<(Vec<Vec3>, Value)>
|
||||
};
|
||||
let at = p.get("point").and_then(vec3).filter(|v| v.len() < 1e7).ok_or_else(|| bad(cmd, "`point` must be [x, y, z], a world point"))?;
|
||||
let mut t = s.scratch();
|
||||
t.pending_moves.clear();
|
||||
let local = solidcraft_doc::mat_inverse(&path_world(&t, &path, cmd)?)
|
||||
.map(|m| solidcraft_doc::apply_point(&m, at))
|
||||
.ok_or_else(|| bad(cmd, "the occurrence's placement is not invertible"))?;
|
||||
Ok((path, local))
|
||||
}
|
||||
|
||||
/// The world placement of an occurrence path (empty: the root).
|
||||
fn path_world(t: &Session, path: &[u64], cmd: &str) -> Result<Mat> {
|
||||
if path.is_empty() { Ok(solidcraft_doc::IDENTITY) } else { t.occurrence_placement(path).map(|o| o.world).map_err(|e| bad(cmd, e.to_string())) }
|
||||
}
|
||||
|
||||
/// Where a point of an occurrence goes as a study plays or a joint is driven: its world
|
||||
/// positions, sampled on a copy (the design is not changed). Each sample is driven from the one
|
||||
/// before, so closed loops follow on their branch and contact sets stop them; the trace ends at
|
||||
/// the sample where the motion stops short. Returns the points and the answer without them.
|
||||
fn sample_trace(s: &Session, p: &Value, cmd: &str) -> Result<(Vec<Vec3>, Value)> {
|
||||
let (path, local) = traced_point(s, p, cmd)?;
|
||||
let mut t = s.scratch();
|
||||
// Traced from where the parts are placed: moves only shown (an exploded view, a move not
|
||||
// captured yet) are not where the joints are solved.
|
||||
t.pending_moves.clear();
|
||||
let world = |t: &Session| -> Result<Mat> {
|
||||
if path.is_empty() {
|
||||
Ok(solidcraft_doc::IDENTITY)
|
||||
} else {
|
||||
t.occurrence_placement(&path).map(|o| o.world).map_err(|e| bad(cmd, e.to_string()))
|
||||
}
|
||||
};
|
||||
let local = solidcraft_doc::mat_inverse(&world(&t)?)
|
||||
.map(|m| solidcraft_doc::apply_point(&m, at))
|
||||
.ok_or_else(|| bad(cmd, "the occurrence's placement is not invertible"))?;
|
||||
let world = |t: &Session| path_world(t, &path, cmd);
|
||||
let (default, given, plan) = motion_plan(s, p, cmd)?;
|
||||
let n = sample_count(p, default.min(MAX_TRACE), MAX_TRACE, cmd)?;
|
||||
let mut points = Vec::new();
|
||||
|
||||
@@ -0,0 +1,456 @@
|
||||
//! Motion analysis: a study played sample by sample on a copy of the design, the way
|
||||
//! `motion.play` places it (closed loops following on their branch). `motion.plot` answers the
|
||||
//! joint values and a point's path with their velocities and accelerations; `motion.interference`
|
||||
//! answers the pairs of parts that pass through each other along the way.
|
||||
|
||||
use std::collections::BTreeMap;
|
||||
use std::sync::Arc;
|
||||
|
||||
use serde_json::{Value, json};
|
||||
use solidcraft_doc::joints;
|
||||
use solidcraft_doc::motion::MotionStudy;
|
||||
use solidcraft_doc::{Document, Mat, apply_point, mat_mul};
|
||||
use solidcraft_geom::{Aabb3, Vec3};
|
||||
|
||||
use super::contact_sweep::{ContactGeometry, Member};
|
||||
use super::{drive_with_contact, joint_id, keyed, occurrences, path_world, study_index, traced_point, values_at};
|
||||
use crate::params::{bad, bool_};
|
||||
use crate::{EngineError, Result, Session};
|
||||
|
||||
pub const PLOT_DOC: &str = "study: id|name; joints?: [id|name] (default: every joint that moves); occurrence? and point? (a point \
|
||||
of a part to follow, as motion.trace takes them); samples? (3…1000, default the study's steps + 1); duration?: seconds the \
|
||||
whole study takes, 0.001 or more (velocities and accelerations then per second, else per step) → step: [the study step of each sample], \
|
||||
time?: [s], joints: [{joint, name, index, dof, unit, value, velocity, acceleration, velocity_unit, acceleration_unit}] \
|
||||
(degrees for angles, mm for distances; angles unwrapped), point?: {occurrence, position: [[x,y,z]…], velocity, \
|
||||
acceleration (vectors), speed, acceleration_norm, unit, velocity_unit, acceleration_unit}, samples (taken), stopped_at, \
|
||||
stopped_by_loop, stopped_by_contact. The study plays as motion.play plays it, each sample driven from the one before, on \
|
||||
a copy (the design is not changed); velocities and accelerations are finite differences between samples (central, \
|
||||
one-sided at the ends)";
|
||||
|
||||
pub const INTERFERENCE_DOC: &str = "study: id|name; occurrences?: [ids or names] (two or more, checked against each other; \
|
||||
default every occurrence), with against?: [ids or names] (then each of `occurrences` is checked only against these, such \
|
||||
as an arm against the parts it must miss), or contact_sets?: true (the pairs of the design's contact sets); samples? \
|
||||
(2…1000, default the study's steps + 1) → collisions: [{a, b, first_step, last_step, steps: [[from, to]…] (the step ranges where they overlap), \
|
||||
depth_mm (how far into each other, the most at deepest_step), volume_mm3 (their overlap there; null when it was not \
|
||||
cheap to work out)}], clear, \
|
||||
pairs (checked), samples (taken), truncated (the work budget ran out; checked_to: the last step checked), stopped_at, \
|
||||
stopped_by_loop. The study plays as motion.play plays it but contact sets do not stop it, on a copy (the design is not \
|
||||
changed). Parts are tested at the samples only: one that passes wholly through another between two samples is missed";
|
||||
|
||||
/// Most samples a plot or an interference check takes: each is a solve.
|
||||
const MAX_SAMPLES: usize = 1000;
|
||||
/// Most joint values one plot answers.
|
||||
const MAX_SERIES: usize = 256;
|
||||
/// Most collision work over a whole study (see `ContactGeometry::hits`).
|
||||
const MAX_WORK: usize = 500_000_000;
|
||||
/// Most colliding pairs whose overlap volume is worked out, and most body booleans that takes.
|
||||
const MAX_VOLUMES: usize = 16;
|
||||
const MAX_BOOLEANS: usize = 64;
|
||||
|
||||
/// How many samples `p` asks for (`min`…`MAX_SAMPLES`, default the study's steps + 1).
|
||||
fn samples(p: &Value, m: &MotionStudy, min: usize, cmd: &str) -> Result<usize> {
|
||||
if m.steps == 0 {
|
||||
return Err(bad(cmd, "the study has no steps"));
|
||||
}
|
||||
match p.get("samples") {
|
||||
Some(v) => v
|
||||
.as_f64()
|
||||
.filter(|n| n.is_finite() && *n >= min as f64 && *n <= MAX_SAMPLES as f64)
|
||||
.map(|n| n.round() as usize)
|
||||
.ok_or_else(|| bad(cmd, format!("`samples` must be {min}…{MAX_SAMPLES}"))),
|
||||
None => Ok((m.steps as usize).saturating_add(1).clamp(min, MAX_SAMPLES)),
|
||||
}
|
||||
}
|
||||
|
||||
/// The study step sample `k` of `n` is at.
|
||||
fn step_of(m: &MotionStudy, k: usize, n: usize) -> f64 {
|
||||
f64::from(m.steps) * k as f64 / n.saturating_sub(1).max(1) as f64
|
||||
}
|
||||
|
||||
/// Plays study `m` on a copy of the design in `n` samples, each driven from the one before (so
|
||||
/// closed loops follow on their branch, as `motion.play` places them; contact sets stop it only
|
||||
/// when `contact`). `each` sees the copy at every sample and answers whether to go on. Returns
|
||||
/// where the motion stopped short (all null when it did not).
|
||||
fn play(s: &Session, m: &MotionStudy, n: usize, contact: bool, mut each: impl FnMut(&Session, usize) -> Result<bool>) -> Result<Value> {
|
||||
let mut t = s.scratch();
|
||||
// Played from where the parts are placed: moves only shown are not where joints are solved.
|
||||
t.pending_moves.clear();
|
||||
if !contact {
|
||||
t.doc_mut().assembly.contact_enabled = false;
|
||||
}
|
||||
let given = keyed(m);
|
||||
for k in 0..n {
|
||||
let want = values_at(&t, m, step_of(m, k, n));
|
||||
let (r, _) = drive_with_contact(&mut t, &want, &given)?;
|
||||
let go = each(&t, k)?;
|
||||
if r["stopped_by_loop"].is_object() || r["stopped_by_contact"].is_object() {
|
||||
return Ok(json!({"stopped_at": k, "stopped_by_loop": r["stopped_by_loop"], "stopped_by_contact": r["stopped_by_contact"]}));
|
||||
}
|
||||
if !go {
|
||||
break;
|
||||
}
|
||||
}
|
||||
Ok(json!({"stopped_at": null, "stopped_by_loop": null, "stopped_by_contact": null}))
|
||||
}
|
||||
|
||||
/// The derivative of samples `v` spaced `h` apart: central differences inside, second-order
|
||||
/// one-sided at the ends (a plain difference with two samples, 0 with one).
|
||||
fn derivative(v: &[f64], h: f64) -> Vec<f64> {
|
||||
let n = v.len();
|
||||
let at = |i: usize| v.get(i).copied().unwrap_or(0.0);
|
||||
(0..n)
|
||||
.map(|k| match n {
|
||||
0 | 1 => 0.0,
|
||||
2 => (at(1) - at(0)) / h,
|
||||
_ if k == 0 => (-3.0 * at(0) + 4.0 * at(1) - at(2)) / (2.0 * h),
|
||||
_ if k + 1 == n => (3.0 * at(k) - 4.0 * at(k - 1) + at(k - 2)) / (2.0 * h),
|
||||
_ => (at(k + 1) - at(k - 1)) / (2.0 * h),
|
||||
})
|
||||
.collect()
|
||||
}
|
||||
|
||||
/// The second derivative of samples `v` spaced `h` apart: central differences inside,
|
||||
/// second-order one-sided at the ends (the one difference there is with three samples, 0 with
|
||||
/// fewer).
|
||||
fn second_derivative(v: &[f64], h: f64) -> Vec<f64> {
|
||||
let n = v.len();
|
||||
let at = |i: usize| v.get(i).copied().unwrap_or(0.0);
|
||||
let h2 = h * h;
|
||||
(0..n)
|
||||
.map(|k| match n {
|
||||
0..=2 => 0.0,
|
||||
3 => (at(0) - 2.0 * at(1) + at(2)) / h2,
|
||||
_ if k == 0 => (2.0 * at(0) - 5.0 * at(1) + 4.0 * at(2) - at(3)) / h2,
|
||||
_ if k + 1 == n => (2.0 * at(k) - 5.0 * at(k - 1) + 4.0 * at(k - 2) - at(k - 3)) / h2,
|
||||
_ => (at(k + 1) - 2.0 * at(k) + at(k - 1)) / h2,
|
||||
})
|
||||
.collect()
|
||||
}
|
||||
|
||||
/// Angles in degrees made continuous: a jump of more than half a turn between samples is taken
|
||||
/// as the angle going round (half a turn exactly is a real step).
|
||||
fn unwrap_degrees(v: &mut [f64]) {
|
||||
let (mut turns, mut prev) = (0.0, None::<f64>);
|
||||
for x in v.iter_mut() {
|
||||
let raw = *x;
|
||||
if let Some(p) = prev
|
||||
&& (raw - p).abs() > 180.0
|
||||
{
|
||||
turns -= 360.0 * ((raw - p) / 360.0).round();
|
||||
}
|
||||
prev = Some(raw);
|
||||
*x = raw + turns;
|
||||
}
|
||||
}
|
||||
|
||||
/// The joint values a plot follows: (joint id, value index).
|
||||
fn plotted(s: &Session, p: &Value, cmd: &str) -> Result<Vec<(u64, usize)>> {
|
||||
let joints = &s.doc.assembly.joints;
|
||||
let ids: Vec<u64> = match p.get("joints") {
|
||||
None | Some(Value::Null) => joints.iter().filter(|j| !j.suppressed).map(|j| j.id).collect(),
|
||||
Some(Value::Array(list)) if list.len() <= MAX_SERIES => list.iter().map(|v| joint_id(s, Some(v), cmd)).collect::<Result<_>>()?,
|
||||
Some(_) => return Err(bad(cmd, format!("`joints` must list at most {MAX_SERIES} joints"))),
|
||||
};
|
||||
let mut out = Vec::new();
|
||||
for id in ids {
|
||||
let dofs = joints.iter().find(|j| j.id == id).map_or(0, |j| j.kind.dofs().len());
|
||||
for i in 0..dofs {
|
||||
if !out.contains(&(id, i)) {
|
||||
out.push((id, i));
|
||||
}
|
||||
}
|
||||
}
|
||||
if out.len() > MAX_SERIES {
|
||||
return Err(bad(cmd, format!("more than {MAX_SERIES} joint values: list the `joints` to plot")));
|
||||
}
|
||||
Ok(out)
|
||||
}
|
||||
|
||||
pub(super) fn plot(s: &mut Session, p: &Value) -> Result<Value> {
|
||||
let cmd = "motion.plot";
|
||||
let m = s.doc.assembly.studies.get(study_index(s, p, cmd)?).cloned().ok_or_else(|| bad(cmd, "study"))?;
|
||||
let n = samples(p, &m, 3, cmd)?;
|
||||
let duration = match p.get("duration") {
|
||||
None | Some(Value::Null) => None,
|
||||
Some(v) => {
|
||||
// At least a millisecond: shorter sample spacings square to nothing.
|
||||
Some(v.as_f64().filter(|d| (1e-3..=1e9).contains(d)).ok_or_else(|| bad(cmd, "`duration` must be seconds, 0.001 or more"))?)
|
||||
}
|
||||
};
|
||||
let series = plotted(s, p, cmd)?;
|
||||
let given = keyed(&m);
|
||||
let point = if p.get("point").is_some() || p.get("occurrence").is_some() { Some(traced_point(s, p, cmd)?) } else { None };
|
||||
let mut values: Vec<Vec<f64>> = vec![Vec::new(); series.len()];
|
||||
let mut path: Vec<Vec3> = Vec::new();
|
||||
let mut taken = 0;
|
||||
let stop = play(s, &m, n, true, |t, _| {
|
||||
taken += 1;
|
||||
let now = joints::joint_values(&t.doc.assembly);
|
||||
for ((id, i), out) in series.iter().zip(values.iter_mut()) {
|
||||
out.push(now.get(id).and_then(|q| q.get(*i)).copied().unwrap_or(0.0));
|
||||
}
|
||||
if let Some((occ, local)) = &point {
|
||||
path.push(apply_point(&path_world(t, occ, cmd)?, *local));
|
||||
}
|
||||
Ok(true)
|
||||
})?;
|
||||
// Sample spacing in steps, or in seconds when the study has a duration.
|
||||
let seconds_per_step = duration.map(|d| d / f64::from(m.steps));
|
||||
let h = f64::from(m.steps) / (n - 1) as f64 * seconds_per_step.unwrap_or(1.0);
|
||||
let per = if duration.is_some() { "s" } else { "step" };
|
||||
let steps: Vec<f64> = (0..taken).map(|k| step_of(&m, k, n)).collect();
|
||||
let mut out = json!({"study": m.name, "steps": m.steps, "samples": taken, "step": steps});
|
||||
if let Some(sps) = seconds_per_step {
|
||||
out["time"] = steps.iter().map(|st| st * sps).collect();
|
||||
}
|
||||
let joints_out: Vec<Value> = series
|
||||
.iter()
|
||||
.zip(values.iter_mut())
|
||||
.map(|((id, i), v)| {
|
||||
let j = s.doc.assembly.joints.iter().find(|j| j.id == *id);
|
||||
let angle = j.is_some_and(|j| j.kind.is_angle(*i));
|
||||
let unit = if angle { "deg" } else { "mm" };
|
||||
if angle {
|
||||
v.iter_mut().for_each(|x| *x = x.to_degrees());
|
||||
// Keyed values are as the study sets them, and values beyond half a turn
|
||||
// (through motion links) already run on; a loop can hand back a follower's
|
||||
// angle within half a turn, a turn round.
|
||||
if !given.contains(&(*id, *i)) && v.iter().all(|x| x.abs() <= 180.0 + 1e-9) {
|
||||
unwrap_degrees(v);
|
||||
}
|
||||
}
|
||||
json!({"joint": id, "name": j.map(|j| j.name.clone()), "index": i, "dof": j.and_then(|j| j.kind.dofs().get(*i).copied()),
|
||||
"unit": unit, "velocity_unit": format!("{unit}/{per}"), "acceleration_unit": format!("{unit}/{per}²"),
|
||||
"value": v, "velocity": derivative(v, h), "acceleration": second_derivative(v, h)})
|
||||
})
|
||||
.collect();
|
||||
out["joints"] = json!(joints_out);
|
||||
if let Some((occ, _)) = &point {
|
||||
let axis = |c: usize| -> Vec<f64> { path.iter().map(|q| [q.x, q.y, q.z].get(c).copied().unwrap_or(0.0)).collect() };
|
||||
let d: Vec<(Vec<f64>, Vec<f64>)> = (0..3).map(|c| (derivative(&axis(c), h), second_derivative(&axis(c), h))).collect();
|
||||
let vec_at = |k: usize, second: bool| -> Vec3 {
|
||||
let c = |i: usize| d.get(i).and_then(|(v, a)| if second { a.get(k) } else { v.get(k) }).copied().unwrap_or(0.0);
|
||||
Vec3::new(c(0), c(1), c(2))
|
||||
};
|
||||
let (vel, acc): (Vec<Vec3>, Vec<Vec3>) = (0..path.len()).map(|k| (vec_at(k, false), vec_at(k, true))).unzip();
|
||||
let xyz = |list: &[Vec3]| -> Value { list.iter().map(|v| json!([v.x, v.y, v.z])).collect() };
|
||||
let name = s.doc.occurrences.iter().find(|o| Some(&o.id) == occ.last()).map_or_else(|| "root".to_string(), |o| o.name.clone());
|
||||
out["point"] = json!({"occurrence": name, "unit": "mm", "velocity_unit": format!("mm/{per}"), "acceleration_unit": format!("mm/{per}²"),
|
||||
"position": xyz(&path), "velocity": xyz(&vel), "acceleration": xyz(&acc),
|
||||
"speed": vel.iter().map(|v| v.len()).collect::<Vec<_>>(), "acceleration_norm": acc.iter().map(|v| v.len()).collect::<Vec<_>>()});
|
||||
}
|
||||
for k in ["stopped_at", "stopped_by_loop", "stopped_by_contact"] {
|
||||
out[k] = stop[k].clone();
|
||||
}
|
||||
Ok(out)
|
||||
}
|
||||
|
||||
/// One pair of parts that collides along a study.
|
||||
struct Collision {
|
||||
names: (String, String),
|
||||
/// The samples where they overlap, in order.
|
||||
samples: Vec<usize>,
|
||||
depth: f64,
|
||||
deepest: usize,
|
||||
/// The design at the deepest sample (its placements).
|
||||
at_deepest: Arc<Document>,
|
||||
}
|
||||
|
||||
/// The volume the parts under occurrences `a` and `b` share, placed as in `doc` (`None`: not
|
||||
/// worked out). Only bodies whose boxes overlap are intersected, at most `booleans` of them.
|
||||
fn shared_volume(s: &Session, doc: &Document, a: u64, b: u64, booleans: &mut usize) -> Option<f64> {
|
||||
let st = s.model.state();
|
||||
// The bodies under each (and not under the other), with their world frames and boxes.
|
||||
let mut sides: [Vec<(Mat, Aabb3, &solidcraft_doc::ModelBody)>; 2] = Default::default();
|
||||
for (comp, path, m) in doc.placements() {
|
||||
// A body belongs to the innermost of the two on its path, as contact sets pair them.
|
||||
let side = match path.iter().rev().find(|o| **o == a || **o == b) {
|
||||
Some(o) if *o == a => 0,
|
||||
Some(_) => 1,
|
||||
None => continue,
|
||||
};
|
||||
for body in st.bodies.iter().filter(|x| doc.model_body_component(x) == comp && !x.body.is_mesh()) {
|
||||
let frame = doc.model_body_offset(body).map(|o| mat_mul(&m, o)).unwrap_or(m);
|
||||
let local = body.mesh().bounds();
|
||||
let mut world = Aabb3::EMPTY;
|
||||
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));
|
||||
world.add(apply_point(&frame, corner));
|
||||
}
|
||||
sides.get_mut(side)?.push((frame, world, body));
|
||||
}
|
||||
}
|
||||
let mut total = 0.0;
|
||||
for (fa, xa, ba) in sides.first()? {
|
||||
for (fb, xb, bb) in sides.get(1)? {
|
||||
if xa.max.x < xb.min.x || xb.max.x < xa.min.x || xa.max.y < xb.min.y || xb.max.y < xa.min.y || xa.max.z < xb.min.z || xb.max.z < xa.min.z
|
||||
{
|
||||
continue;
|
||||
}
|
||||
*booleans = booleans.checked_sub(1)?;
|
||||
let (pa, pb) = (solidcraft_kernel::transform_matrix(&ba.body, *fa).ok()?, solidcraft_kernel::transform_matrix(&bb.body, *fb).ok()?);
|
||||
if let Some(shared) = solidcraft_kernel::boolean(&pa, &pb, solidcraft_kernel::BoolOp::Intersect).ok()? {
|
||||
total += solidcraft_kernel::measure(&shared).ok()?.volume;
|
||||
}
|
||||
}
|
||||
}
|
||||
Some(total)
|
||||
}
|
||||
|
||||
/// Contiguous runs of samples, as study step ranges.
|
||||
fn ranges(m: &MotionStudy, samples: &[usize], n: usize) -> Vec<[f64; 2]> {
|
||||
let mut out: Vec<(usize, usize)> = Vec::new();
|
||||
for &k in samples {
|
||||
match out.last_mut() {
|
||||
Some(run) if run.1 + 1 == k => run.1 = k,
|
||||
_ => out.push((k, k)),
|
||||
}
|
||||
}
|
||||
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)?;
|
||||
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") {
|
||||
None | Some(Value::Null) => None,
|
||||
Some(_) if p.get("occurrences").is_none() => return Err(bad(cmd, "`against` checks the `occurrences` against other parts: list them too")),
|
||||
Some(v) => Some(occurrences(s, &json!({"occurrences": v}), cmd)?),
|
||||
};
|
||||
let sets: Vec<Vec<u64>> = match (p.get("occurrences"), bool_(p, "contact_sets").unwrap_or(false)) {
|
||||
(Some(_), true) => return Err(bad(cmd, "give `occurrences` or `contact_sets`, not both")),
|
||||
(Some(_), false) => {
|
||||
let mut list = occurrences(s, p, cmd)?;
|
||||
if list.len() + against.as_ref().map_or(0, Vec::len) < 2 {
|
||||
return Err(bad(cmd, "list two or more occurrences to check against each other"));
|
||||
}
|
||||
for o in against.iter().flatten() {
|
||||
if !list.contains(o) {
|
||||
list.push(*o);
|
||||
}
|
||||
}
|
||||
vec![list]
|
||||
}
|
||||
(None, true) => {
|
||||
let a = &s.doc.assembly;
|
||||
let sets: Vec<Vec<u64>> =
|
||||
if a.contact_all { everything() } else { a.contact_sets.iter().filter(|c| !c.suppressed).map(|c| c.occurrences.clone()).collect() };
|
||||
if sets.is_empty() {
|
||||
return Err(bad(cmd, "the design has no contact sets"));
|
||||
}
|
||||
sets
|
||||
}
|
||||
(None, false) => everything(),
|
||||
};
|
||||
let mut geometry = ContactGeometry::of_sets(s, &sets).map_err(|e| match e {
|
||||
EngineError::BadParams { msg, .. } => bad(cmd, format!("{msg} (check fewer parts: list the `occurrences`)")),
|
||||
e => e,
|
||||
})?;
|
||||
if let Some(b) = &against {
|
||||
let a = occurrences(s, p, cmd)?;
|
||||
geometry.keep_pairs(|x, y| (a.contains(&x) && b.contains(&y)) || (a.contains(&y) && b.contains(&x)));
|
||||
}
|
||||
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| {
|
||||
// Placing the meshes is work too, also where nothing comes near.
|
||||
work = work.saturating_add(geometry.vertex_count());
|
||||
if work > MAX_WORK {
|
||||
truncated = true;
|
||||
return Ok(false);
|
||||
}
|
||||
let hits: Vec<(Member, Member, f64)> = match geometry.hits(t, &mut work, MAX_WORK, true) {
|
||||
Ok(h) => h,
|
||||
Err(_) if work > MAX_WORK => {
|
||||
truncated = true;
|
||||
return Ok(false);
|
||||
}
|
||||
Err(e) => return Err(e),
|
||||
};
|
||||
checked = k + 1;
|
||||
for ((a, an), (b, bn), depth) in hits {
|
||||
// Each pair once, the older occurrence (the lower id) first.
|
||||
let ((a, an), (b, bn)) = if a <= b { ((a, an), (b, bn)) } else { ((b, bn), (a, an)) };
|
||||
let c = found.entry((a, b)).or_insert_with(|| Collision {
|
||||
names: (an, bn),
|
||||
samples: Vec::new(),
|
||||
depth: 0.0,
|
||||
deepest: k,
|
||||
at_deepest: t.doc.clone(),
|
||||
});
|
||||
if c.samples.last() != Some(&k) {
|
||||
c.samples.push(k);
|
||||
}
|
||||
if depth > c.depth {
|
||||
(c.depth, c.deepest, c.at_deepest) = (depth, k, t.doc.clone());
|
||||
}
|
||||
}
|
||||
Ok(true)
|
||||
})?;
|
||||
// Nothing checked is not clear.
|
||||
if truncated && checked == 0 {
|
||||
return Err(bad(cmd, "the check ran out of its work budget before its first step: check fewer parts (occurrences, against)"));
|
||||
}
|
||||
let mut list: Vec<(&(u64, u64), &Collision)> = found.iter().collect();
|
||||
list.sort_by_key(|(_, c)| c.samples.first().copied().unwrap_or(usize::MAX));
|
||||
let mut booleans = MAX_BOOLEANS;
|
||||
let collisions: Vec<Value> = list
|
||||
.iter()
|
||||
.enumerate()
|
||||
.map(|(i, ((a, b), c))| {
|
||||
let volume = if i < MAX_VOLUMES { shared_volume(s, &c.at_deepest, *a, *b, &mut booleans) } else { None };
|
||||
let first = c.samples.first().map(|k| step_of(&m, *k, n));
|
||||
let last = c.samples.last().map(|k| step_of(&m, *k, n));
|
||||
json!({"a": c.names.0, "b": c.names.1, "first_step": first, "last_step": last, "steps": ranges(&m, &c.samples, n),
|
||||
"depth_mm": c.depth, "deepest_step": step_of(&m, c.deepest, n), "volume_mm3": volume})
|
||||
})
|
||||
.collect();
|
||||
let mut out = json!({"study": m.name, "steps": m.steps, "clear": collisions.is_empty(), "collisions": collisions,
|
||||
"pairs": geometry.pair_count(), "samples": checked, "truncated": truncated});
|
||||
if truncated {
|
||||
out["checked_to"] = json!(checked.checked_sub(1).map(|k| step_of(&m, k, n)));
|
||||
}
|
||||
out["stopped_at"] = stop["stopped_at"].clone();
|
||||
out["stopped_by_loop"] = stop["stopped_by_loop"].clone();
|
||||
Ok(out)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
/// The differences are exact for a quadratic: its velocity and acceleration everywhere,
|
||||
/// the ends too.
|
||||
#[test]
|
||||
fn finite_differences_follow_a_polynomial() {
|
||||
let h = 0.5;
|
||||
let f = |x: f64| 2.0 * x * x - 3.0 * x + 1.0;
|
||||
let v: Vec<f64> = (0..7).map(|k| f(k as f64 * h)).collect();
|
||||
let d = derivative(&v, h);
|
||||
let a = second_derivative(&v, h);
|
||||
for (k, (dk, ak)) in d.iter().zip(&a).enumerate() {
|
||||
let x = k as f64 * h;
|
||||
assert!((dk - (4.0 * x - 3.0)).abs() < 1e-9, "{k}: {dk}");
|
||||
assert!((ak - 4.0).abs() < 1e-9, "{k}: {ak}");
|
||||
}
|
||||
assert_eq!(derivative(&[1.0, 3.0], 2.0), vec![1.0, 1.0]);
|
||||
assert_eq!(second_derivative(&[1.0, 3.0], 2.0), vec![0.0, 0.0]);
|
||||
assert_eq!(derivative(&[5.0], 1.0), vec![0.0]);
|
||||
assert!(derivative(&[], 1.0).is_empty());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn angles_unwrap_across_half_turns() {
|
||||
let mut v = vec![170.0, 179.0, -172.0, -160.0, 175.0];
|
||||
unwrap_degrees(&mut v);
|
||||
assert_eq!(v, vec![170.0, 179.0, 188.0, 200.0, 175.0]);
|
||||
// Half a turn a sample is a step, not a wrap.
|
||||
let mut v = vec![0.0, 180.0, 360.0];
|
||||
unwrap_degrees(&mut v);
|
||||
assert_eq!(v, vec![0.0, 180.0, 360.0]);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,211 @@
|
||||
//! `motion.plot` against the analytic motion of a four-bar and a slider-crank, and
|
||||
//! `motion.interference` on two blocks pushed through each other at a known step.
|
||||
|
||||
use serde_json::{Value, json};
|
||||
use solidcraft_geom::Vec3;
|
||||
|
||||
use super::{four_bar, occ, part, pin, rocker_pin, run, slider_crank};
|
||||
use crate::Session;
|
||||
|
||||
/// A joint's series in a plot, by joint name.
|
||||
fn series<'a>(r: &'a Value, name: &str) -> &'a Value {
|
||||
r["joints"].as_array().and_then(|list| list.iter().find(|j| j["name"] == name)).unwrap_or(&Value::Null)
|
||||
}
|
||||
|
||||
fn nums(v: &Value) -> Vec<f64> {
|
||||
v.as_array().into_iter().flatten().map(|x| x.as_f64().unwrap_or(f64::NAN)).collect()
|
||||
}
|
||||
|
||||
/// The crank turns at a constant 50°/s (a full turn in 7.2 s). The rocker turns at
|
||||
/// ω4 = ω2·a·sin(θ2 − θ3) / (c·sin(θ4 − θ3)) (crank a = 20, rocker c = 40; θ2, θ3 and θ4 the
|
||||
/// crank, coupler and rocker angles), from differentiating the loop a·e^iθ2 + b·e^iθ3 = d + c·e^iθ4.
|
||||
#[test]
|
||||
fn four_bar_rocker_velocity_matches_its_analytic_derivative() {
|
||||
let (mut s, _, q0) = four_bar();
|
||||
run(&mut s, "motion.study", json!({"name": "Turn", "steps": 360, "keys": [{"joint": "Crank", "points": [[0, 0], [360, 360]]}]}));
|
||||
let before = s.doc.clone();
|
||||
let r = run(&mut s, "motion.plot", json!({"study": "Turn", "joints": ["Crank", "Rocker"], "samples": 181, "duration": 7.2}));
|
||||
assert_eq!(*s.doc, *before, "a plot leaves the design as it was");
|
||||
assert!(r["stopped_at"].is_null() && r["samples"] == 181, "{}", r["stopped_by_loop"]);
|
||||
assert_eq!(r["joints"].as_array().map(Vec::len), Some(2));
|
||||
let (crank, rocker) = (series(&r, "Crank"), series(&r, "Rocker"));
|
||||
assert_eq!((rocker["unit"].as_str(), rocker["velocity_unit"].as_str()), (Some("deg"), Some("deg/s")));
|
||||
let time = nums(&r["time"]);
|
||||
assert!((time.last().copied().unwrap_or(0.0) - 7.2).abs() < 1e-9 && (time[1] - 0.04).abs() < 1e-12, "{time:?}");
|
||||
// The crank: exactly as keyed, at a constant speed.
|
||||
for ((v, w), a) in nums(&crank["value"]).iter().zip(nums(&crank["velocity"])).zip(nums(&crank["acceleration"])) {
|
||||
assert!((w - 50.0).abs() < 1e-6 && a.abs() < 1e-6, "{v}°: {w} deg/s, {a} deg/s²");
|
||||
}
|
||||
let (value, velocity) = (nums(&rocker["value"]), nums(&rocker["velocity"]));
|
||||
let start = q0.y.atan2(q0.x - 45.0);
|
||||
let (mut q, mut peak, mut worst) = (q0, 0.0f64, 0.0f64);
|
||||
for k in 0..181 {
|
||||
let v = 2.0 * k as f64;
|
||||
if k > 0 {
|
||||
q = rocker_pin(v - 2.0, v, q);
|
||||
}
|
||||
let th2 = (90.0 + v).to_radians();
|
||||
let p = Vec3::new(20.0 * th2.cos(), 20.0 * th2.sin(), 0.0);
|
||||
let (th3, th4) = ((q - p).y.atan2((q - p).x), q.y.atan2(q.x - 45.0));
|
||||
// The rocker's value is its turn from where it was assembled, made continuous.
|
||||
let turned = (th4 - start).to_degrees();
|
||||
assert!(((value[k] - turned + 540.0).rem_euclid(360.0) - 180.0).abs() < 1e-6, "{v}°: {} wants {turned}", value[k]);
|
||||
let w4 = 50.0 * 20.0 * (th2 - th3).sin() / (40.0 * (th4 - th3).sin());
|
||||
peak = peak.max(w4.abs());
|
||||
worst = worst.max((velocity[k] - w4).abs());
|
||||
}
|
||||
// Central differences 0.04 s apart: within 0.2 % of the fastest swing.
|
||||
assert!(peak > 40.0 && worst < 2e-3 * peak, "worst {worst} deg/s of {peak} deg/s");
|
||||
}
|
||||
|
||||
/// The piston of a slider-crank (crank a = 15, rod b = 40) turning at 100°/s sits at
|
||||
/// x = a·cos θ + S with S = √(b² − a²·sin²θ), so it accelerates at
|
||||
/// ω²·(−a·cos θ − a²·cos 2θ / S − a⁴·sin²θ·cos²θ / S³).
|
||||
#[test]
|
||||
fn slider_crank_piston_acceleration_matches_its_analytic_form() {
|
||||
let (mut s, _, s0) = slider_crank();
|
||||
run(&mut s, "motion.study", json!({"name": "Turn", "steps": 360, "keys": [{"joint": "Crank", "points": [[0, 0], [360, 360]]}]}));
|
||||
let p = json!({"study": "Turn", "joints": ["Bore"], "occurrence": "Piston", "point": [s0.x, s0.y, s0.z], "samples": 181, "duration": 3.6});
|
||||
let r = run(&mut s, "motion.plot", p);
|
||||
assert!(r["stopped_at"].is_null() && r["samples"] == 181, "{}", r["stopped_by_loop"]);
|
||||
let point = &r["point"];
|
||||
assert_eq!((point["occurrence"].as_str(), point["acceleration_unit"].as_str()), (Some("Piston:1"), Some("mm/s²")));
|
||||
let at = |key: &str, k: usize, c: usize| point[key][k][c].as_f64().unwrap_or(f64::NAN);
|
||||
let (a, b, w) = (15.0f64, 40.0f64, 100.0f64.to_radians());
|
||||
let (mut peak, mut worst) = (0.0f64, 0.0f64);
|
||||
for k in 0..181 {
|
||||
let th = (90.0 + 2.0 * k as f64).to_radians();
|
||||
let root = (b * b - (a * th.sin()).powi(2)).sqrt();
|
||||
assert!((at("position", k, 0) - (a * th.cos() + root)).abs() < 1e-6, "{k}");
|
||||
let want = w * w * (-a * th.cos() - a * a * (2.0 * th).cos() / root - a.powi(4) * (th.sin() * th.cos()).powi(2) / root.powi(3));
|
||||
peak = peak.max(want.abs());
|
||||
// The ends take one-sided differences; inside, central ones.
|
||||
let tolerance = if k == 0 || k == 180 { 2e-2 } else { 2e-3 };
|
||||
worst = worst.max((at("acceleration", k, 0) - want).abs() / tolerance);
|
||||
assert!(at("acceleration", k, 1).abs() < 1e-6 && at("acceleration", k, 2).abs() < 1e-6, "it stays on its axis");
|
||||
}
|
||||
assert!(peak > 40.0 && worst < peak, "worst {worst} of {peak} mm/s² (scaled by tolerance)");
|
||||
// The bore's value is the piston's travel, so its acceleration is the piston's along x.
|
||||
let bore = series(&r, "Bore");
|
||||
assert_eq!(bore["acceleration_unit"], "mm/s²");
|
||||
let along = |k: usize| nums(&bore["value"])[k] - nums(&bore["value"])[0];
|
||||
let sign = if along(90) * (at("position", 90, 0) - at("position", 0, 0)) < 0.0 { -1.0 } else { 1.0 };
|
||||
for (k, acc) in nums(&bore["acceleration"]).iter().enumerate() {
|
||||
assert!((acc - sign * at("acceleration", k, 0)).abs() < 1e-6, "{k}: {acc}");
|
||||
}
|
||||
// Per step without a duration: 2 steps between samples, 1° a step.
|
||||
let r = run(&mut s, "motion.plot", json!({"study": "Turn", "joints": ["Crank"], "samples": 181}));
|
||||
let crank = series(&r, "Crank");
|
||||
assert!(r["time"].is_null() && crank["velocity_unit"] == "deg/step", "{crank}");
|
||||
assert!(nums(&crank["velocity"]).iter().all(|v| (v - 1.0).abs() < 1e-9));
|
||||
}
|
||||
|
||||
/// A dial linked 2:1 to the crank turns on past a turn: its values are not taken a turn round,
|
||||
/// even half a turn a sample.
|
||||
#[test]
|
||||
fn a_linked_value_past_half_a_turn_runs_on() {
|
||||
let (mut s, _, _) = four_bar();
|
||||
part(&mut s, "Dial", [80.0, 0.0, -5.0]);
|
||||
pin(&mut s, "revolute", "Dial", "Ground", "Dial", Vec3::new(80.0, 0.0, 0.0), Vec3::Z);
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Crank", "b": "Dial", "ratio": 2}));
|
||||
run(&mut s, "motion.study", json!({"name": "Turn", "steps": 2, "keys": [{"joint": "Crank", "points": [[0, 0], [2, 360]]}]}));
|
||||
let r = run(&mut s, "motion.plot", json!({"study": "Turn", "joints": ["Dial"], "samples": 3}));
|
||||
let v: Vec<f64> = nums(&series(&r, "Dial")["value"]).iter().map(|x| x.abs()).collect();
|
||||
assert!((v[1] - 360.0).abs() < 1e-6 && (v[2] - 720.0).abs() < 1e-6, "{r}");
|
||||
}
|
||||
|
||||
/// Cubes A (grounded, x 0…10) and B (x 20…30) on a slider along x, B 10 mm away at value 10.
|
||||
fn rail() -> Session {
|
||||
let mut s = Session::default();
|
||||
for (name, x) in [("A", 0), ("B", 20)] {
|
||||
run(&mut s, "component.activate", json!({"component": "root"}));
|
||||
run(&mut s, "component.create", json!({"name": name}));
|
||||
run(&mut s, "solid.box", json!({"length": 10, "width": 10, "height": 10, "corner": [x, 0, 0]}));
|
||||
}
|
||||
run(&mut s, "component.activate", json!({"component": "root"}));
|
||||
let a = occ(&s, "A");
|
||||
run(&mut s, "occurrence.ground", json!({"occurrence": a, "grounded": true}));
|
||||
run(&mut s, "joint.create", json!({"type": "slider", "a": {"face": [10, 5, 5]}, "b": {"face": [20, 5, 5]}, "name": "Rail"}));
|
||||
run(&mut s, "joint.drive", json!({"joint": "Rail", "value": 10}));
|
||||
s
|
||||
}
|
||||
|
||||
/// B is pushed 1 mm a step from 10 mm away to 5 mm into A: the faces touch at step 10 and the
|
||||
/// blocks overlap from step 11 to the end, 100 mm³ more each step.
|
||||
#[test]
|
||||
fn interference_names_the_step_two_parts_start_to_collide() {
|
||||
let mut s = rail();
|
||||
run(&mut s, "motion.study", json!({"name": "Push", "steps": 15, "keys": [{"joint": "Rail", "points": [[0, 10], [15, -5]]}]}));
|
||||
// A contact set would stop the motion where the faces touch; the check plays through.
|
||||
run(&mut s, "contact.create", json!({"occurrences": ["A", "B"]}));
|
||||
let before = s.doc.clone();
|
||||
for p in [json!({"study": "Push"}), json!({"study": "Push", "occurrences": ["A", "B"]}), json!({"study": "Push", "contact_sets": true})] {
|
||||
let r = run(&mut s, "motion.interference", p.clone());
|
||||
assert_eq!(*s.doc, *before, "the design is left as it was");
|
||||
assert!(r["clear"] == false && r["samples"] == 16 && r["pairs"] == 1 && r["truncated"] == false, "{p}: {r}");
|
||||
let c = &r["collisions"][0];
|
||||
assert_eq!((c["a"].as_str(), c["b"].as_str()), (Some("A:1"), Some("B:1")), "{r}");
|
||||
assert_eq!((c["first_step"].as_f64(), c["last_step"].as_f64()), (Some(11.0), Some(15.0)), "{r}");
|
||||
assert_eq!(c["steps"], json!([[11.0, 15.0]]), "{r}");
|
||||
let deepest = c["deepest_step"].as_f64().unwrap_or(f64::NAN);
|
||||
assert!(c["depth_mm"].as_f64().unwrap_or(0.0) > 1.0 && (11.0..=15.0).contains(&deepest), "{r}");
|
||||
let volume = c["volume_mm3"].as_f64().unwrap_or(f64::NAN);
|
||||
assert!((volume - 100.0 * (deepest - 10.0)).abs() < 1e-6, "{r}");
|
||||
}
|
||||
// B against a far block C only: B's pass through A is not asked about.
|
||||
run(&mut s, "component.create", json!({"name": "C"}));
|
||||
run(&mut s, "solid.box", json!({"length": 10, "width": 10, "height": 10, "corner": [0, 50, 0]}));
|
||||
run(&mut s, "component.activate", json!({"component": "root"}));
|
||||
let r = run(&mut s, "motion.interference", json!({"study": "Push", "occurrences": ["B"], "against": ["C"]}));
|
||||
assert!(r["clear"] == true && r["pairs"] == 1, "{r}");
|
||||
let r = run(&mut s, "motion.interference", json!({"study": "Push", "occurrences": ["B"], "against": ["A", "C"]}));
|
||||
assert!(r["pairs"] == 2 && r["collisions"].as_array().map(Vec::len) == Some(1) && r["collisions"][0]["first_step"] == 11.0, "{r}");
|
||||
assert!(s.execute("motion.interference", &json!({"study": "Push", "against": ["A"]})).is_err());
|
||||
// Pushed only to touching, they never pass through each other.
|
||||
run(&mut s, "motion.study", json!({"study": "Push", "keys": [{"joint": "Rail", "points": [[0, 10], [15, 0]]}]}));
|
||||
let r = run(&mut s, "motion.interference", json!({"study": "Push"}));
|
||||
assert!(r["clear"] == true && r["collisions"] == json!([]), "{r}");
|
||||
}
|
||||
|
||||
/// A number names a study by its id first, text by its name first: a study may be named like
|
||||
/// another's id.
|
||||
#[test]
|
||||
fn studies_by_id_or_name() {
|
||||
let mut s = rail();
|
||||
let keys = json!([{"joint": "Rail", "points": [[0, 10], [15, -5]]}]);
|
||||
let first = run(&mut s, "motion.study", json!({"name": "Late", "steps": 15, "keys": keys}))["study"].as_u64().unwrap_or(0);
|
||||
run(&mut s, "motion.study", json!({"name": first.to_string(), "steps": 15, "keys": keys}));
|
||||
assert_eq!(run(&mut s, "motion.plot", json!({"study": first, "samples": 3}))["study"], "Late");
|
||||
assert_eq!(run(&mut s, "motion.plot", json!({"study": first.to_string(), "samples": 3}))["study"], first.to_string());
|
||||
}
|
||||
|
||||
/// Hostile parameters are refused, never a panic.
|
||||
#[test]
|
||||
fn motion_analysis_refuses_what_it_cannot_do() {
|
||||
let mut s = rail();
|
||||
run(&mut s, "motion.study", json!({"name": "Push", "steps": 15, "keys": [{"joint": "Rail", "points": [[0, 10], [15, -5]]}]}));
|
||||
let refused = [
|
||||
("motion.plot", json!({"study": "Nope"})),
|
||||
("motion.plot", json!({"study": "Push", "samples": 2})),
|
||||
("motion.plot", json!({"study": "Push", "samples": 1e9})),
|
||||
("motion.plot", json!({"study": "Push", "samples": "many"})),
|
||||
("motion.plot", json!({"study": "Push", "duration": 0})),
|
||||
("motion.plot", json!({"study": "Push", "duration": -3})),
|
||||
("motion.plot", json!({"study": "Push", "duration": 1e-200})),
|
||||
("motion.plot", json!({"study": "Push", "joints": ["Nope"]})),
|
||||
("motion.plot", json!({"study": "Push", "joints": vec![json!("Rail"); 300]})),
|
||||
("motion.plot", json!({"study": "Push", "point": [1, 2]})),
|
||||
("motion.plot", json!({"study": "Push", "occurrence": "B", "point": [1e300, 0, 0]})),
|
||||
("motion.interference", json!({"study": "Push", "samples": 1})),
|
||||
("motion.interference", json!({"study": "Push", "samples": f64::MAX})),
|
||||
("motion.interference", json!({"study": "Push", "occurrences": ["A"]})),
|
||||
("motion.interference", json!({"study": "Push", "occurrences": ["A", "B"], "contact_sets": true})),
|
||||
("motion.interference", json!({"study": "Push", "contact_sets": true})),
|
||||
];
|
||||
for (cmd, p) in refused {
|
||||
assert!(s.execute(cmd, &p).is_err(), "{cmd} {p}");
|
||||
}
|
||||
// A point of a part that does not move has no velocity.
|
||||
let r = run(&mut s, "motion.plot", json!({"study": "Push", "occurrence": "A", "point": [5, 5, 5]}));
|
||||
assert!(r["point"]["speed"].as_array().is_some_and(|v| v.len() == 16 && v.iter().all(|x| x.as_f64() == Some(0.0))), "{r}");
|
||||
}
|
||||
Reference in new issue
Block a user