Skip to content

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

33.1 KB
Raw Blame
//! The equations of motion: body states, joint constraint equations with their Jacobians,
//! applied forces, the constrained accelerations and the projections back onto the joints.
//!
//! Bodies use maximal coordinates: centre-of-mass position `x`, unit quaternion `q`, velocity
//! `v` and angular velocity `ω` (both world). With `u = (v, ω)` per body, mass matrix
//! `M = diag(m·I₃, I_w)` (`I_w = R I_body Rᵀ`), joint equations `Φ(x, q) = 0`, their Jacobian
//! `J` (`Φ̇ = J u`) and `γ = −J̇ u`, the accelerations solve
//!
//! ```text
//! M u̇ + Jᵀ λ = F − ω × I_w ω,      J u̇ = γ
//! ```
//!
//! through the Schur complement `(J M⁻¹ Jᵀ) λ = J M⁻¹ F − γ` (`linalg::solve_psd`, which takes
//! repeated joint equations), and `λ` gives the joints' reactions.
//!
//! Joint springs and motors act along joint values (generalized forces `Jᵀ Q` along a value's
//! row); drives add a rheonomic equation `value − f(t) = 0` (so `J u = ḟ`, `J u̇ = f̈ − J̇ u`),
//! and motion links a linear one between two values. The values they use are also kept in the
//! state, so an angle counts whole turns.

use std::f64::consts::TAU;

use solidcore_record::Vec3;
use solidcraft_geom::prelude::*;

use crate::linalg::{Sym, solve_psd};
use crate::math::{Mat3, Quat, cross2};
use crate::model::{Coord, Force, JointKind, MAX_MAGNITUDE, MAX_PROJECTION_ITERATIONS, Model, Profile, marker_axes};
use crate::{BodyState, DynamicsError, Energy, JointResult, Result};

/// Numbers per body in the flat state: x (3), q (4), v (3), ω (3).
pub(crate) const STRIDE: usize = 13;

/// Pivots under this fraction of the largest are taken as repeated equations.
const RANK_TOL: f64 = 1e-10;

fn v3(y: &[f64], at: usize) -> Vec3 {
    let g = |i: usize| y.get(at + i).copied().unwrap_or(0.0);
    Vec3::new(g(0), g(1), g(2))
}
fn put3(y: &mut [f64], at: usize, v: Vec3) {
    for (i, c) in [v.x, v.y, v.z].into_iter().enumerate() {
        if let Some(s) = y.get_mut(at + i) {
            *s = c;
        }
    }
}
fn quat(y: &[f64], at: usize) -> Quat {
    let g = |i: usize| y.get(at + i).copied().unwrap_or(0.0);
    Quat { w: g(0), x: g(1), y: g(2), z: g(3) }
}
fn put_quat(y: &mut [f64], at: usize, q: Quat) {
    for (i, c) in [q.w, q.x, q.y, q.z].into_iter().enumerate() {
        if let Some(s) = y.get_mut(at + i) {
            *s = c;
        }
    }
}

/// One body's precomputed constants.
struct BodyConst {
    mass: f64,
    inertia: Mat3,
    inertia_inv: Mat3,
}

/// A joint frame: its body, origin and axes in the body's coordinates.
struct MarkerConst {
    body: Option<usize>,
    origin: Vec3,
    axes: Mat3,
}

struct JointConst {
    kind: JointKind,
    a: MarkerConst,
    b: MarkerConst,
}

/// A body's kinematics at one state.
#[derive(Clone, Copy)]
struct Kin {
    x: Vec3,
    rot: Mat3,
    v: Vec3,
    w: Vec3,
}

const GROUND: Kin = Kin { x: Vec3::ZERO, rot: Mat3::IDENTITY, v: Vec3::ZERO, w: Vec3::ZERO };

/// A joint frame at one state (world).
struct MarkerWorld {
    body: Option<usize>,
    /// Origin minus the body's centre of mass.
    r: Vec3,
    /// Origin.
    p: Vec3,
    /// Velocity of the origin.
    pdot: Vec3,
    axes: [Vec3; 3],
    w: Vec3,
}

/// A body's part of one Jacobian row: (linear, angular).
type Block = (usize, Vec3, Vec3);

/// One constraint equation at a state: `Φ = 0`, `J u = rate_target`.
pub(crate) struct Row {
    /// A's and B's parts (for a joint's own equations, its two sides).
    a: Option<Block>,
    b: Option<Block>,
    /// A motion link's driver's parts.
    c: Option<Block>,
    d: Option<Block>,
    pub(crate) phi: f64,
    /// `J̇ u` less the imposed acceleration: the equation's second derivative at zero
    /// accelerations.
    jdot_u: f64,
    /// The rate a drive imposes (`ḟ`); 0 for the others.
    rate_target: f64,
}

