Skip to content

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

23.6 KB
Raw Blame
//! What a simulation is made of: rigid bodies, joints between frames on them, force elements
//! and the solver's settings. Everything is checked by [`Model::validate`] before a run.

use serde::{Deserialize, Serialize};
use solidcore_record::Vec3;
use solidcraft_geom::prelude::*;

use crate::math::{Mat3, Quat};
use crate::{DynamicsError, Result};

/// Bodies one simulation takes at most.
pub const MAX_BODIES: usize = 200;
/// Joints one simulation takes at most.
pub const MAX_JOINTS: usize = 400;
/// Constraint equations one simulation takes at most (the dense solve grows with their cube).
pub const MAX_CONSTRAINT_ROWS: usize = 1200;
/// Force elements one simulation takes at most.
pub const MAX_FORCES: usize = 2000;
/// Integration steps one run takes at most (accepted and rejected together).
pub const MAX_STEPS: usize = 2_000_000;
/// Output samples one run keeps at most.
pub const MAX_SAMPLES: usize = 100_000;
/// Projection iterations per step at most.
pub const MAX_PROJECTION_ITERATIONS: usize = 50;
/// Coordinates are kept within this size (mm, mm/s, rad/s); beyond it the run stops with
/// [`DynamicsError::Diverged`].
pub const MAX_MAGNITUDE: f64 = 1e12;

/// A rigid body. Lengths in mm, mass in kg, time in s, so forces are in kg·mm/s² (mN) and
/// torques in mN·mm.
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
pub struct Body {
    /// Mass (kg), > 0.
    pub mass: f64,
    /// Inertia tensor about the centre of mass in the body's own axes (kg·mm²), symmetric
    /// positive definite.
    pub inertia: Mat3,
    /// Centre of mass at the start (world, mm). The body's own frame has its origin here.
    pub position: Vec3,
    /// The body's axes at the start (body → world).
    #[serde(default)]
    pub orientation: Quat,
    /// Velocity of the centre of mass at the start (world, mm/s).
    #[serde(default)]
    pub velocity: Vec3,
    /// Angular velocity at the start (world, rad/s).
    #[serde(default)]
    pub angular_velocity: Vec3,
}

impl Body {
    /// A body at rest at `position`, axes along the world's, with a diagonal inertia.
    pub fn at_rest(mass: f64, principal_inertia: Vec3, position: Vec3) -> Body {
        Body {
            mass,
            inertia: Mat3::diagonal(principal_inertia),
            position,
            orientation: Quat::IDENTITY,
            velocity: Vec3::ZERO,
            angular_velocity: Vec3::ZERO,
        }
    }
}

/// A frame on a body (in the body's own coordinates: origin at its centre of mass) or, with no
/// body, fixed in the world. `z` and `x` need not be unit or square: `x` is laid square to `z`.
#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
pub struct Marker {
    #[serde(default, skip_serializing_if = "Option::is_none")]
    pub body: Option<usize>,
    pub origin: Vec3,
    pub z: Vec3,
    pub x: Vec3,
}

impl Marker {
    pub fn new(body: Option<usize>, origin: Vec3, z: Vec3, x: Vec3) -> Marker {
        Marker { body, origin, z, x }
    }
    /// A frame fixed in the world.
    pub fn ground(origin: Vec3, z: Vec3, x: Vec3) -> Marker {
        Marker { body: None, origin, z, x }
    }
}

/// A point on a body (body coordinates) or, with no body, in the world.
#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
pub struct Anchor {
    #[serde(default, skip_serializing_if = "Option::is_none")]
    pub body: Option<usize>,
    pub point: Vec3,
}

/// The joints the solver knows; each holds `b`'s frame to `a`'s as the matching assembly joint
/// does (z is the joint's axis), with the frames coinciding at zero joint values.
#[derive(Clone, Copy, Debug, PartialEq, Eq, Serialize, Deserialize)]
#[serde(rename_all = "snake_case")]
pub enum JointKind {
    /// No relative motion (6 equations).
    Rigid,
    /// Turns about z (5 equations).
    Revolute,
    /// Slides along z (5 equations).
    Slider,
    /// Turns about and slides along z (4 equations).
    Cylindrical,
    /// Slides in the xy plane and turns about z (3 equations).
    Planar,
    /// Turns any way about the common origin (3 equations).
    Ball,
}

