Skip to content

SolidCraft: an open-source parametric 3D CAD application built in pure Rust. Desktop, web, and automation through MCP.

38.8 KB
Raw Blame
//! 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![], drives: vec![], links: 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![], drives: vec![], links: 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(),
        ..Model::default()
    }
}

#[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,
        ..Model::default()
    };
    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(),
        ..Model::default()
    }
}

#[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![], drives: vec![], links: 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(),
        ..Model::default()
    };
    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![], drives: vec![], links: vec![], gravity: Vec3::ZERO };
            let sys = ok(System::new(&model, None));
            let y = ok(sys.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,
            ..Model::default()
        };
        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 });
    model.forces.push(Force::JointSpring { at: Coord::new(0, 0), stiffness: 3.0, damping: 0.5, rest: 1.0 });
    model.forces.push(Force::Motor { at: Coord::new(0, 0), effort: Profile::Table { points: vec![(0.0, 1.0), (2.0, 3.0)] } });
    model.drives.push(Drive { at: Coord::new(0, 0), motion: Profile::Ramp { from: 0.0, to: 1.0, start: 0.5, duration: 2.0 } });
    model.links.push(Link { driver: Coord::new(0, 0), follower: Coord::new(1, 0), ratio: 2.0, offset: Some(0.5) });
    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());
}

// Joint springs, motors, drives and motion links.

/// `a − b` turned into (−π, π].
fn wrap(x: f64) -> f64 {
    (x + PI).rem_euclid(TAU) - PI
}

/// A wheel turning about Z on a revolute to the ground at `at`, its centre of mass on the axis.
/// Wheels turning about Z on revolutes to the ground, centres of mass on their axes (inertia,
/// centre); joint `i` holds body `i`.
fn rotors(parts: &[(f64, Vec3)]) -> Model {
    let mut model = Model::default();
    for (i, (inertia, at)) in parts.iter().enumerate() {
        model.bodies.push(Body::at_rest(1.0, Vec3::new(*inertia, *inertia, *inertia), *at));
        model.joints.push(Joint {
            kind: JointKind::Revolute,
            a: Marker::ground(*at, Vec3::Z, Vec3::X),
            b: Marker::new(Some(i), Vec3::ZERO, Vec3::Z, Vec3::X),
        });
    }
    model
}

fn value(s: &Sample, joint: usize, dof: usize) -> f64 {
    s.joints.get(joint).and_then(|j| j.values.get(dof)).copied().unwrap_or(f64::NAN)
}

#[test]
fn profiles_give_consistent_derivatives() {
    let profiles = [
        Profile::Constant { value: 2.0 },
        Profile::Linear { value: 1.0, rate: -3.0 },
        Profile::Ramp { from: 0.5, to: 2.0, start: 0.2, duration: 0.6 },
        Profile::Harmonic { offset: 1.0, amplitude: 0.3, frequency: 2.0, phase: 0.4 },
    ];
    let h = 1e-5;
    for p in &profiles {
        for k in 0..100 {
            let t = k as f64 * 0.0123;
            let (f, fd, fdd) = p.eval(t);
            let (fp, fdp, _) = p.eval(t + h);
            let (fm, fdm, _) = p.eval(t - h);
            assert!((fd - (fp - fm) / (2.0 * h)).abs() < 1e-5 * (1.0 + fd.abs()), "{p:?} at {t}: ḟ");
            assert!((fdd - (fdp - fdm) / (2.0 * h)).abs() < 1e-3 * (1.0 + fdd.abs()), "{p:?} at {t}: f̈");
            assert!(f.is_finite());
        }
    }
    let ramp = Profile::Ramp { from: 0.5, to: 2.0, start: 0.2, duration: 0.6 };
    assert_eq!(ramp.eval(0.0).0, 0.5);
    assert_eq!(ramp.eval(0.8).0, 2.0);
    assert!((ramp.eval(0.5).0 - 1.25).abs() < 1e-12);
    let table = Profile::Table { points: vec![(0.0, 1.0), (1.0, 3.0), (2.0, 0.0)] };
    assert_eq!(table.eval(-1.0), (1.0, 0.0, 0.0));
    assert_eq!(table.eval(0.5), (2.0, 2.0, 0.0));
    assert_eq!(table.eval(1.5), (1.5, -3.0, 0.0));
    assert_eq!(table.eval(9.0), (0.0, 0.0, 0.0));
}

