Rigid-body dynamics

Deep dive

How it works

In: body state x, q, v, ω, accumulated force and torque, step dt · Out: the state one step later · State: the 13 numbers of the body plus constant mass and principal moments · Cost: O(1) per body, 58 ns measured (scalar f32)

State per body: x, q, v, ω (ω in world frame) plus constants m and the body-frame principal moments I = diag(Ix, Iy, Iz). Total force F acts at the center of mass; a force at world point p adds torque τ = (p − x) × F.

1. Linear: v += dt·F/m; x += dt·v (new v).
2. Inertia: choose body axes along the principal axes, so I is diagonal and constant. Box of full extents a,b,c: Ix = m(b²+c²)/12, and cyclic. Solid sphere 2mr²/5. Compound: sum the parts about the common center of mass with the parallel-axis term m(|d|²E − d dᵀ), then diagonalize once at build time.
3. Euler's equation in the body frame (w = Rᵀω): I ẇ = τ_b − w × (I w). The cross term is the gyroscopic torque. Explicit Euler on it adds energy; the implicit form is stable: solve f(w') = I(w' − w) + dt·w'×(I w') − dt·τ_b = 0 with one Newton step from w, with Jacobian J = I + dt([w]ₓ I − [I w]ₓ) (3×3 solve).
4. Orientation: with the new ω, q ← (cos(θ/2), sin(θ/2)·ω̂)·q, θ = |ω|·dt; normalize against rounding drift.
5. Clear force and torque accumulators.

force/torque → v,ω (body-frame Newton step) → [contact/joint solver edits v,ω] → x, q

Engines split step: integrate velocities, run the solver (sequential impulses), then integrate positions.

Code

type V3 = [f32; 3];
type M3 = [[f32; 3]; 3]; // row-major
type Q = [f32; 4];       // (w, x, y, z), unit length

fn dot(a: V3, b: V3) -> f32 { a[0] * b[0] + a[1] * b[1] + a[2] * b[2] }
fn cross(a: V3, b: V3) -> V3 {
    [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]]
}
fn mv(m: &M3, v: V3) -> V3 { [dot(m[0], v), dot(m[1], v), dot(m[2], v)] }
fn tmv(m: &M3, v: V3) -> V3 { // mᵀ v
    [0, 1, 2].map(|j| m[0][j] * v[0] + m[1][j] * v[1] + m[2][j] * v[2])
}
fn skew(a: V3) -> M3 { [[0.0, -a[2], a[1]], [a[2], 0.0, -a[0]], [-a[1], a[0], 0.0]] }
/// Solve J x = b by Cramer's rule (J = I + h(...) is well conditioned for sane h).
fn solve3(j: &M3, b: V3) -> V3 {
    let det = dot(j[0], cross(j[1], j[2]));
    let (c0, c1, c2) = ([j[0][0], j[1][0], j[2][0]], [j[0][1], j[1][1], j[2][1]], [j[0][2], j[1][2], j[2][2]]);
    [dot(b, cross(c1, c2)), dot(c0, cross(b, c2)), dot(c0, cross(c1, b))].map(|d| d / det)
}
fn qmul(a: Q, b: Q) -> Q {
    [a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3],
     a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2],
     a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1],
     a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0]]
}
fn to_mat(q: Q) -> M3 {
    let [w, x, y, z] = q;
    [[1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - w * z), 2.0 * (x * z + w * y)],
     [2.0 * (x * y + w * z), 1.0 - 2.0 * (x * x + z * z), 2.0 * (y * z - w * x)],
     [2.0 * (x * z - w * y), 2.0 * (y * z + w * x), 1.0 - 2.0 * (x * x + y * y)]]
}

#[derive(Clone)]
pub struct Body {
    pub inv_mass: f32,
    pub inertia: V3, // principal moments in the body frame (diagonal I)
    pub pos: V3, pub vel: V3,
    pub rot: Q, pub ang_vel: V3, // ω in world frame
    pub force: V3, pub torque: V3, // accumulated this step, world frame
}