impl JointKind {
    /// How many constraint equations the joint adds.
    pub fn equations(self) -> usize {
        match self {
            JointKind::Rigid => 6,
            JointKind::Revolute | JointKind::Slider => 5,
            JointKind::Cylindrical => 4,
            JointKind::Planar | JointKind::Ball => 3,
        }
    }
    /// The joint's values, in the order [`crate::JointResult::values`] gives them.
    pub fn values(self) -> &'static [&'static str] {
        match self {
            JointKind::Rigid => &[],
            JointKind::Revolute => &["angle"],
            JointKind::Slider => &["distance"],
            JointKind::Cylindrical => &["angle", "distance"],
            JointKind::Planar => &["x", "y", "angle"],
            JointKind::Ball => &["rx", "ry", "rz"],
        }
    }
}

/// A joint: `b`'s frame held to `a`'s.
#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
pub struct Joint {
    pub kind: JointKind,
    pub a: Marker,
    pub b: Marker,
}

/// One value of a joint: `dof` indexes [`JointKind::values`]. Revolute, slider, cylindrical
/// and planar joints have values that springs, motors, drives and links can act on; ball and
/// rigid joints have none.
#[derive(Clone, Copy, Debug, PartialEq, Eq, Serialize, Deserialize)]
pub struct Coord {
    pub joint: usize,
    #[serde(default)]
    pub dof: usize,
}

impl Coord {
    pub fn new(joint: usize, dof: usize) -> Coord {
        Coord { joint, dof }
    }
}

/// Points a table may hold at most.
pub const MAX_PROFILE_POINTS: usize = 10_000;

/// A value along time (s): a motor's torque or force, or the value a drive imposes (radians,
/// mm).
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(tag = "type", rename_all = "snake_case")]
pub enum Profile {
    Constant {
        value: f64,
    },
    /// `value + rate·t`.
    Linear {
        value: f64,
        rate: f64,
    },
    /// From `from` to `to` between `start` and `start + duration`, along a cycloidal step
    /// (`τ − sin(2πτ)/2π`) whose speed and acceleration both start and end at zero.
    Ramp {
        from: f64,
        to: f64,
        #[serde(default)]
        start: f64,
        duration: f64,
    },
    /// `offset + amplitude·sin(2π·frequency·t + phase)` (frequency in Hz).
    Harmonic {
        #[serde(default)]
        offset: f64,
        amplitude: f64,
        frequency: f64,
        #[serde(default)]
        phase: f64,
    },
    /// (time, value) points, times increasing: linear in between, held before the first and
    /// after the last. As a drive its speed jumps at the points (its acceleration is taken as 0).
    Table {
        points: Vec<(f64, f64)>,
    },
}

impl Profile {
    /// The value and its first and second time derivatives at `t`.
    pub fn eval(&self, t: f64) -> (f64, f64, f64) {
        match self {
            Profile::Constant { value } => (*value, 0.0, 0.0),
            Profile::Linear { value, rate } => (value + rate * t, *rate, 0.0),
            Profile::Ramp { from, to, start, duration } => {
                let tau = (t - start) / duration;
                if !(tau > 0.0) {
                    return (*from, 0.0, 0.0);
                }
                if tau >= 1.0 {
                    return (*to, 0.0, 0.0);
                }
                let span = to - from;
                let (s, c) = (std::f64::consts::TAU * tau).sin_cos();
                (
                    from + span * (tau - s / std::f64::consts::TAU),
                    span / duration * (1.0 - c),
                    span / (duration * duration) * std::f64::consts::TAU * s,
                )
            }
            Profile::Harmonic { offset, amplitude, frequency, phase } => {
                let w = std::f64::consts::TAU * frequency;
                let (s, c) = (w * t + phase).sin_cos();
                (offset + amplitude * s, amplitude * w * c, -amplitude * w * w * s)
            }
            Profile::Table { points } => {
                let (Some(first), Some(last)) = (points.first(), points.last()) else { return (0.0, 0.0, 0.0) };
                if t <= first.0 {
                    return (first.1, 0.0, 0.0);
                }
                if t >= last.0 {
                    return (last.1, 0.0, 0.0);
                }
                points
                    .windows(2)
                    .find_map(|w| {
                        let (a, b) = (w.first()?, w.get(1)?);
                        (t >= a.0 && t < b.0).then(|| {
                            let slope = (b.1 - a.1) / (b.0 - a.0);
                            (a.1 + slope * (t - a.0), slope, 0.0)
                        })
                    })
                    .unwrap_or((last.1, 0.0, 0.0))
            }
        }
    }