#[test]
fn driven_pendulum_reaches_its_commanded_angle_and_holds_it() {
    // A drive lifts the pendulum from hanging to horizontal along a cycloidal ramp in 1 s,
    // then holds it: the angle follows the ramp, and the holding torque is m·g·L.
    let (len, mass) = (100.0, 1.0);
    let mut model = pendulum(len, 0.0, 0.0, 40.0);
    let ramp = Profile::Ramp { from: 0.0, to: PI / 2.0, start: 0.0, duration: 1.0 };
    model.drives.push(Drive { at: Coord::new(0, 0), motion: ramp.clone() });
    let traj = ok(simulate(&model, &Settings::rk4(1.5, 1e-3, 1e-2)));
    let b0 = traj.samples.first().map(|s| s.energy.balance()).unwrap_or(f64::NAN);
    for s in &traj.samples {
        let angle = value(s, 0, 0);
        assert!((angle - ramp.eval(s.time).0).abs() < 1e-9, "t={}: angle {angle} vs {}", s.time, ramp.eval(s.time).0);
        // Work put in = kinetic + gravity energy gained.
        assert!((s.energy.balance() - b0).abs() < 1e-6 * G * len, "t={}: balance {}", s.time, s.energy.balance() - b0);
    }
    let held: Vec<&Sample> = traj.samples.iter().filter(|s| s.time >= 1.0).collect();
    assert!(held.len() > 40);
    for s in held {
        let effort = s.drives.first().copied().unwrap_or(f64::NAN);
        assert!((effort.abs() - mass * G * len).abs() < 1e-6 * G * len, "t={}: holding torque {effort}", s.time);
        assert!((s.energy.input - mass * G * len).abs() < 1e-6 * G * len, "work put in {}", s.energy.input);
        assert!(s.energy.kinetic < 1e-9 * G * len);
    }
}

#[test]
fn driven_speed_counts_whole_turns() {
    // 10 rad/s for 2 s: more than three turns; the driven angle never jumps a turn.
    let mut model = rotors(&[(500.0, Vec3::ZERO)]);
    model.drives.push(Drive { at: Coord::new(0, 0), motion: Profile::Linear { value: 0.0, rate: 10.0 } });
    let traj = ok(simulate(&model, &Settings::rk4(2.0, 1e-3, 1e-2)));
    for s in &traj.samples {
        assert!(wrap(value(s, 0, 0) - 10.0 * s.time).abs() < 1e-9, "t={}", s.time);
        let w = s.bodies.first().map(|b| b.angular_velocity.z).unwrap_or(f64::NAN);
        assert!((w - 10.0).abs() < 1e-9);
        assert!(s.drives.first().is_some_and(|e| e.abs() < 1e-6), "{:?}", s.drives);
    }
    assert!(traj.stats.max_constraint_error < 1e-9);
}

#[test]
fn torsion_spring_period() {
    // ω = √(k/I) = √(5e5 / 500) rad/s; released from 0.3 rad.
    let (inertia, k) = (500.0_f64, 5e5_f64);
    let w = (k / inertia).sqrt();
    let mut model = rotors(&[(inertia, Vec3::ZERO)]);
    if let Some(b) = model.bodies.first_mut() {
        b.orientation = Quat::from_axis_angle(Vec3::Z, 0.3);
    }
    model.forces.push(Force::JointSpring { at: Coord::new(0, 0), stiffness: k, damping: 0.0, rest: 0.0 });
    let traj = ok(simulate(&model, &Settings::rk4(1.0, 1e-4, 1e-3)));
    for s in &traj.samples {
        let expect = 0.3 * (w * s.time).cos();
        assert!((value(s, 0, 0) - expect).abs() < 1e-7, "t={}: {} vs {expect}", s.time, value(s, 0, 0));
    }
    let measured = mean_period(&upward_crossings(&traj, |s| value(s, 0, 0)));
    assert!((measured - TAU / w).abs() < 1e-5 * TAU / w);
    let e0 = traj.samples.first().map(|s| s.energy.total()).unwrap_or(0.0);
    assert!((e0 - 0.5 * k * 0.09).abs() < 1e-9 * e0);
    // With damping the energy left plus the energy dissipated stays what it was.
    if let Some(Force::JointSpring { damping, .. }) = model.forces.first_mut() {
        *damping = 2.0 * 0.05 * (k * inertia).sqrt();
    }
    let traj = ok(simulate(&model, &Settings::rk4(1.0, 1e-4, 1e-2)));
    let last = traj.samples.last().map(|s| s.energy).unwrap_or_default();
    assert!(last.total() < 0.2 * e0 && last.dissipated > 0.8 * e0);
    assert!(traj.samples.iter().all(|s| (s.energy.balance() - e0).abs() < 1e-7 * e0));
}