fn scaled(b: Option<Block>, k: f64) -> Option<Block> {
    b.map(|(i, l, a)| (i, l * k, a * k))
}

impl Row {
    fn new(a: Option<Block>, b: Option<Block>, phi: f64, jdot_u: f64) -> Row {
        Row { a, b, c: None, d: None, phi, jdot_u, rate_target: 0.0 }
    }
    fn blocks(&self) -> impl Iterator<Item = &Block> {
        self.a.iter().chain(self.b.iter()).chain(self.c.iter()).chain(self.d.iter())
    }
    /// `J u`.
    fn rate(&self, kin: &[Kin]) -> f64 {
        self.blocks().map(|(i, l, a)| kin.get(*i).map_or(0.0, |k| l.dot(k.v) + a.dot(k.w))).sum()
    }
}

/// How a joint value is measured.
#[derive(Clone, Copy, PartialEq)]
enum CoordKind {
    /// B's turn about A's z (from A's x towards A's y).
    Angle,
    /// B's origin along one of A's axes (0, 1, 2 = x, y, z).
    Along(usize),
}

/// A joint value springs, motors, drives or links act on. Its value is kept in the state too,
/// so an angle counts whole turns.
#[derive(Clone, Copy)]
struct CoordConst {
    joint: usize,
    kind: CoordKind,
}

struct LinkConst {
    driver: usize,
    follower: usize,
    ratio: f64,
    offset: f64,
}

/// A model ready to integrate.
///
/// The flat state holds every body (`STRIDE` numbers each), then the tracked joint values,
/// then the work put in (motors, drives, loads) and the energy dissipated (dampers).
pub(crate) struct System {
    bodies: Vec<BodyConst>,
    joints: Vec<JointConst>,
    /// Point springs and loads.
    forces: Vec<Force>,
    gravity: Vec3,
    baumgarte: Option<(f64, f64)>,
    coords: Vec<CoordConst>,
    /// (value, stiffness, damping, rest).
    springs: Vec<(usize, f64, f64, f64)>,
    /// (value, effort).
    motors: Vec<(usize, Profile)>,
    /// (value, motion).
    drives: Vec<(usize, Profile)>,
    links: Vec<LinkConst>,
}

/// `θ` turned by whole turns to lie nearest `near`.
fn unwrap_angle(theta: f64, near: f64) -> f64 {
    if !near.is_finite() {
        return theta;
    }
    let turns = ((near - theta) / TAU).round();
    if turns.is_finite() { theta + TAU * turns } else { theta }
}

/// Accelerations and multipliers at one state.
pub(crate) struct Accel {
    /// The flat state's derivative.
    pub(crate) ydot: Vec<f64>,
    /// One multiplier per constraint row, in joint order.
    lambda: Vec<f64>,
    rows: Vec<Row>,
    /// The rank of `J M⁻¹ Jᵀ`.
    pub(crate) rank: usize,
}

impl Accel {
    /// The number of joint equations.
    pub(crate) fn row_count(&self) -> usize {
        self.rows.len()
    }
}

fn marker_const(m: &crate::Marker) -> Result<MarkerConst> {
    let axes = marker_axes(m).ok_or_else(|| DynamicsError::InvalidModel("a joint frame's axes are not usable".into()))?;
    Ok(MarkerConst { body: m.body, origin: m.origin, axes })
}