    fn validate(&self) -> std::result::Result<(), String> {
        let fin = |x: f64| x.is_finite() && x.abs() < MAX_MAGNITUDE;
        let ok = match self {
            Profile::Constant { value } => fin(*value),
            Profile::Linear { value, rate } => fin(*value) && fin(*rate),
            Profile::Ramp { from, to, start, duration } => fin(*from) && fin(*to) && fin(*start) && fin(*duration) && *duration > 1e-9,
            Profile::Harmonic { offset, amplitude, frequency, phase } => {
                fin(*offset) && fin(*amplitude) && fin(*frequency) && fin(*phase) && frequency.abs() < 1e6
            }
            Profile::Table { points } => {
                if points.is_empty() || points.len() > MAX_PROFILE_POINTS {
                    return Err(format!("a table needs 1 to {MAX_PROFILE_POINTS} points"));
                }
                points.iter().all(|(t, v)| fin(*t) && fin(*v))
                    && points.windows(2).all(|w| matches!((w.first(), w.get(1)), (Some(a), Some(b)) if b.0 > a.0))
            }
        };
        if ok { Ok(()) } else { Err("a profile's numbers must be finite (ramps need a duration, tables increasing times)".into()) }
    }
}

/// A joint value made to follow a profile along time (radians or mm): the joint turns or
/// slides as told, whatever it takes; [`crate::Sample::drives`] reports what it takes.
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
pub struct Drive {
    pub at: Coord,
    pub motion: Profile,
}

/// A motion link: `follower = ratio·driver + offset`, between values of two joints (a gear
/// pair: angle to angle; a rack and pinion: angle to distance, the ratio being the pinion's
/// radius; a screw: angle to distance along the same cylindrical joint, the ratio being
/// pitch / 2π). Without an offset, the one that holds at the start is kept.
#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
pub struct Link {
    pub driver: Coord,
    pub follower: Coord,
    pub ratio: f64,
    #[serde(default, skip_serializing_if = "Option::is_none")]
    pub offset: Option<f64>,
}

/// A force element.
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(tag = "type", rename_all = "snake_case")]
pub enum Force {
    /// A spring and damper on a joint value: the joint is pushed back by
    /// `stiffness·(value − rest) + damping·rate` (torsional for an angle: mN·mm per radian and
    /// mN·mm·s; linear for a distance: kg/s² and kg/s).
    JointSpring {
        at: Coord,
        stiffness: f64,
        #[serde(default)]
        damping: f64,
        #[serde(default)]
        rest: f64,
    },
    /// A motor's torque (mN·mm) or force (mN) along a joint value, following a profile: it
    /// pushes B's side forward and A's side back.
    Motor { at: Coord, effort: Profile },
    /// A linear spring and damper between two points: the tension is
    /// `stiffness·(length − free_length) + damping·(rate of length) + preload`.
    Spring {
        a: Anchor,
        b: Anchor,
        /// kg/s² (= N/m = mN/mm), ≥ 0.
        stiffness: f64,
        /// kg/s (= N·s/m), ≥ 0.
        damping: f64,
        /// mm, ≥ 0.
        free_length: f64,
        /// mN, tension at free length.
        #[serde(default)]
        preload: f64,
    },
    /// A constant force (world, mN) at a point of a body, and a constant torque (world, mN·mm).
    Load {
        body: usize,
        #[serde(default)]
        point: Vec3,
        #[serde(default)]
        force: Vec3,
        #[serde(default)]
        torque: Vec3,
    },
}