impl Body {
    /// Solid box with half extents (hx, hy, hz).
    pub fn new_box(mass: f32, h: V3) -> Body {
        let (x, y, z) = (4.0 * h[0] * h[0], 4.0 * h[1] * h[1], 4.0 * h[2] * h[2]);
        Body { inv_mass: 1.0 / mass, inertia: [mass / 12.0 * (y + z), mass / 12.0 * (x + z), mass / 12.0 * (x + y)],
               pos: [0.0; 3], vel: [0.0; 3], rot: [1.0, 0.0, 0.0, 0.0], ang_vel: [0.0; 3],
               force: [0.0; 3], torque: [0.0; 3] }
    }
    /// L = R I Rᵀ ω, the conserved quantity when torque = 0.
    pub fn angular_momentum(&self) -> V3 {
        let r = to_mat(self.rot);
        let wb = tmv(&r, self.ang_vel);
        mv(&r, [self.inertia[0] * wb[0], self.inertia[1] * wb[1], self.inertia[2] * wb[2]])
    }
    pub fn step(&mut self, dt: f32) {
        // Linear: velocity first, then position with the new velocity (semi-implicit Euler).
        for i in 0..3 {
            self.vel[i] += dt * self.inv_mass * self.force[i];
            self.pos[i] += dt * self.vel[i];
        }
        // Angular, in the body frame where I is constant. Implicit gyroscopic term:
        // solve f(w') = I(w' - w) + dt w'×(I w') - dt τ = 0 with one Newton step from w.
        let r = to_mat(self.rot);
        let (w, tau) = (tmv(&r, self.ang_vel), tmv(&r, self.torque));
        let iw = [self.inertia[0] * w[0], self.inertia[1] * w[1], self.inertia[2] * w[2]];
        let f = { let g = cross(w, iw); [0, 1, 2].map(|i| dt * (g[i] - tau[i])) };
        let (sw, siw) = (skew(w), skew(iw));
        let mut jac = [[0.0; 3]; 3]; // J = I + dt([w]× I - [Iw]×)
        for i in 0..3 { for j in 0..3 {
            jac[i][j] = dt * (sw[i][j] * self.inertia[j] - siw[i][j]) + if i == j { self.inertia[i] } else { 0.0 };
        } }
        let dw = solve3(&jac, f);
        let w_new = [w[0] - dw[0], w[1] - dw[1], w[2] - dw[2]];
        self.ang_vel = mv(&r, w_new);
        // Orientation: q ← exp(dt ω / 2) q, exact for constant ω; stays unit up to rounding.
        let a = dot(self.ang_vel, self.ang_vel).sqrt() * dt;
        let dq: Q = if a < 1e-6 { [1.0, 0.5 * dt * self.ang_vel[0], 0.5 * dt * self.ang_vel[1], 0.5 * dt * self.ang_vel[2]] }
        else { let s = (0.5 * a).sin() / a * dt; [(0.5 * a).cos(), s * self.ang_vel[0], s * self.ang_vel[1], s * self.ang_vel[2]] };
        let q = qmul(dq, self.rot);
        let n = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]).sqrt().recip();
        self.rot = q.map(|c| c * n);
        self.force = [0.0; 3]; self.torque = [0.0; 3];
    }
}

Tested (f32, dt = 1/60): a falling body matches y = −g·dt²·n(n+1)/2 after n steps. A torque-free box (half extents 0.5, 0.25, 0.1) spinning at 6 rad/s about its largest-inertia axis plus a 0.2 rad/s perturbation keeps angular momentum within 0.36 % and energy within 0.12 % over 10 s.

Complexity and constants

• O(1) per body: order of 150 flops, one sin, one cos, one sqrt, one 3×3 solve; timings under Efficiency.
• State: 92 B (inv_mass 4, inertia 12, x v ω F τ 60, q 16), plus the world inverse inertia (symmetric, 6 floats) cached per step for the solver.
• Cost drivers: matrix from quaternion (compute once), two basis changes, the trig.

Efficiency