impl System {
    pub(crate) fn new(model: &Model, baumgarte: Option<(f64, f64)>) -> Result<System> {
        model.validate()?;
        let bodies = model
            .bodies
            .iter()
            .map(|b| {
                let inertia_inv = b.inertia.spd_inverse().ok_or_else(|| DynamicsError::InvalidModel("inertia is not positive definite".into()))?;
                Ok(BodyConst { mass: b.mass, inertia: b.inertia, inertia_inv })
            })
            .collect::<Result<Vec<_>>>()?;
        let joints = model
            .joints
            .iter()
            .map(|j| Ok(JointConst { kind: j.kind, a: marker_const(&j.a)?, b: marker_const(&j.b)? }))
            .collect::<Result<Vec<_>>>()?;
        let mut sys = System {
            bodies,
            joints,
            forces: model.forces.iter().filter(|f| matches!(f, Force::Spring { .. } | Force::Load { .. })).cloned().collect(),
            gravity: model.gravity,
            baumgarte,
            coords: Vec::new(),
            springs: Vec::new(),
            motors: Vec::new(),
            drives: Vec::new(),
            links: Vec::new(),
        };
        let mut found: Vec<Coord> = Vec::new();
        let mut index = |c: Coord, sys: &mut System| -> Result<usize> {
            if let Some(i) = found.iter().position(|f| *f == c) {
                return Ok(i);
            }
            let kind = model.joints.get(c.joint).map(|j| j.kind);
            let kind = match (kind, c.dof) {
                (Some(JointKind::Revolute | JointKind::Cylindrical), 0) | (Some(JointKind::Planar), 2) => CoordKind::Angle,
                (Some(JointKind::Slider), 0) | (Some(JointKind::Cylindrical), 1) => CoordKind::Along(2),
                (Some(JointKind::Planar), d @ (0 | 1)) => CoordKind::Along(d),
                _ => return Err(DynamicsError::InvalidModel(format!("joint {} has no value {} to act on", c.joint, c.dof))),
            };
            found.push(c);
            sys.coords.push(CoordConst { joint: c.joint, kind });
            Ok(found.len() - 1)
        };
        for f in &model.forces {
            match f {
                Force::JointSpring { at, stiffness, damping, rest } => {
                    let i = index(*at, &mut sys)?;
                    sys.springs.push((i, *stiffness, *damping, *rest));
                }
                Force::Motor { at, effort } => {
                    let i = index(*at, &mut sys)?;
                    sys.motors.push((i, effort.clone()));
                }
                Force::Spring { .. } | Force::Load { .. } => {}
            }
        }
        for d in &model.drives {
            let i = index(d.at, &mut sys)?;
            sys.drives.push((i, d.motion.clone()));
        }
        let mut pending = Vec::new();
        for l in &model.links {
            pending.push((index(l.driver, &mut sys)?, index(l.follower, &mut sys)?, l.ratio, l.offset));
        }
        // A link without an offset keeps the one that holds at the start.
        let y = sys.initial_state(model)?;
        let kin = sys.kinematics(&y);
        let value = |i: usize| sys.coord_row(&kin, &y, i).phi;
        let links = pending
            .into_iter()
            .map(|(d, f, ratio, offset)| LinkConst { driver: d, follower: f, ratio, offset: offset.unwrap_or_else(|| value(f) - ratio * value(d)) })
            .collect();
        sys.links = links;
        Ok(sys)
    }

    fn coord_base(&self) -> usize {
        self.bodies.len() * STRIDE
    }
    fn work_base(&self) -> usize {
        self.coord_base() + self.coords.len()
    }
    /// Numbers in the flat state.
    pub(crate) fn state_len(&self) -> usize {
        self.work_base() + 2
    }

    /// The flat state of the model's starting conditions.
    pub(crate) fn initial_state(&self, model: &Model) -> Result<Vec<f64>> {
        let mut y = vec![0.0; self.state_len()];
        for (i, b) in model.bodies.iter().enumerate() {
            let at = i * STRIDE;
            let q = b.orientation.normalized().ok_or_else(|| DynamicsError::InvalidModel(format!("body {i}: orientation is not a rotation")))?;
            put3(&mut y, at, b.position);
            put_quat(&mut y, at + 3, q);
            put3(&mut y, at + 7, b.velocity);
            put3(&mut y, at + 10, b.angular_velocity);
        }
        // A driven angle counts its turns from where its motion starts.
        for (i, motion) in &self.drives {
            if let Some(s) = y.get_mut(self.coord_base() + i) {
                *s = motion.eval(0.0).0;
            }
        }
        self.resync(&mut y);
        Ok(y)
    }

    /// Sets each tracked joint value to the one the bodies show (an angle: the turn nearest
    /// the tracked one).
    pub(crate) fn resync(&self, y: &mut [f64]) {
        let kin = self.kinematics(y);
        let values: Vec<f64> = (0..self.coords.len()).map(|i| self.coord_row(&kin, y, i).phi).collect();
        for (i, v) in values.into_iter().enumerate() {
            if let Some(s) = y.get_mut(self.coord_base() + i) {
                *s = v;
            }
        }
    }

    /// A tracked joint value's equation row: `Φ` is the value itself, `J u` its rate.
    fn coord_row(&self, kin: &[Kin], y: &[f64], i: usize) -> Row {
        let (Some(c), tracked) = (self.coords.get(i), y.get(self.coord_base() + i).copied().unwrap_or(f64::NAN)) else {
            return Row::new(None, None, 0.0, 0.0);
        };
        let Some(j) = self.joints.get(c.joint) else { return Row::new(None, None, 0.0, 0.0) };
        let a = Self::marker_world(kin, &j.a);
        let b = Self::marker_world(kin, &j.b);
        let [ax, ay, az] = a.axes;
        match c.kind {
            CoordKind::Angle => {
                let bx = b.axes[0];
                let theta = unwrap_angle(bx.dot(ay).atan2(bx.dot(ax)), tracked);
                // θ̇ = (ω_B − ω_A)·z_A; at zero accelerations its rate is (ω_B − ω_A)·(ω_A × z_A).
                Row::new(a.body.map(|k| (k, Vec3::ZERO, -az)), b.body.map(|k| (k, Vec3::ZERO, az)), theta, (b.w - a.w).dot(a.w.cross(az)))
            }
            CoordKind::Along(k) => dot2_row(&a, a.axes.get(k).copied().unwrap_or(az), &b),
        }
    }