/// How the equations are stepped in time.
#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
#[serde(tag = "type", rename_all = "snake_case")]
pub enum Integrator {
    /// Classical fourth-order Runge–Kutta with a fixed step (s).
    Rk4 { step: f64 },
    /// Dormand–Prince 5(4) with error control: steps between `min_step` and `max_step` (s)
    /// keep the local error under `abs_tol + rel_tol·|y|`.
    Adaptive { rel_tol: f64, abs_tol: f64, min_step: f64, max_step: f64 },
}

/// A run's settings.
#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
pub struct Settings {
    /// Simulated time (s), > 0.
    pub duration: f64,
    /// Time between output samples (s), > 0; the integrator lands on every sample.
    pub output_interval: f64,
    pub integrator: Integrator,
    /// Project positions and velocities back onto the joints after every step (on by default).
    #[serde(default = "yes")]
    pub projection: bool,
    /// The constraint error (mm, or radians times mm for directions) projection reaches.
    #[serde(default = "default_projection_tol")]
    pub projection_tol: f64,
    /// Baumgarte stabilisation `(α, β)` (1/s): the constraint accelerations become
    /// `Φ̈ + 2αΦ̇ + β²Φ = 0`. Off by default (projection keeps the joints).
    #[serde(default, skip_serializing_if = "Option::is_none")]
    pub baumgarte: Option<(f64, f64)>,
}

fn yes() -> bool {
    true
}
fn default_projection_tol() -> f64 {
    1e-10
}

impl Settings {
    /// Fixed-step RK4 settings.
    pub fn rk4(duration: f64, step: f64, output_interval: f64) -> Settings {
        Settings {
            duration,
            output_interval,
            integrator: Integrator::Rk4 { step },
            projection: true,
            projection_tol: default_projection_tol(),
            baumgarte: None,
        }
    }
    /// Adaptive settings with the given relative and absolute tolerances.
    pub fn adaptive(duration: f64, output_interval: f64, rel_tol: f64, abs_tol: f64) -> Settings {
        Settings {
            duration,
            output_interval,
            integrator: Integrator::Adaptive { rel_tol, abs_tol, min_step: 1e-9, max_step: output_interval },
            projection: true,
            projection_tol: default_projection_tol(),
            baumgarte: None,
        }
    }
}

/// A multibody system.
#[derive(Clone, Debug, Default, PartialEq, Serialize, Deserialize)]
pub struct Model {
    pub bodies: Vec<Body>,
    #[serde(default)]
    pub joints: Vec<Joint>,
    #[serde(default)]
    pub forces: Vec<Force>,
    #[serde(default, skip_serializing_if = "Vec::is_empty")]
    pub drives: Vec<Drive>,
    #[serde(default, skip_serializing_if = "Vec::is_empty")]
    pub links: Vec<Link>,
    /// Gravity (mm/s²), e.g. `(0, 0, −9806.65)`.
    #[serde(default)]
    pub gravity: Vec3,
}

/// Standard gravity (mm/s²).
pub const STANDARD_GRAVITY: f64 = 9806.65;

fn finite_vec(v: Vec3) -> bool {
    v.is_finite() && v.x.abs() < MAX_MAGNITUDE && v.y.abs() < MAX_MAGNITUDE && v.z.abs() < MAX_MAGNITUDE
}

fn bad(what: String) -> DynamicsError {
    DynamicsError::InvalidModel(what)
}

/// A marker's axes in its body's coordinates (columns x, y, z), from its `z` and `x`.
pub(crate) fn marker_axes(m: &Marker) -> Option<Mat3> {
    let z = m.z.normalized()?;
    let x = (m.x - z * m.x.dot(z)).normalized()?;
    let y = z.cross(x).normalized()?;
    Some(Mat3::from_cols(x, y, z))
}