• Cost: 58 ns per body per step measured (scalar f32, Apple Silicon Mac, one body, 10 M steps), about 0.6 ms per 10 000 bodies per core; 92 B of state. The bound is arithmetic on a hot record, not memory; collision detection and the solver dominate a frame, so integration is cheap enough for every substep.
• Levers:
  • skip the Newton step — removes the 3×3 solve and basis changes — spheres (I a multiple of identity) or ω on a principal axis, where the gyroscopic term is exactly zero
  • skip sleeping and static bodies — removes their whole cost — resting scenes (islands and sleeping)
  • struct-of-arrays plus SIMD — order of 4-8 bodies per instruction (lane count, not measured here) — many bodies; the only branch is the small-angle case (Bullet uses a Taylor series of sin(x)/x for |ω| < 0.001 rad/s)
  • exponential map instead of first-order update — exact for constant ω; the first-order q += ½·dt·(0,ω)q plus normalize loses 0.050 rad at 60 Hz and 0.003 rad at 240 Hz (measured, f64, ω = 6 rad/s, 10 s) — costs one sin and one cos more per body
• Naive → standard → state of the art:
  • explicit Euler, ω updated from torque only, first-order quaternion · cheapest per body, but gains energy and loses rotation
  • semi-implicit Euler, implicit gyroscopic Newton step, exponential map · 58 ns measured (the code above)
  • the same in SoA SIMD form, with sleeping and gyroscopic skip for symmetric bodies · lower per body by roughly the lane count on awake bodies, nothing for sleeping ones (order of)

Implementation notes

• Keep ω in world frame (what the solver wants); do the gyroscopic step in the body frame with the one rotation matrix per step.
• Static bodies: inv_mass = 0, zero inverse inertia. Locked axes: zero that inverse-inertia entry.
• Clamp speeds. Jolt defaults to 500 m/s linear and 0.25π·60 ≈ 47 rad/s angular; Bullet caps rotation at 0.5·π/2 rad per step.
• Fixed step (fixed-timestep game loop): the Jacobian and the damping depend on dt. f32 suffices for q; far from the origin positions lose precision (floating-point error).

Pitfalls

• Explicit gyroscopic torque (ω += dt·I⁻¹(−ω×Iω)): in the test above at 60 Hz it drifts 7.7x more in angular momentum and gains 6.7 % energy (measured).
• No gyroscopic term: ω stays constant in free flight, so there is no precession or intermediate-axis tumbling (the Dzhanibekov effect).
• The implicit step damps: spin about the intermediate axis lost 35 % of its energy in 10 s at 60 Hz (measured). Bepu documents the same trade-off.
• Not renormalizing q: rotations start to scale and shear.

Variants and alternatives

In production

Bullet: implicit body-frame gyroscopic step (one Newton-Raphson update, default) plus exponential-map orientation. Jolt: the norm-preserving variant. BepuPhysics: nonconserving, momentum-conserving and implicit-gyroscopic modes. Robotics: MuJoCo simulates rigid bodies for model-based control (Todorov 2012). Molecular dynamics: rigid-molecule integrators advance orientation as quaternions (Miller 2002). Spacecraft: attitude propagation integrates Euler's equations (Markley and Crassidis 2014).

Implementations

Sources

• David Baraff, An Introduction to Physically Based Modeling: Rigid Body Simulation I, Unconstrained Rigid Body Dynamics, SIGGRAPH course notes. pdf
• Erin Catto, Numerical Methods, GDC 2015 (implicit gyroscopic term). pdf
• F. Sebastian Grassia, Practical Parameterization of Rotations Using the Exponential Map, Journal of Graphics Tools 3(3), 1998. doi
• Ernst Hairer, Christian Lubich, Gerhard Wanner, Geometric Numerical Integration, Springer, 2nd ed. 2006. doi
• Glenn Fiedler, Physics in 3D, Gaffer On Games. post
• F. Landis Markley, John L. Crassidis, Fundamentals of Spacecraft Attitude Determination and Control, Springer, 2014. doi
• Thomas F. Miller III et al., Symplectic quaternion scheme for biophysical molecular dynamics, J. Chem. Phys. 116, 2002. doi