    /// Unit quaternions again after a step.
    pub(crate) fn normalize(&self, y: &mut [f64]) -> Result<()> {
        for i in 0..self.bodies.len() {
            let q = quat(y, i * STRIDE + 3).normalized().ok_or(DynamicsError::Diverged { time: f64::NAN })?;
            put_quat(y, i * STRIDE + 3, q);
        }
        Ok(())
    }

    fn kinematics(&self, y: &[f64]) -> Vec<Kin> {
        (0..self.bodies.len())
            .map(|i| {
                let at = i * STRIDE;
                Kin { x: v3(y, at), rot: quat(y, at + 3).to_mat3(), v: v3(y, at + 7), w: v3(y, at + 10) }
            })
            .collect()
    }

    fn kin_of(kin: &[Kin], body: Option<usize>) -> Kin {
        body.and_then(|b| kin.get(b).copied()).unwrap_or(GROUND)
    }

    fn marker_world(kin: &[Kin], m: &MarkerConst) -> MarkerWorld {
        let k = Self::kin_of(kin, m.body);
        let r = k.rot.mul_vec(m.origin);
        let axes = [k.rot.mul_vec(m.axes.col(0)), k.rot.mul_vec(m.axes.col(1)), k.rot.mul_vec(m.axes.col(2))];
        MarkerWorld { body: m.body, r, p: k.x + r, pdot: k.v + k.w.cross(r), axes, w: k.w }
    }

    /// The constraint equations at time `t`: every joint's in joint order, then one per drive
    /// (`value − f(t) = 0`), then one per link (`follower − ratio·driver − offset = 0`).
    fn rows(&self, kin: &[Kin], t: f64, y: &[f64]) -> Vec<Row> {
        let mut rows = Vec::new();
        for j in &self.joints {
            let a = Self::marker_world(kin, &j.a);
            let b = Self::marker_world(kin, &j.b);
            joint_rows(j.kind, &a, &b, &mut rows);
        }
        for (i, motion) in &self.drives {
            let (f, fd, fdd) = motion.eval(t);
            let mut r = self.coord_row(kin, y, *i);
            r.phi -= f;
            r.jdot_u -= fdd;
            r.rate_target = fd;
            rows.push(r);
        }
        for l in &self.links {
            let (f, d) = (self.coord_row(kin, y, l.follower), self.coord_row(kin, y, l.driver));
            rows.push(Row {
                a: f.a,
                b: f.b,
                c: scaled(d.a, -l.ratio),
                d: scaled(d.b, -l.ratio),
                phi: f.phi - l.ratio * d.phi - l.offset,
                jdot_u: f.jdot_u - l.ratio * d.jdot_u,
                rate_target: 0.0,
            });
        }
        rows
    }

    fn joint_row_count(&self) -> usize {
        self.joints.iter().map(|j| j.kind.equations()).sum()
    }

    /// `M⁻¹` applied to one body's (force, torque).
    fn minv(&self, kin: &[Kin], body: usize, f: Vec3, t: Vec3) -> (Vec3, Vec3) {
        match (self.bodies.get(body), kin.get(body)) {
            (Some(b), Some(k)) => {
                let iw_inv = k.rot.mul(&b.inertia_inv).mul(&k.rot.transpose());
                (f * (1.0 / b.mass), iw_inv.mul_vec(t))
            }
            _ => (Vec3::ZERO, Vec3::ZERO),
        }
    }

    /// `J M⁻¹ Jᵀ`, and each row's `M⁻¹ Jᵀ` blocks.
    fn schur(&self, kin: &[Kin], rows: &[Row]) -> (Sym, Vec<Vec<Block>>) {
        let w: Vec<Vec<Block>> = rows
            .iter()
            .map(|r| {
                r.blocks()
                    .map(|(i, l, a)| {
                        let (ml, ma) = self.minv(kin, *i, *l, *a);
                        (*i, ml, ma)
                    })
                    .collect()
            })
            .collect();
        let n = rows.len();
        let mut s = Sym::zeros(n);
        for (r, row) in rows.iter().enumerate() {
            for (c, wc) in w.iter().enumerate().skip(r) {
                let mut sum = 0.0;
                for (i, l, a) in row.blocks() {
                    for (k, ml, ma) in wc {
                        if i == k {
                            sum += l.dot(*ml) + a.dot(*ma);
                        }
                    }
                }
                s.set(r, c, sum);
                s.set(c, r, sum);
            }
        }
        (s, w)
    }