impl Model {
    /// Checks every number and reference, and the caps. Called by [`crate::simulate`].
    pub fn validate(&self) -> Result<()> {
        if self.bodies.is_empty() {
            return Err(bad("no bodies".into()));
        }
        if self.bodies.len() > MAX_BODIES {
            return Err(DynamicsError::TooLarge(format!("{} bodies (at most {MAX_BODIES})", self.bodies.len())));
        }
        if self.joints.len() > MAX_JOINTS {
            return Err(DynamicsError::TooLarge(format!("{} joints (at most {MAX_JOINTS})", self.joints.len())));
        }
        if self.forces.len() > MAX_FORCES {
            return Err(DynamicsError::TooLarge(format!("{} force elements (at most {MAX_FORCES})", self.forces.len())));
        }
        if self.drives.len() > MAX_JOINTS || self.links.len() > MAX_JOINTS {
            return Err(DynamicsError::TooLarge(format!("drives and links: at most {MAX_JOINTS} each")));
        }
        let rows: usize = self.joints.iter().map(|j| j.kind.equations()).sum::<usize>() + self.drives.len() + self.links.len();
        if rows > MAX_CONSTRAINT_ROWS {
            return Err(DynamicsError::TooLarge(format!("{rows} constraint equations (at most {MAX_CONSTRAINT_ROWS})")));
        }
        if !finite_vec(self.gravity) {
            return Err(bad("gravity is not a finite vector".into()));
        }
        for (i, b) in self.bodies.iter().enumerate() {
            if !(b.mass > 0.0) || !(b.mass < MAX_MAGNITUDE) {
                return Err(bad(format!("body {i}: mass must be positive and finite")));
            }
            if b.inertia.spd_inverse().is_none() {
                return Err(bad(format!("body {i}: inertia must be symmetric positive definite")));
            }
            if !finite_vec(b.position) || !finite_vec(b.velocity) || !finite_vec(b.angular_velocity) {
                return Err(bad(format!("body {i}: position and velocities must be finite")));
            }
            if b.orientation.normalized().is_none() {
                return Err(bad(format!("body {i}: orientation is not a rotation")));
            }
        }
        let body_ok = |b: Option<usize>| b.is_none_or(|b| b < self.bodies.len());
        for (i, j) in self.joints.iter().enumerate() {
            for m in [&j.a, &j.b] {
                if !body_ok(m.body) {
                    return Err(bad(format!("joint {i}: no body {}", m.body.unwrap_or(0))));
                }
                if !finite_vec(m.origin) || marker_axes(m).is_none() {
                    return Err(bad(format!("joint {i}: a frame's origin or axes are not usable")));
                }
            }
            if j.a.body == j.b.body {
                return Err(bad(format!("joint {i}: both frames are on the same body")));
            }
        }
        for (i, f) in self.forces.iter().enumerate() {
            match f {
                Force::Spring { a, b, stiffness, damping, free_length, preload } => {
                    if !body_ok(a.body) || !body_ok(b.body) {
                        return Err(bad(format!("force {i}: no such body")));
                    }
                    if !finite_vec(a.point) || !finite_vec(b.point) {
                        return Err(bad(format!("force {i}: anchor points must be finite")));
                    }
                    let ok = |x: f64| (0.0..MAX_MAGNITUDE).contains(&x);
                    if !ok(*stiffness) || !ok(*damping) || !ok(*free_length) || !preload.is_finite() || preload.abs() >= MAX_MAGNITUDE {
                        return Err(bad(format!("force {i}: stiffness, damping and free length must be ≥ 0 and finite")));
                    }
                }
                Force::Load { body, point, force, torque } => {
                    if *body >= self.bodies.len() {
                        return Err(bad(format!("force {i}: no body {body}")));
                    }
                    if !finite_vec(*point) || !finite_vec(*force) || !finite_vec(*torque) {
                        return Err(bad(format!("force {i}: point, force and torque must be finite")));
                    }
                }
                Force::JointSpring { at, stiffness, damping, rest } => {
                    self.check_coord(*at).map_err(|e| bad(format!("force {i}: {e}")))?;
                    let ok = |x: f64| (0.0..MAX_MAGNITUDE).contains(&x);
                    if !ok(*stiffness) || !ok(*damping) || !rest.is_finite() || rest.abs() >= MAX_MAGNITUDE {
                        return Err(bad(format!("force {i}: stiffness and damping must be ≥ 0 and finite, rest finite")));
                    }
                }
                Force::Motor { at, effort } => {
                    self.check_coord(*at).map_err(|e| bad(format!("force {i}: {e}")))?;
                    effort.validate().map_err(|e| bad(format!("force {i}: {e}")))?;
                }
            }
        }
        for (i, d) in self.drives.iter().enumerate() {
            self.check_coord(d.at).map_err(|e| bad(format!("drive {i}: {e}")))?;
            d.motion.validate().map_err(|e| bad(format!("drive {i}: {e}")))?;
        }
        for (i, l) in self.links.iter().enumerate() {
            self.check_coord(l.driver).map_err(|e| bad(format!("link {i}: {e}")))?;
            self.check_coord(l.follower).map_err(|e| bad(format!("link {i}: {e}")))?;
            if l.driver == l.follower {
                return Err(bad(format!("link {i}: a value can't drive itself")));
            }
            let fin = |x: f64| x.is_finite() && x.abs() < MAX_MAGNITUDE;
            if !fin(l.ratio) || !l.offset.is_none_or(fin) {
                return Err(bad(format!("link {i}: ratio and offset must be finite")));
            }
        }
        Ok(())
    }