#[test]
fn gear_pair_keeps_its_ratio_and_motor_work_is_kinetic_energy() {
    // A motor's constant torque τ on gear 1; gear 2 turns at −r₁/r₂ = −0.5 of it. The pair
    // accelerates as one inertia I₁ + 0.25·I₂: θ₁ = ½(τ/I_eff)t², and the motor's work τ·θ₁ is
    // all kinetic energy. Over 3 s gear 1 makes more than half a turn past ±π.
    let (i1, i2, ratio, tau) = (1000.0, 2000.0, -0.5, 1500.0);
    let alpha = tau / (i1 + ratio * ratio * i2);
    let mut model = rotors(&[(i1, Vec3::ZERO), (i2, Vec3::new(30.0, 0.0, 0.0))]);
    model.forces.push(Force::Motor { at: Coord::new(0, 0), effort: Profile::Constant { value: tau } });
    model.links.push(Link { driver: Coord::new(0, 0), follower: Coord::new(1, 0), ratio, offset: None });
    let traj = ok(simulate(&model, &Settings::rk4(3.0, 1e-3, 1e-2)));
    for s in &traj.samples {
        let theta = 0.5 * alpha * s.time * s.time;
        assert!(wrap(value(s, 0, 0) - theta).abs() < 1e-8, "t={}: gear 1 {} vs {theta}", s.time, value(s, 0, 0));
        assert!(wrap(value(s, 1, 0) - ratio * theta).abs() < 1e-8, "t={}: gear 2", s.time);
        assert!((s.energy.input - tau * theta).abs() < 1e-6 * tau * 4.5, "t={}: work {}", s.time, s.energy.input);
        assert!((s.energy.kinetic - s.energy.input).abs() < 1e-6 * tau * 4.5, "t={}: kinetic {}", s.time, s.energy.kinetic);
    }
    assert!(0.5 * alpha * 9.0 > PI);
}

#[test]
fn rack_and_pinion_follow_the_pinion_radius() {
    // A pinion of radius 20 mm drives a rack on a slider along X; a harmonic motor torque. The
    // rack's travel is 20·θ, and the motor's work is the kinetic energy of both.
    let mut model = rotors(&[(800.0, Vec3::ZERO)]);
    model.bodies.push(Body::at_rest(0.5, Vec3::new(100.0, 100.0, 100.0), Vec3::new(0.0, 20.0, 0.0)));
    model.joints.push(Joint {
        kind: JointKind::Slider,
        a: Marker::ground(Vec3::new(0.0, 20.0, 0.0), Vec3::X, Vec3::Y),
        b: Marker::new(Some(1), Vec3::ZERO, Vec3::X, Vec3::Y),
    });
    model
        .forces
        .push(Force::Motor { at: Coord::new(0, 0), effort: Profile::Harmonic { offset: 0.0, amplitude: 2000.0, frequency: 1.0, phase: 0.0 } });
    model.links.push(Link { driver: Coord::new(0, 0), follower: Coord::new(1, 0), ratio: 20.0, offset: None });
    let traj = ok(simulate(&model, &Settings::rk4(2.0, 1e-3, 1e-2)));
    // I_eff = 800 + 0.5·20²; θ̈ = τ(t)/I_eff with τ = A sin(2πt): θ = A/I_eff·(t/2π − sin(2πt)/4π²).
    let i_eff = 800.0 + 0.5 * 400.0;
    for s in &traj.samples {
        let t = s.time;
        let theta = 2000.0 / i_eff * (t / TAU - (TAU * t).sin() / (TAU * TAU));
        assert!((value(s, 0, 0) - theta).abs() < 1e-8, "t={t}: pinion {} vs {theta}", value(s, 0, 0));
        assert!((value(s, 1, 0) - 20.0 * theta).abs() < 1e-7, "t={t}: rack {}", value(s, 1, 0));
        assert!((s.energy.kinetic - s.energy.input).abs() < 1e-8 * 2000.0, "t={t}: {:?}", s.energy);
    }
}

#[test]
fn screw_turns_as_it_falls() {
    // A nut on a vertical cylindrical joint, its travel tied to its turn by a 5 mm pitch, falls
    // under gravity: z̈ = −m·g·k² / (I + m·k²) with k = pitch / 2π.
    let (mass, inertia, k) = (0.2, 50.0, 5.0 / TAU);
    let model = Model {
        bodies: vec![Body::at_rest(mass, Vec3::new(inertia, inertia, inertia), Vec3::ZERO)],
        joints: vec![Joint {
            kind: JointKind::Cylindrical,
            a: Marker::ground(Vec3::ZERO, Vec3::Z, Vec3::X),
            b: Marker::new(Some(0), Vec3::ZERO, Vec3::Z, Vec3::X),
        }],
        links: vec![Link { driver: Coord::new(0, 0), follower: Coord::new(0, 1), ratio: k, offset: None }],
        gravity: down(),
        ..Model::default()
    };
    let traj = ok(simulate(&model, &Settings::rk4(1.0, 1e-3, 1e-2)));
    let a = -mass * G * k * k / (inertia + mass * k * k);
    for s in &traj.samples {
        let z = s.bodies.first().map(|b| b.position.z).unwrap_or(f64::NAN);
        assert!((z - 0.5 * a * s.time * s.time).abs() < 1e-7, "t={}: z {z}", s.time);
        assert!(wrap(value(s, 0, 0) - z / k).abs() < 1e-8, "t={}: turn", s.time);
        assert!(s.energy.balance().abs() < 1e-6 * mass * G, "t={}: {:?}", s.time, s.energy);
    }
    // It went down more than one turn.
    assert!(0.5 * a < -5.0);
}