    /// Applied forces and torques (about each centre of mass, world), gyroscopic terms included,
    /// and the power they put in and the power dampers take out.
    fn applied(&self, kin: &[Kin], t: f64, y: &[f64]) -> (Vec<(Vec3, Vec3)>, f64, f64) {
        let mut input = 0.0;
        let mut dissipated = 0.0;
        let mut out = self.point_forces(kin, &mut input, &mut dissipated);
        // A generalized force Q along a joint value is Jᵀ Q on the bodies.
        let mut along = |row: &Row, q: f64| {
            for (i, l, a) in row.blocks() {
                if let Some(slot) = out.get_mut(*i) {
                    slot.0 += *l * q;
                    slot.1 += *a * q;
                }
            }
        };
        for (i, k, c, rest) in &self.springs {
            let row = self.coord_row(kin, y, *i);
            let rate = row.rate(kin);
            along(&row, -k * (row.phi - rest) - c * rate);
            dissipated += c * rate * rate;
        }
        for (i, effort) in &self.motors {
            let row = self.coord_row(kin, y, *i);
            let q = effort.eval(t).0;
            along(&row, q);
            input += q * row.rate(kin);
        }
        (out, input, dissipated)
    }

    /// Gravity, gyroscopic terms, point springs and loads.
    fn point_forces(&self, kin: &[Kin], input: &mut f64, dissipated: &mut f64) -> Vec<(Vec3, Vec3)> {
        let mut out: Vec<(Vec3, Vec3)> = self
            .bodies
            .iter()
            .zip(kin)
            .map(|(b, k)| {
                let iw = k.rot.mul(&b.inertia).mul(&k.rot.transpose());
                (self.gravity * b.mass, -k.w.cross(iw.mul_vec(k.w)))
            })
            .collect();
        let mut add = |body: Option<usize>, r: Vec3, f: Vec3, t: Vec3| {
            if let Some(slot) = body.and_then(|b| out.get_mut(b)) {
                slot.0 += f;
                slot.1 += r.cross(f) + t;
            }
        };
        for f in &self.forces {
            match *f {
                Force::Spring { a, b, stiffness, damping, free_length, preload } => {
                    let (ka, kb) = (Self::kin_of(kin, a.body), Self::kin_of(kin, b.body));
                    let (ra, rb) = (ka.rot.mul_vec(a.point), kb.rot.mul_vec(b.point));
                    let d = (kb.x + rb) - (ka.x + ra);
                    let len = d.len();
                    let Some(n) = d.normalized().filter(|_| len > 1e-9) else { continue };
                    let rate = n.dot((kb.v + kb.w.cross(rb)) - (ka.v + ka.w.cross(ra)));
                    let tension = stiffness * (len - free_length) + damping * rate + preload;
                    add(a.body, ra, n * tension, Vec3::ZERO);
                    add(b.body, rb, n * -tension, Vec3::ZERO);
                    *dissipated += damping * rate * rate;
                }
                Force::Load { body, point, force, torque } => {
                    let k = kin.get(body).copied().unwrap_or(GROUND);
                    let r = k.rot.mul_vec(point);
                    add(Some(body), r, force, torque);
                    *input += force.dot(k.v + k.w.cross(r)) + torque.dot(k.w);
                }
                Force::JointSpring { .. } | Force::Motor { .. } => {}
            }
        }
        out
    }

    /// The state's derivative and the joints' multipliers.
    pub(crate) fn accelerations(&self, t: f64, y: &[f64]) -> Result<Accel> {
        let kin = self.kinematics(y);
        let (applied, mut input, dissipated) = self.applied(&kin, t, y);
        let rows = self.rows(&kin, t, y);
        let (s, w) = self.schur(&kin, &rows);
        // Unconstrained accelerations M⁻¹F.
        let free: Vec<(Vec3, Vec3)> = applied.iter().enumerate().map(|(i, (f, t))| self.minv(&kin, i, *f, *t)).collect();
        let rhs: Vec<f64> = rows
            .iter()
            .map(|r| {
                let jmf: f64 = r.blocks().map(|(i, l, a)| free.get(*i).map_or(0.0, |(fl, fa)| l.dot(*fl) + a.dot(*fa))).sum();
                let stab = self.baumgarte.map_or(0.0, |(alpha, beta)| 2.0 * alpha * (r.rate(&kin) - r.rate_target) + beta * beta * r.phi);
                jmf + r.jdot_u + stab
            })
            .collect();
        let (lambda, rank) =
            if rows.is_empty() { (Vec::new(), 0) } else { solve_psd(s, &rhs, RANK_TOL).ok_or(DynamicsError::Singular { time: f64::NAN })? };
        let mut acc = free;
        for (wr, l) in w.iter().zip(&lambda) {
            for (i, ml, ma) in wr {
                if let Some(a) = acc.get_mut(*i) {
                    a.0 -= *ml * *l;
                    a.1 -= *ma * *l;
                }
            }
        }
        let mut ydot = vec![0.0; y.len()];
        for (i, (k, a)) in kin.iter().zip(&acc).enumerate() {
            let at = i * STRIDE;
            put3(&mut ydot, at, k.v);
            put_quat(&mut ydot, at + 3, quat(y, at + 3).rate(k.w));
            put3(&mut ydot, at + 7, a.0);
            put3(&mut ydot, at + 10, a.1);
        }
        for i in 0..self.coords.len() {
            let rate = self.coord_row(&kin, y, i).rate(&kin);
            if let Some(s) = ydot.get_mut(self.coord_base() + i) {
                *s = rate;
            }
        }
        // A drive's generalized force is −λ along its value: its power is −λ·(value rate).
        let first_drive = self.joint_row_count();
        for k in 0..self.drives.len() {
            if let (Some(r), Some(l)) = (rows.get(first_drive + k), lambda.get(first_drive + k)) {
                input -= l * r.rate(&kin);
            }
        }
        let wb = self.work_base();
        for (k, p) in [input, dissipated].into_iter().enumerate() {
            if let Some(s) = ydot.get_mut(wb + k) {
                *s = p;
            }
        }
        if !ydot.iter().all(|x| x.is_finite()) {
            return Err(DynamicsError::Diverged { time: f64::NAN });
        }
        Ok(Accel { ydot, lambda, rows, rank })
    }