    /// The joint and value a [`Coord`] names, if they exist and can be acted on.
    fn check_coord(&self, c: Coord) -> std::result::Result<(), String> {
        let j = self.joints.get(c.joint).ok_or_else(|| format!("no joint {}", c.joint))?;
        if matches!(j.kind, JointKind::Ball | JointKind::Rigid) {
            return Err(format!("joint {}: not supported yet: springs, motors, drives and links on {:?} joints", c.joint, j.kind));
        }
        if c.dof >= j.kind.values().len() {
            return Err(format!("joint {} has no value {}", c.joint, c.dof));
        }
        Ok(())
    }
}

impl Settings {
    /// Checks the times, tolerances and that the run fits the step and sample caps.
    pub fn validate(&self) -> Result<()> {
        let pos = |x: f64| x > 0.0 && x < MAX_MAGNITUDE;
        if !pos(self.duration) || !pos(self.output_interval) {
            return Err(DynamicsError::InvalidSettings("duration and output interval must be positive and finite".into()));
        }
        let samples = (self.duration / self.output_interval).ceil();
        if !(samples < MAX_SAMPLES as f64) {
            return Err(DynamicsError::TooLarge(format!("{samples} output samples (at most {MAX_SAMPLES})")));
        }
        if !pos(self.projection_tol) {
            return Err(DynamicsError::InvalidSettings("projection tolerance must be positive".into()));
        }
        if let Some((a, b)) = self.baumgarte
            && (!(0.0..MAX_MAGNITUDE).contains(&a) || !(0.0..MAX_MAGNITUDE).contains(&b))
        {
            return Err(DynamicsError::InvalidSettings("Baumgarte terms must be ≥ 0 and finite".into()));
        }
        match self.integrator {
            Integrator::Rk4 { step } => {
                if !pos(step) {
                    return Err(DynamicsError::InvalidSettings("step must be positive".into()));
                }
                let steps = (self.duration / step.min(self.output_interval)).ceil();
                if !(steps < MAX_STEPS as f64) {
                    return Err(DynamicsError::TooLarge(format!("{steps} steps (at most {MAX_STEPS})")));
                }
            }
            Integrator::Adaptive { rel_tol, abs_tol, min_step, max_step } => {
                if !pos(rel_tol) || !pos(abs_tol) || !pos(min_step) || !pos(max_step) || min_step > max_step {
                    return Err(DynamicsError::InvalidSettings("tolerances and step bounds must be positive, min ≤ max".into()));
                }
            }
        }
        Ok(())
    }
}