#[test]
fn hostile_springs_motors_drives_and_links_are_errors() {
    let good = rotors(&[(500.0, Vec3::ZERO), (500.0, Vec3::new(30.0, 0.0, 0.0))]);
    let mut bad: Vec<Model> = Vec::new();
    let mut with = |f: &dyn Fn(&mut Model)| {
        let mut m = good.clone();
        f(&mut m);
        bad.push(m);
    };
    with(&|m| m.drives.push(Drive { at: Coord::new(5, 0), motion: Profile::Constant { value: 0.0 } }));
    with(&|m| m.drives.push(Drive { at: Coord::new(0, 1), motion: Profile::Constant { value: 0.0 } }));
    with(&|m| m.drives.push(Drive { at: Coord::new(0, 0), motion: Profile::Constant { value: f64::NAN } }));
    with(&|m| m.drives.push(Drive { at: Coord::new(0, 0), motion: Profile::Ramp { from: 0.0, to: 1.0, start: 0.0, duration: 0.0 } }));
    with(&|m| m.drives.push(Drive { at: Coord::new(0, 0), motion: Profile::Table { points: vec![] } }));
    with(&|m| m.drives.push(Drive { at: Coord::new(0, 0), motion: Profile::Table { points: vec![(1.0, 0.0), (0.5, 1.0)] } }));
    with(&|m| m.drives.push(Drive { at: Coord::new(0, 0), motion: Profile::Table { points: vec![(0.0, 0.0); MAX_PROFILE_POINTS + 1] } }));
    with(&|m| m.forces.push(Force::JointSpring { at: Coord::new(0, 0), stiffness: -1.0, damping: 0.0, rest: 0.0 }));
    with(&|m| m.forces.push(Force::JointSpring { at: Coord::new(0, 0), stiffness: 1.0, damping: f64::INFINITY, rest: 0.0 }));
    with(&|m| m.forces.push(Force::Motor { at: Coord::new(9, 0), effort: Profile::Constant { value: 1.0 } }));
    with(&|m| {
        m.forces
            .push(Force::Motor { at: Coord::new(0, 0), effort: Profile::Harmonic { offset: 0.0, amplitude: 1.0, frequency: f64::NAN, phase: 0.0 } })
    });
    with(&|m| m.links.push(Link { driver: Coord::new(0, 0), follower: Coord::new(0, 0), ratio: 1.0, offset: None }));
    with(&|m| m.links.push(Link { driver: Coord::new(0, 0), follower: Coord::new(1, 0), ratio: f64::NAN, offset: None }));
    with(&|m| m.links.push(Link { driver: Coord::new(0, 0), follower: Coord::new(1, 0), ratio: 1.0, offset: Some(f64::INFINITY) }));
    with(&|m| m.links = vec![Link { driver: Coord::new(0, 0), follower: Coord::new(1, 0), ratio: 1.0, offset: None }; MAX_JOINTS + 1]);
    with(&|m| {
        // Ball and rigid joints have no value to act on yet.
        if let Some(j) = m.joints.get_mut(0) {
            j.kind = JointKind::Ball;
        }
        m.drives.push(Drive { at: Coord::new(0, 0), motion: Profile::Constant { value: 0.0 } });
    });
    for (i, m) in bad.iter().enumerate() {
        assert!(simulate(m, &Settings::rk4(0.1, 1e-3, 1e-2)).is_err(), "case {i} should be refused");
    }
    // Links that fight each other, and drives that can't be met, stop or finish but never panic.
    let mut locked = good.clone();
    locked.links.push(Link { driver: Coord::new(0, 0), follower: Coord::new(1, 0), ratio: 1.0, offset: None });
    locked.links.push(Link { driver: Coord::new(1, 0), follower: Coord::new(0, 0), ratio: 2.0, offset: None });
    locked.drives.push(Drive { at: Coord::new(0, 0), motion: Profile::Harmonic { offset: 0.0, amplitude: 1e6, frequency: 1e3, phase: 0.0 } });
    let _ = simulate(&locked, &Settings::rk4(0.1, 1e-3, 1e-2));
    let _ = simulate(&locked, &Settings::adaptive(0.1, 1e-2, 1e-9, 1e-9));
}