    /// What each drive applies along its value (mN·mm or mN), from the multipliers.
    pub(crate) fn drive_efforts(&self, acc: &Accel) -> Vec<f64> {
        let first = self.joint_row_count();
        (0..self.drives.len()).map(|k| -acc.lambda.get(first + k).copied().unwrap_or(0.0)).collect()
    }

    /// The largest constraint equation error at `y` and time `t`.
    pub(crate) fn constraint_error(&self, t: f64, y: &[f64]) -> f64 {
        self.rows(&self.kinematics(y), t, y).iter().map(|r| r.phi.abs()).fold(0.0, f64::max)
    }

    /// Moves the bodies the least (mass-weighted) onto the joints: Gauss–Newton on `Φ = 0`.
    /// Returns the error left.
    pub(crate) fn project_positions(&self, t: f64, y: &mut [f64], tol: f64) -> Result<f64> {
        let mut err;
        let mut iteration = 0;
        loop {
            let kin = self.kinematics(y);
            let rows = self.rows(&kin, t, y);
            err = rows.iter().map(|r| r.phi.abs()).fold(0.0, f64::max);
            if !(err > tol) || iteration >= MAX_PROJECTION_ITERATIONS {
                break;
            }
            iteration += 1;
            let (s, w) = self.schur(&kin, &rows);
            let phi: Vec<f64> = rows.iter().map(|r| r.phi).collect();
            let (mu, _) = solve_psd(s, &phi, RANK_TOL).ok_or(DynamicsError::Singular { time: f64::NAN })?;
            let mut delta = vec![(Vec3::ZERO, Vec3::ZERO); self.bodies.len()];
            for (wr, m) in w.iter().zip(&mu) {
                for (i, ml, ma) in wr {
                    if let Some(d) = delta.get_mut(*i) {
                        d.0 -= *ml * *m;
                        d.1 -= *ma * *m;
                    }
                }
            }
            for (i, (dx, dth)) in delta.into_iter().enumerate() {
                let at = i * STRIDE;
                put3(y, at, v3(y, at) + dx);
                let q = Quat::from_rotation_vector(dth).compose(quat(y, at + 3)).normalized().ok_or(DynamicsError::Diverged { time: f64::NAN })?;
                put_quat(y, at + 3, q);
            }
        }
        if !err.is_finite() || !y.iter().all(|x| x.is_finite() && x.abs() < MAX_MAGNITUDE) {
            return Err(DynamicsError::Diverged { time: f64::NAN });
        }
        Ok(err)
    }

    /// Removes the velocities the joints don't allow (mass-weighted): `J u = ḟ` (0 but for
    /// drives).
    pub(crate) fn project_velocities(&self, t: f64, y: &mut [f64]) -> Result<()> {
        let kin = self.kinematics(y);
        let rows = self.rows(&kin, t, y);
        if rows.is_empty() {
            return Ok(());
        }
        let (s, w) = self.schur(&kin, &rows);
        let ju: Vec<f64> = rows.iter().map(|r| r.rate(&kin) - r.rate_target).collect();
        let (mu, _) = solve_psd(s, &ju, RANK_TOL).ok_or(DynamicsError::Singular { time: f64::NAN })?;
        for (wr, m) in w.iter().zip(&mu) {
            for (i, ml, ma) in wr {
                let at = i * STRIDE;
                put3(y, at + 7, v3(y, at + 7) - *ml * *m);
                put3(y, at + 10, v3(y, at + 10) - *ma * *m);
            }
        }
        Ok(())
    }

