Files
solidcraft/crates/dynamics/src/tests.rs
T
774c45a512 Dynamics: rigid-body solver core with joints, gravity and springs (#9719) (#9721)
* claim: #9719

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

* Dynamics: rigid-body solver core with joints, gravity and springs (#9719)

A new L1 crate, solidcraft-dynamics (geom only): rigid bodies in maximal coordinates, joint constraint equations with analytic Jacobians for rigid, revolute, slider, cylindrical, planar and ball joints, gravity, linear spring-dampers and constant loads; the index-1 equations of motion solved through a rank-revealing Schur complement (repeated joint equations are solved, not refused); RK4 and Dormand-Prince 5(4) with position and velocity projection; joint values, rates, reactions and energies at every output sample; caps on bodies, joints, rows, steps and samples; Result errors only.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

* Dynamics: take Vec3 from the record crate after the layer split, refresh the lock file

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

---------

Co-authored-by: Claude Opus 5.5 <noreply@anthropic.com>
Co-authored-by: bherbruck <brennen.herbruck@gmail.com>
2026-11-04 21:27:17 +00:00

541 lines
25 KiB
Rust
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
//! Analytic checks: free fall, pendulum periods, spring-mass oscillation, energy and momentum
//! conservation, slider-crank kinematics, joint reactions, Jacobians against finite
//! differences, and hostile input.
use std::f64::consts::{PI, TAU};
use solidcore_record::Vec3;
use solidcraft_geom::prelude::*;
use crate::system::{System, testing};
use crate::*;
const G: f64 = STANDARD_GRAVITY;
fn down() -> Vec3 {
Vec3::new(0.0, 0.0, -G)
}
fn ok<T>(r: Result<T>) -> T {
match r {
Ok(v) => v,
Err(e) => panic!("simulation failed: {e}"),
}
}
/// A body hanging `len` mm under a hinge at the origin turning about Y, at angle `theta` from
/// straight down (towards +X) and turning at `omega`.
fn pendulum(len: f64, theta: f64, omega: f64, inertia: f64) -> Model {
let dir = Vec3::new(theta.sin(), 0.0, -theta.cos());
// Turning about +Y takes −Z towards −X; the hinge's own Y axis is the world's.
let rot = Quat::from_axis_angle(Vec3::Y, -theta);
let mut body = Body::at_rest(1.0, Vec3::new(inertia, inertia, inertia), dir * len);
body.orientation = rot;
body.angular_velocity = Vec3::Y * -omega;
body.velocity = (Vec3::Y * -omega).cross(dir * len);
let joint = Joint {
kind: JointKind::Revolute,
a: Marker::ground(Vec3::ZERO, Vec3::Y, Vec3::X),
b: Marker::new(Some(0), Vec3::new(0.0, 0.0, len), Vec3::Y, Vec3::X),
};
Model { bodies: vec![body], joints: vec![joint], forces: vec![], gravity: down() }
}
/// Times where `f` crosses zero upwards, linearly interpolated between samples.
fn upward_crossings(traj: &Trajectory, f: impl Fn(&Sample) -> f64) -> Vec<f64> {
traj.samples
.windows(2)
.filter_map(|w| {
let (a, b) = (w.first()?, w.get(1)?);
let (fa, fb) = (f(a), f(b));
(fa < 0.0 && fb >= 0.0).then(|| a.time + (b.time - a.time) * fa / (fa - fb))
})
.collect()
}
fn mean_period(crossings: &[f64]) -> f64 {
assert!(crossings.len() >= 3, "too few crossings: {crossings:?}");
let (first, last) = (crossings.first().copied().unwrap_or(0.0), crossings.last().copied().unwrap_or(0.0));
(last - first) / (crossings.len() - 1) as f64
}
fn angle_of(s: &Sample) -> f64 {
s.joints.first().and_then(|j| j.values.first()).copied().unwrap_or(f64::NAN)
}
#[test]
fn free_fall_follows_the_parabola() {
let mut body = Body::at_rest(2.5, Vec3::new(10.0, 20.0, 30.0), Vec3::new(1.0, 2.0, 3.0));
body.velocity = Vec3::new(10.0, -5.0, 50.0);
body.angular_velocity = Vec3::new(0.0, 0.0, 1.0);
let model = Model { bodies: vec![body.clone()], joints: vec![], forces: vec![], gravity: down() };
let traj = ok(simulate(&model, &Settings::rk4(1.0, 0.01, 0.1)));
assert_eq!(traj.samples.len(), 11);
for s in &traj.samples {
let t = s.time;
let expect = body.position + body.velocity * t + down() * (0.5 * t * t);
let got = s.bodies.first().map(|b| b.position).unwrap_or(Vec3::ZERO);
assert!(got.dist(expect) < 1e-7, "t={t}: {got:?} vs {expect:?}");
// A symmetric body spinning about z keeps spinning.
let w = s.bodies.first().map(|b| b.angular_velocity).unwrap_or(Vec3::ZERO);
assert!(w.dist(Vec3::Z) < 1e-12);
// Energy is conserved exactly enough.
let e0 = traj.samples.first().map(|s| s.energy.total()).unwrap_or(0.0);
assert!((s.energy.total() - e0).abs() < 1e-6 * e0.abs().max(1.0));
}
}
#[test]
fn pendulum_small_angle_period() {
let (len, inertia) = (100.0, 40.0);
let i_pivot = inertia + len * len;
let period = TAU * (i_pivot / (G * len)).sqrt();
let amplitude = 0.01;
let model = pendulum(len, 0.0, amplitude * TAU / period, inertia);
let traj = ok(simulate(&model, &Settings::rk4(3.0, 1e-3, 1e-3)));
let measured = mean_period(&upward_crossings(&traj, angle_of));
// The finite amplitude lengthens the period by about θ²/16 (6e-6 here).
assert!((measured - period).abs() < 1e-4 * period, "period {measured} vs {period}");
assert!(traj.stats.max_constraint_error < 1e-9, "{:?}", traj.stats);
}
#[test]
fn adaptive_integration_matches_the_pendulum_period() {
let (len, inertia) = (100.0, 40.0);
let period = TAU * ((inertia + len * len) / (G * len)).sqrt();
let model = pendulum(len, 0.0, 0.01 * TAU / period, inertia);
let traj = ok(simulate(&model, &Settings::adaptive(3.0, 1e-3, 1e-9, 1e-9)));
let measured = mean_period(&upward_crossings(&traj, angle_of));
assert!((measured - period).abs() < 1e-4 * period, "period {measured} vs {period}");
// Larger tolerances take fewer steps than samples (the output still lands on each).
let coarse = Settings {
integrator: Integrator::Adaptive { rel_tol: 1e-6, abs_tol: 1e-6, min_step: 1e-9, max_step: 0.05 },
..Settings::rk4(3.0, 1.0, 0.05)
};
let traj = ok(simulate(&model, &coarse));
assert_eq!(traj.samples.len(), 61);
let measured = mean_period(&upward_crossings(&traj, angle_of));
assert!((measured - period).abs() < 2e-3 * period, "coarse period {measured} vs {period}");
}
#[test]
fn hanging_body_pin_carries_its_weight() {
let model = pendulum(100.0, 0.0, 0.0, 40.0);
let traj = ok(simulate(&model, &Settings::rk4(0.1, 1e-3, 0.05)));
for s in &traj.samples {
let j = s.joints.first().map(|j| (j.force, j.torque)).unwrap_or_default();
assert!(j.0.dist(Vec3::new(0.0, 0.0, G)) < 1e-6 * G, "force {:?}", j.0);
assert!(j.1.len() < 1e-6 * G, "torque {:?}", j.1);
assert!(angle_of(s).abs() < 1e-12);
}
}
#[test]
fn large_swing_conserves_energy() {
// Released from horizontal: a compound pendulum swinging through ±90°.
let model = pendulum(100.0, PI / 2.0, 0.0, 400.0);
let traj = ok(simulate(&model, &Settings::rk4(2.0, 1e-3, 1e-2)));
let e0 = traj.samples.first().map(|s| s.energy.total()).unwrap_or(0.0);
let scale = G * 100.0;
for s in &traj.samples {
assert!((s.energy.total() - e0).abs() < 1e-6 * scale, "t={} drift {}", s.time, s.energy.total() - e0);
}
}
/// Two links hinged about Y, released from horizontal.
fn double_pendulum() -> Model {
let link = |x: f64| Body::at_rest(1.0, Vec3::new(100.0, 900.0, 900.0), Vec3::new(x, 0.0, 0.0));
let hinge = |a: Option<usize>, pa: Vec3, b: usize, pb: Vec3| Joint {
kind: JointKind::Revolute,
a: Marker::new(a, pa, Vec3::Y, Vec3::X),
b: Marker::new(Some(b), pb, Vec3::Y, Vec3::X),
};
Model {
bodies: vec![link(50.0), link(150.0)],
joints: vec![
hinge(None, Vec3::ZERO, 0, Vec3::new(-50.0, 0.0, 0.0)),
hinge(Some(0), Vec3::new(50.0, 0.0, 0.0), 1, Vec3::new(-50.0, 0.0, 0.0)),
],
forces: vec![],
gravity: down(),
}
}
#[test]
fn double_pendulum_conserves_energy() {
let traj = ok(simulate(&double_pendulum(), &Settings::rk4(5.0, 5e-4, 1e-2)));
let e0 = traj.samples.first().map(|s| s.energy.total()).unwrap_or(0.0);
let scale = 2.0 * G * 150.0;
let worst = traj.samples.iter().map(|s| (s.energy.total() - e0).abs()).fold(0.0, f64::max);
assert!(worst < 1e-5 * scale, "energy drift {worst} of {scale}");
// It did swing hard: the lower link went well past vertical.
let lowest = traj.samples.iter().filter_map(|s| s.bodies.get(1)).map(|b| b.position.z).fold(0.0, f64::min);
assert!(lowest < -100.0, "{lowest}");
assert_eq!(traj.stats.redundant_equations, 0);
}
#[test]
fn constraint_drift_stays_bounded_over_long_runs() {
// Projection holds the joints to its tolerance (1e-10) however long the run.
let traj = ok(simulate(&double_pendulum(), &Settings::rk4(20.0, 1e-3, 0.1)));
assert!(traj.stats.max_constraint_error <= 1e-10, "{:?}", traj.stats);
// Without projection, Baumgarte terms keep the drift bounded too, if less tightly.
let settings = Settings { projection: false, baumgarte: Some((20.0, 20.0)), ..Settings::rk4(20.0, 1e-3, 0.1) };
let traj = ok(simulate(&double_pendulum(), &settings));
assert!(traj.stats.max_constraint_error < 1e-3, "{:?}", traj.stats);
}
#[test]
fn slider_crank_follows_its_closed_form() {
let (r, l) = (50.0, 150.0);
let theta0 = PI / 3.0;
let pin = Vec3::new(r * theta0.cos(), 0.0, -r * theta0.sin());
let s0 = r * theta0.cos() + (l * l - (r * theta0.sin()).powi(2)).sqrt();
let slide = Vec3::new(s0, 0.0, 0.0);
let u = (slide - pin) * (1.0 / l);
let mut crank = Body::at_rest(1.0, Vec3::new(100.0, 2000.0, 2000.0), pin * 0.5);
crank.orientation = Quat::from_axis_angle(Vec3::Y, theta0);
crank.angular_velocity = Vec3::Y * TAU;
crank.velocity = (Vec3::Y * TAU).cross(pin * 0.5);
let mut rod = Body::at_rest(0.5, Vec3::new(50.0, 1000.0, 1000.0), (pin + slide) * 0.5);
rod.orientation = Quat::from_axis_angle(Vec3::Y, (-u.z).atan2(u.x));
let slider = Body::at_rest(0.3, Vec3::new(50.0, 50.0, 50.0), slide);
let hinge = |a: Option<usize>, pa: Vec3, b: usize, pb: Vec3| Joint {
kind: JointKind::Revolute,
a: Marker::new(a, pa, Vec3::Y, Vec3::X),
b: Marker::new(Some(b), pb, Vec3::Y, Vec3::X),
};
let model = Model {
bodies: vec![crank, rod, slider],
joints: vec![
hinge(None, Vec3::ZERO, 0, Vec3::new(-r / 2.0, 0.0, 0.0)),
hinge(Some(0), Vec3::new(r / 2.0, 0.0, 0.0), 1, Vec3::new(-l / 2.0, 0.0, 0.0)),
hinge(Some(1), Vec3::new(l / 2.0, 0.0, 0.0), 2, Vec3::ZERO),
Joint { kind: JointKind::Slider, a: Marker::ground(Vec3::ZERO, Vec3::X, Vec3::Y), b: Marker::new(Some(2), Vec3::ZERO, Vec3::X, Vec3::Y) },
],
forces: vec![],
gravity: Vec3::ZERO,
};
let traj = ok(simulate(&model, &Settings::rk4(2.0, 1e-3, 1e-2)));
// A planar linkage built from 3D joints repeats three equations; they are solved, not refused.
assert_eq!(traj.stats.redundant_equations, 3);
let mut turned = 0.0_f64;
let mut last = theta0;
for s in &traj.samples {
let theta = angle_of(s);
let x = s.joints.get(3).and_then(|j| j.values.first()).copied().unwrap_or(f64::NAN);
let expect = r * theta.cos() + (l * l - (r * theta.sin()).powi(2)).sqrt();
assert!((x - expect).abs() < 1e-6, "t={}: slider {x} vs {expect}", s.time);
let step = (theta - last + PI).rem_euclid(TAU) - PI;
turned += step;
last = theta;
}
assert!(turned.abs() > TAU, "the crank turned {turned} rad");
assert!(traj.stats.max_constraint_error < 1e-9, "{:?}", traj.stats);
// No forces: kinetic energy is conserved.
let e0 = traj.samples.first().map(|s| s.energy.kinetic).unwrap_or(0.0);
assert!(traj.samples.iter().all(|s| (s.energy.kinetic - e0).abs() < 1e-6 * e0), "kinetic energy drifted");
}
/// A body on a slider along X, pulled toward the origin by a spring of free length 40 mm,
/// starting 10 mm stretched, with gravity across the slide.
fn spring_mass(damping: f64) -> Model {
Model {
bodies: vec![Body::at_rest(2.0, Vec3::new(500.0, 500.0, 500.0), Vec3::new(50.0, 0.0, 0.0))],
joints: vec![Joint {
kind: JointKind::Slider,
a: Marker::ground(Vec3::ZERO, Vec3::X, Vec3::Y),
b: Marker::new(Some(0), Vec3::ZERO, Vec3::X, Vec3::Y),
}],
forces: vec![Force::Spring {
a: Anchor { body: None, point: Vec3::ZERO },
b: Anchor { body: Some(0), point: Vec3::ZERO },
stiffness: 800.0,
damping,
free_length: 40.0,
preload: 0.0,
}],
gravity: down(),
}
}
#[test]
fn spring_mass_oscillates_at_its_natural_frequency() {
// ω = √(k/m) = 20 rad/s.
let traj = ok(simulate(&spring_mass(0.0), &Settings::rk4(1.0, 1e-4, 1e-3)));
for s in &traj.samples {
let x = s.bodies.first().map(|b| b.position.x).unwrap_or(f64::NAN);
let expect = 40.0 + 10.0 * (20.0 * s.time).cos();
assert!((x - expect).abs() < 1e-6, "t={}: {x} vs {expect}", s.time);
// The slide holds the body up against gravity.
let f = s.joints.first().map(|j| j.force).unwrap_or_default();
assert!((f.z - 2.0 * G).abs() < 1e-6 * G && f.x.abs() < 1e-6 * G, "reaction {f:?}");
}
let e0 = traj.samples.first().map(|s| s.energy.total()).unwrap_or(0.0);
assert!(traj.samples.iter().all(|s| (s.energy.total() - e0).abs() < 1e-8 * 800.0 * 100.0));
}
#[test]
fn spring_damper_decays_as_the_closed_form() {
// c = 8 kg/s: ζ = c / (2√(km)) = 0.1.
let (wn, zeta) = (20.0_f64, 0.1_f64);
let wd = wn * (1.0 - zeta * zeta).sqrt();
let traj = ok(simulate(&spring_mass(8.0), &Settings::rk4(1.0, 1e-4, 1e-3)));
for s in &traj.samples {
let t = s.time;
let x = s.bodies.first().map(|b| b.position.x).unwrap_or(f64::NAN);
let expect = 40.0 + (-zeta * wn * t).exp() * 10.0 * ((wd * t).cos() + zeta * wn / wd * (wd * t).sin());
assert!((x - expect).abs() < 1e-6, "t={t}: {x} vs {expect}");
}
}
#[test]
fn torque_free_body_keeps_energy_and_momentum() {
let mut body = Body::at_rest(1.0, Vec3::new(1000.0, 2000.0, 3000.0), Vec3::ZERO);
body.orientation = Quat::from_axis_angle(Vec3::new(1.0, 2.0, 3.0), 0.7);
body.angular_velocity = Vec3::new(1.0, 0.3, 5.0);
let model = Model { bodies: vec![body], joints: vec![], forces: vec![], gravity: Vec3::ZERO };
let traj = ok(simulate(&model, &Settings::rk4(2.0, 1e-4, 1e-2)));
let momentum = |s: &Sample| {
let b = s.bodies.first().copied();
b.map_or(Vec3::ZERO, |b| {
let r = b.orientation.to_mat3();
r.mul(&Mat3::diagonal(Vec3::new(1000.0, 2000.0, 3000.0))).mul(&r.transpose()).mul_vec(b.angular_velocity)
})
};
let (l0, e0) = traj.samples.first().map(|s| (momentum(s), s.energy.kinetic)).unwrap_or_default();
let mut tumbled = false;
for s in &traj.samples {
assert!(momentum(s).dist(l0) < 1e-7 * l0.len(), "t={} momentum {:?} vs {l0:?}", s.time, momentum(s));
assert!((s.energy.kinetic - e0).abs() < 1e-7 * e0);
let w = s.bodies.first().map(|b| b.angular_velocity).unwrap_or_default();
tumbled |= w.dist(Vec3::new(1.0, 0.3, 5.0)) > 0.1;
}
assert!(tumbled, "the angular velocity should precess");
}
#[test]
fn spherical_pendulum_keeps_energy_and_vertical_momentum() {
// A body on a ball joint, tilted and pushed sideways: it swings round the vertical.
let com = Vec3::new(60.0, 0.0, -80.0);
let mut body = Body::at_rest(1.5, Vec3::new(300.0, 500.0, 700.0), com);
body.velocity = Vec3::new(0.0, 400.0, 0.0);
let model = Model {
bodies: vec![body],
joints: vec![Joint {
kind: JointKind::Ball,
a: Marker::ground(Vec3::ZERO, Vec3::Z, Vec3::X),
b: Marker::new(Some(0), -com, Vec3::Z, Vec3::X),
}],
forces: vec![],
gravity: down(),
};
let traj = ok(simulate(&model, &Settings::rk4(3.0, 5e-4, 1e-2)));
let lz = |s: &Sample| {
s.bodies.first().map_or(0.0, |b| {
let r = b.orientation.to_mat3();
let iw = r.mul(&Mat3::diagonal(Vec3::new(300.0, 500.0, 700.0))).mul(&r.transpose());
(b.position.cross(b.velocity * 1.5) + iw.mul_vec(b.angular_velocity)).z
})
};
let (l0, e0) = traj.samples.first().map(|s| (lz(s), s.energy.total())).unwrap_or_default();
assert!(l0.abs() > 1e3);
for s in &traj.samples {
assert!((lz(s) - l0).abs() < 1e-6 * l0.abs(), "t={}: Lz {} vs {l0}", s.time, lz(s));
assert!((s.energy.total() - e0).abs() < 1e-6 * G * 100.0, "t={}: energy drift {}", s.time, s.energy.total() - e0);
// The joint holds the body's pivot at the origin.
let pivot = s.bodies.first().map(|b| b.position + b.orientation.rotate(-com)).unwrap_or_default();
assert!(pivot.len() < 1e-9, "{pivot:?}");
}
}
/// Every joint kind between two moving bodies and between the ground and a body: the
/// Jacobian and `J̇u` agree with finite differences of `Φ` along the motion.
#[test]
fn jacobians_match_finite_differences() {
let kinds = [JointKind::Rigid, JointKind::Revolute, JointKind::Slider, JointKind::Cylindrical, JointKind::Planar, JointKind::Ball];
let mut a = Body::at_rest(1.0, Vec3::new(1.0, 2.0, 3.0), Vec3::new(10.0, -20.0, 5.0));
a.orientation = Quat::from_axis_angle(Vec3::new(0.3, -1.0, 0.4), 0.9);
a.velocity = Vec3::new(3.0, -1.0, 2.0);
a.angular_velocity = Vec3::new(0.7, -0.4, 1.1);
let mut b = Body::at_rest(2.0, Vec3::new(3.0, 2.0, 1.0), Vec3::new(-5.0, 15.0, 30.0));
b.orientation = Quat::from_axis_angle(Vec3::new(1.0, 0.2, -0.5), -1.3);
b.velocity = Vec3::new(-2.0, 4.0, 1.0);
b.angular_velocity = Vec3::new(-0.5, 1.3, 0.2);
for kind in kinds {
for a_body in [Some(0), None] {
let joint = Joint {
kind,
a: Marker::new(a_body, Vec3::new(4.0, -3.0, 7.0), Vec3::new(0.2, 0.5, 1.0), Vec3::new(1.0, 0.0, 0.3)),
b: Marker::new(Some(1), Vec3::new(-6.0, 2.0, 1.0), Vec3::new(-0.4, 1.0, 0.1), Vec3::new(0.0, 0.3, 1.0)),
};
let model = Model { bodies: vec![a.clone(), b.clone()], joints: vec![joint], forces: vec![], gravity: Vec3::ZERO };
let sys = ok(System::new(&model, None));
let y = ok(System::initial_state(&model));
let h = 1e-4;
let (now, plus, minus) = (
testing::rows_at(&sys, &y),
testing::rows_at(&sys, &testing::drift(&sys, &y, h)),
testing::rows_at(&sys, &testing::drift(&sys, &y, -h)),
);
assert_eq!(now.len(), kind.equations());
for ((phi, ju, jdu), ((p, _, _), (m, _, _))) in now.iter().zip(plus.iter().zip(&minus)) {
let rate = (p - m) / (2.0 * h);
let accel = (p - 2.0 * phi + m) / (h * h);
assert!((rate - ju).abs() < 1e-5 * (1.0 + ju.abs()), "{kind:?} {a_body:?}: J u {ju} vs {rate}");
assert!((accel - jdu).abs() < 1e-3 * (1.0 + jdu.abs()), "{kind:?} {a_body:?}: J̇ u {jdu} vs {accel}");
}
}
}
}
#[test]
fn joint_values_read_the_relative_motion() {
// A body on each joint kind, displaced along its free directions, reads them back.
let place = |kind: JointKind, offset: Vec3, turn: f64| {
let mut body = Body::at_rest(1.0, Vec3::new(1.0, 1.0, 1.0), offset);
body.orientation = Quat::from_axis_angle(Vec3::Z, turn);
let model = Model {
bodies: vec![body],
joints: vec![Joint { kind, a: Marker::ground(Vec3::ZERO, Vec3::Z, Vec3::X), b: Marker::new(Some(0), Vec3::ZERO, Vec3::Z, Vec3::X) }],
forces: vec![],
gravity: Vec3::ZERO,
};
let traj = ok(simulate(&model, &Settings::rk4(0.01, 0.01, 0.01)));
traj.samples.first().and_then(|s| s.joints.first()).map(|j| j.values.clone()).unwrap_or_default()
};
let close = |a: &[f64], b: &[f64]| a.len() == b.len() && a.iter().zip(b).all(|(x, y)| (x - y).abs() < 1e-9);
assert!(close(&place(JointKind::Revolute, Vec3::ZERO, 0.4), &[0.4]));
assert!(close(&place(JointKind::Slider, Vec3::new(0.0, 0.0, 12.0), 0.0), &[12.0]));
assert!(close(&place(JointKind::Cylindrical, Vec3::new(0.0, 0.0, -3.0), -1.0), &[-1.0, -3.0]));
assert!(close(&place(JointKind::Planar, Vec3::new(5.0, -7.0, 0.0), 2.0), &[5.0, -7.0, 2.0]));
assert!(close(&place(JointKind::Ball, Vec3::ZERO, 0.5), &[0.0, 0.0, 0.5]));
assert!(close(&place(JointKind::Rigid, Vec3::ZERO, 0.0), &[]));
}
#[test]
fn inconsistent_start_is_projected_onto_the_joints() {
// The pendulum's body starts 2 mm off its hinge and moving across it.
let mut model = pendulum(100.0, 0.3, 0.0, 40.0);
if let Some(b) = model.bodies.first_mut() {
b.position += Vec3::new(1.0, 2.0, -1.0);
b.velocity = Vec3::new(0.0, 50.0, 0.0);
}
let traj = ok(simulate(&model, &Settings::rk4(0.5, 1e-3, 0.05)));
assert!(traj.stats.max_constraint_error < 1e-9, "{:?}", traj.stats);
let y = traj.samples.first().and_then(|s| s.bodies.first()).map(|b| b.velocity.y).unwrap_or(f64::NAN);
assert!(y.abs() < 1e-9, "velocity along the hinge is removed: {y}");
}
#[test]
fn hostile_input_is_an_error_not_a_panic() {
let good = pendulum(100.0, 0.2, 0.0, 40.0);
let settings = Settings::rk4(0.1, 1e-3, 1e-2);
let mut cases: Vec<(Model, Settings)> = Vec::new();
let mut with = |f: &dyn Fn(&mut Model)| {
let mut m = good.clone();
f(&mut m);
cases.push((m, settings));
};
with(&|m| m.bodies.clear());
with(&|m| m.bodies.iter_mut().for_each(|b| b.mass = f64::NAN));
with(&|m| m.bodies.iter_mut().for_each(|b| b.mass = 0.0));
with(&|m| m.bodies.iter_mut().for_each(|b| b.mass = -1.0));
with(&|m| m.bodies.iter_mut().for_each(|b| b.inertia = Mat3::diagonal(Vec3::new(1.0, -1.0, 1.0))));
with(&|m| m.bodies.iter_mut().for_each(|b| b.inertia = Mat3([[1.0, 5.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])));
with(&|m| m.bodies.iter_mut().for_each(|b| b.inertia = Mat3::ZERO));
with(&|m| m.bodies.iter_mut().for_each(|b| b.position.x = f64::INFINITY));
with(&|m| m.bodies.iter_mut().for_each(|b| b.orientation = Quat { w: 0.0, x: 0.0, y: 0.0, z: 0.0 }));
with(&|m| m.bodies.iter_mut().for_each(|b| b.velocity.z = 1e300));
with(&|m| m.gravity = Vec3::new(f64::NAN, 0.0, 0.0));
with(&|m| m.joints.iter_mut().for_each(|j| j.b.body = Some(7)));
with(&|m| m.joints.iter_mut().for_each(|j| j.a.body = j.b.body));
with(&|m| m.joints.iter_mut().for_each(|j| j.a.z = Vec3::ZERO));
with(&|m| m.joints.iter_mut().for_each(|j| j.b.x = j.b.z));
with(&|m| m.forces.push(Force::Load { body: 3, point: Vec3::ZERO, force: Vec3::X, torque: Vec3::ZERO }));
with(&|m| {
m.forces.push(Force::Spring {
a: Anchor { body: None, point: Vec3::ZERO },
b: Anchor { body: Some(0), point: Vec3::ZERO },
stiffness: -1.0,
damping: 0.0,
free_length: 0.0,
preload: 0.0,
})
});
with(&|m| {
m.forces.push(Force::Spring {
a: Anchor { body: Some(9), point: Vec3::ZERO },
b: Anchor { body: Some(0), point: Vec3::ZERO },
stiffness: 1.0,
damping: 0.0,
free_length: 0.0,
preload: 0.0,
})
});
with(&|m| m.bodies = vec![Body::at_rest(1.0, Vec3::new(1.0, 1.0, 1.0), Vec3::ZERO); MAX_BODIES + 1]);
with(&|m| {
m.joints = vec![
m.joints.first().copied().unwrap_or(Joint {
kind: JointKind::Ball,
a: Marker::ground(Vec3::ZERO, Vec3::Z, Vec3::X),
b: Marker::new(Some(0), Vec3::ZERO, Vec3::Z, Vec3::X)
});
MAX_JOINTS + 1
]
});
let bad_settings = [
Settings::rk4(0.0, 1e-3, 1e-2),
Settings::rk4(-1.0, 1e-3, 1e-2),
Settings::rk4(f64::NAN, 1e-3, 1e-2),
Settings::rk4(1.0, 0.0, 1e-2),
Settings::rk4(1.0, f64::INFINITY, 1e-2),
Settings::rk4(1.0, 1e-3, 0.0),
Settings::rk4(1e6, 1e-6, 1e-2),
Settings::rk4(1e3, 1e-3, 1e-6),
Settings { projection_tol: -1.0, ..settings },
Settings { baumgarte: Some((f64::NAN, 1.0)), ..settings },
Settings { integrator: Integrator::Adaptive { rel_tol: 1e-6, abs_tol: 1e-6, min_step: 1.0, max_step: 0.1 }, ..settings },
Settings { integrator: Integrator::Adaptive { rel_tol: 0.0, abs_tol: 1e-6, min_step: 1e-9, max_step: 0.1 }, ..settings },
];
cases.extend(bad_settings.into_iter().map(|s| (good.clone(), s)));
for (i, (m, s)) in cases.iter().enumerate() {
assert!(simulate(m, s).is_err(), "case {i} should be refused");
}
// Inputs that are valid but extreme stop with an error or finish; they never panic.
let stiff = Model {
forces: vec![Force::Spring {
a: Anchor { body: None, point: Vec3::ZERO },
b: Anchor { body: Some(0), point: Vec3::ZERO },
stiffness: 1e11,
damping: 0.0,
free_length: 0.0,
preload: 0.0,
}],
..good.clone()
};
let _ = simulate(&stiff, &Settings::rk4(0.1, 1e-2, 1e-2));
let tight = Settings { integrator: Integrator::Adaptive { rel_tol: 1e-14, abs_tol: 1e-14, min_step: 1e-3, max_step: 1e-3 }, ..settings };
let _ = simulate(&stiff, &tight);
}
#[test]
fn models_round_trip_through_json() {
let mut model = spring_mass(1.5);
model.forces.push(Force::Load { body: 0, point: Vec3::new(1.0, 0.0, 0.0), force: Vec3::new(0.0, 5.0, 0.0), torque: Vec3::ZERO });
let text = match serde_json::to_string(&model) {
Ok(t) => t,
Err(e) => panic!("{e}"),
};
let back: Model = match serde_json::from_str(&text) {
Ok(m) => m,
Err(e) => panic!("{e}"),
};
assert_eq!(back, model);
let settings: Settings = match serde_json::from_str(r#"{"duration":1.0,"output_interval":0.1,"integrator":{"type":"rk4","step":0.001}}"#) {
Ok(s) => s,
Err(e) => panic!("{e}"),
};
assert!(settings.projection && settings.validate().is_ok());
}