    /// The bodies' states.
    pub(crate) fn body_states(&self, y: &[f64]) -> Vec<BodyState> {
        (0..self.bodies.len())
            .map(|i| {
                let at = i * STRIDE;
                BodyState { position: v3(y, at), orientation: quat(y, at + 3), velocity: v3(y, at + 7), angular_velocity: v3(y, at + 10) }
            })
            .collect()
    }

    /// Kinetic, gravity and spring energy, and the work put in and dissipated so far.
    pub(crate) fn energy(&self, y: &[f64]) -> Energy {
        let kin = self.kinematics(y);
        let mut e = Energy::default();
        for (b, k) in self.bodies.iter().zip(&kin) {
            let iw = k.rot.mul(&b.inertia).mul(&k.rot.transpose());
            e.kinetic += 0.5 * b.mass * k.v.len2() + 0.5 * k.w.dot(iw.mul_vec(k.w));
            e.gravity -= b.mass * self.gravity.dot(k.x);
        }
        for f in &self.forces {
            if let Force::Spring { a, b, stiffness, free_length, preload, .. } = *f {
                let (ka, kb) = (Self::kin_of(&kin, a.body), Self::kin_of(&kin, b.body));
                let stretch = ((kb.x + kb.rot.mul_vec(b.point)) - (ka.x + ka.rot.mul_vec(a.point))).len() - free_length;
                e.spring += 0.5 * stiffness * stretch * stretch + preload * stretch;
            }
        }
        for (i, k, _, rest) in &self.springs {
            let stretch = self.coord_row(&kin, y, *i).phi - rest;
            e.spring += 0.5 * k * stretch * stretch;
        }
        let wb = self.work_base();
        e.input = y.get(wb).copied().unwrap_or(0.0);
        e.dissipated = y.get(wb + 1).copied().unwrap_or(0.0);
        e
    }

    /// Each joint's values, rates and reaction (on `b`'s side, at its frame's origin, world),
    /// from the multipliers `acc` found at `y`.
    pub(crate) fn joint_results(&self, y: &[f64], acc: &Accel) -> Vec<JointResult> {
        let kin = self.kinematics(y);
        let mut at = 0;
        let mut out = Vec::with_capacity(self.joints.len());
        for j in &self.joints {
            let a = Self::marker_world(&kin, &j.a);
            let b = Self::marker_world(&kin, &j.b);
            let (values, rates) = joint_values(j.kind, &a, &b);
            let n = j.kind.equations();
            // The wrench the joint's equations put on each body: −Jᵀλ.
            let (mut fa, mut ta, mut fb, mut tb) = (Vec3::ZERO, Vec3::ZERO, Vec3::ZERO, Vec3::ZERO);
            for k in at..at + n {
                let (Some(row), Some(l)) = (acc.rows.get(k), acc.lambda.get(k)) else { continue };
                if let Some((_, lin, ang)) = row.a {
                    fa -= lin * *l;
                    ta -= ang * *l;
                }
                if let Some((_, lin, ang)) = row.b {
                    fb -= lin * *l;
                    tb -= ang * *l;
                }
            }
            at += n;
            let (force, torque) = if b.body.is_some() {
                (fb, tb - b.r.cross(fb))
            } else {
                // B is the ground: it takes what A takes, turned round, about B's origin.
                let xa = Self::kin_of(&kin, a.body).x;
                (-fa, -(ta + (xa - b.p).cross(fa)))
            };
            out.push(JointResult { values, rates, force, torque });
        }
        out
    }
}

/// `(Φ, J, J̇u)` for one body point's world component along `e`: `e·(p_B − p_A)`.
fn point_row(a: &MarkerWorld, b: &MarkerWorld, e: Vec3) -> Row {
    let acc = |m: &MarkerWorld| cross2(m.w, m.r);
    Row::new(a.body.map(|i| (i, -e, -a.r.cross(e))), b.body.map(|i| (i, e, b.r.cross(e))), e.dot(b.p - a.p), e.dot(acc(b) - acc(a)))
}

/// `u·v = 0` for a direction `u` of A and `v` of B.
fn dot1_row(a: &MarkerWorld, u: Vec3, b: &MarkerWorld, v: Vec3) -> Row {
    let (ud, vd) = (a.w.cross(u), b.w.cross(v));
    let (udd, vdd) = (cross2(a.w, u), cross2(b.w, v));
    Row::new(
        a.body.map(|i| (i, Vec3::ZERO, u.cross(v))),
        b.body.map(|i| (i, Vec3::ZERO, v.cross(u))),
        u.dot(v),
        udd.dot(v) + 2.0 * ud.dot(vd) + u.dot(vdd),
    )
}

/// `d·u = 0` for `d = p_B − p_A` and a direction `u` of A.
fn dot2_row(a: &MarkerWorld, u: Vec3, b: &MarkerWorld) -> Row {
    let d = b.p - a.p;
    let dd = b.pdot - a.pdot;
    let ddd = cross2(b.w, b.r) - cross2(a.w, a.r);
    let (ud, udd) = (a.w.cross(u), cross2(a.w, u));
    Row::new(
        a.body.map(|i| (i, -u, u.cross(d) - a.r.cross(u))),
        b.body.map(|i| (i, u, b.r.cross(u))),
        d.dot(u),
        ddd.dot(u) + 2.0 * dd.dot(ud) + d.dot(udd),
    )
}

fn joint_rows(kind: JointKind, a: &MarkerWorld, b: &MarkerWorld, rows: &mut Vec<Row>) {
    let [ax, ay, az] = a.axes;
    let [bx, by, bz] = b.axes;
    let point = |rows: &mut Vec<Row>| rows.extend([Vec3::X, Vec3::Y, Vec3::Z].map(|e| point_row(a, b, e)));
    match kind {
        JointKind::Rigid => {
            point(rows);
            rows.extend([dot1_row(a, ay, b, bz), dot1_row(a, az, b, bx), dot1_row(a, ax, b, by)]);
        }
        JointKind::Revolute => {
            point(rows);
            rows.extend([dot1_row(a, ax, b, bz), dot1_row(a, ay, b, bz)]);
        }
        JointKind::Ball => point(rows),
        JointKind::Cylindrical => {
            rows.extend([dot2_row(a, ax, b), dot2_row(a, ay, b), dot1_row(a, ax, b, bz), dot1_row(a, ay, b, bz)]);
        }
        JointKind::Slider => {
            rows.extend([dot2_row(a, ax, b), dot2_row(a, ay, b), dot1_row(a, ax, b, bz), dot1_row(a, ay, b, bz), dot1_row(a, ax, b, by)]);
        }
        JointKind::Planar => {
            rows.extend([dot2_row(a, az, b), dot1_row(a, az, b, bx), dot1_row(a, az, b, by)]);
        }
    }
}

/// A joint's values and their rates (see [`JointKind::values`]).
fn joint_values(kind: JointKind, a: &MarkerWorld, b: &MarkerWorld) -> (Vec<f64>, Vec<f64>) {
    let [ax, ay, az] = a.axes;
    let bx = b.axes[0];
    let d = b.p - a.p;
    let dd = b.pdot - a.pdot;
    let angle = || bx.dot(ay).atan2(bx.dot(ax));
    let spin = || (b.w - a.w).dot(az);
    let along = |u: Vec3| (d.dot(u), dd.dot(u) + d.dot(a.w.cross(u)));
    match kind {
        JointKind::Rigid => (vec![], vec![]),
        JointKind::Revolute => (vec![angle()], vec![spin()]),
        JointKind::Slider => {
            let (s, r) = along(az);
            (vec![s], vec![r])
        }
        JointKind::Cylindrical => {
            let (s, r) = along(az);
            (vec![angle(), s], vec![spin(), r])
        }
        JointKind::Planar => {
            let ((x, xr), (y, yr)) = (along(ax), along(ay));
            (vec![x, y, angle()], vec![xr, yr, spin()])
        }
        JointKind::Ball => {
            // B's turn from A as a rotation vector in A's frame, and the relative angular
            // velocity in A's frame.
            let am = Mat3::from_cols(ax, ay, az);
            let rel = am.transpose().mul(&Mat3::from_cols(b.axes[0], b.axes[1], b.axes[2]));
            let rv = Quat::from_mat3(&rel).map_or(Vec3::ZERO, Quat::rotation_vector);
            let w = am.transpose().mul_vec(b.w - a.w);
            (vec![rv.x, rv.y, rv.z], vec![w.x, w.y, w.z])
        }
    }
}

#[cfg(test)]
pub(crate) mod testing {
    //! Finite-difference checks of the Jacobians and `J̇u` (tests only).
    use super::*;

    /// `(Φ, J u, J̇ u)` of every row at `y`.
    pub(crate) fn rows_at(sys: &System, y: &[f64]) -> Vec<(f64, f64, f64)> {
        let kin = sys.kinematics(y);
        sys.rows(&kin, 0.0, y).iter().map(|r| (r.phi, r.rate(&kin), r.jdot_u)).collect()
    }

    /// The state moved along its velocities for `h` (positions and orientations only).
    pub(crate) fn drift(sys: &System, y: &[f64], h: f64) -> Vec<f64> {
        let mut out = y.to_vec();
        let kin = sys.kinematics(y);
        for (i, k) in kin.iter().enumerate() {
            let at = i * STRIDE;
            put3(&mut out, at, k.x + k.v * h);
            let q = Quat::from_rotation_vector(k.w * h).compose(quat(y, at + 3));
            put_quat(&mut out, at + 3, q);
        }
        out
    }
}