Quaternion rotations

Deep dive

How it works

In: an axis and angle, a rotation matrix, or two quaternions (compose or interpolate) and a vector to rotate · Out: a unit quaternion, a rotation matrix or the rotated vector · State: none (the caller keeps the orientation) · Cost: O(1); nlerp 3 ns, slerp 17 ns, matrix conversion about 1-2 ns (measured below)

Poses, exp/log and screw motion: see rigid-body transforms.

1. Convention. Hamilton, [w, x, y, z], active rotation v' = q v q*. q = a * b applies b first, as matrix A B. JPL quaternions flip the product order.
2. To matrix: products of components, no trig; needs |q| = 1 (or scale 2 by 1/|q|^2).
3. From matrix (Shepperd): 1+tr, 1+2m00-tr, 1+2m11-tr, 1+2m22-tr equal 4w^2, 4x^2, 4y^2, 4z^2. Take the largest component and divide the others by it; the trace formula alone divides by w, about 0 for 180-degree rotations.
4. Axis-angle: angle = 2 atan2(|v|, w); acos(w) is inaccurate near w = 1. Negate q first if w < 0 for angle in [0, pi].
5. Two unit vectors a -> b: q = normalize(1 + a.b, a x b); antiparallel needs an arbitrary perpendicular axis.
6. Interpolate: align signs (dot < 0 -> negate one side), then nlerp (lerp + normalise) or slerp. Slerp has constant angular speed on the great arc; nlerp takes the same path at uneven speed.
7. Angle between (after sign alignment): 4 atan2(|a-b|, |a+b|). 2 acos(|a.b|) returns 0 for a true 1e-9 rad difference (measured); the atan2 form gave 1e-9.

Code

pub type Q = [f64; 4]; // [w, x, y, z], Hamilton, unit length
pub type M3 = [[f64; 3]; 3]; // row-major, active rotation: v' = M v

pub fn to_matrix([w, x, y, z]: Q) -> M3 {
    let (xx, yy, zz) = (x * x, y * y, z * z);
    let (xy, xz, yz) = (x * y, x * z, y * z);
    let (wx, wy, wz) = (w * x, w * y, w * z);
    [[1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy)],
     [2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx)],
     [2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy)]]
}

/// Shepperd: divide by the largest of w, x, y, z, so the sqrt argument is always >= 1.
pub fn from_matrix(m: &M3) -> Q {
    let tr = m[0][0] + m[1][1] + m[2][2];
    let q = if tr >= m[0][0] && tr >= m[1][1] && tr >= m[2][2] {
        let s = (1.0 + tr).sqrt() * 2.0;
        [s / 4.0, (m[2][1] - m[1][2]) / s, (m[0][2] - m[2][0]) / s, (m[1][0] - m[0][1]) / s]
    } else if m[0][0] >= m[1][1] && m[0][0] >= m[2][2] {
        let s = (1.0 + m[0][0] - m[1][1] - m[2][2]).sqrt() * 2.0;
        [(m[2][1] - m[1][2]) / s, s / 4.0, (m[0][1] + m[1][0]) / s, (m[0][2] + m[2][0]) / s]
    } else if m[1][1] >= m[2][2] {
        let s = (1.0 + m[1][1] - m[0][0] - m[2][2]).sqrt() * 2.0;
        [(m[0][2] - m[2][0]) / s, (m[0][1] + m[1][0]) / s, s / 4.0, (m[1][2] + m[2][1]) / s]
    } else {
        let s = (1.0 + m[2][2] - m[0][0] - m[1][1]).sqrt() * 2.0;
        [(m[1][0] - m[0][1]) / s, (m[0][2] + m[2][0]) / s, (m[1][2] + m[2][1]) / s, s / 4.0]
    };
    if q[0] < 0.0 { [-q[0], -q[1], -q[2], -q[3]] } else { q }
}

pub fn from_axis_angle(axis: [f64; 3], angle: f64) -> Q {
    let (s, c) = (angle / 2.0).sin_cos(); // axis must be unit length
    [c, axis[0] * s, axis[1] * s, axis[2] * s]
}

/// Angle in [0, pi]; atan2 stays accurate near 0 and pi where acos(w) does not.
pub fn to_axis_angle(q: Q) -> ([f64; 3], f64) {
    let q = if q[0] < 0.0 { [-q[0], -q[1], -q[2], -q[3]] } else { q };
    let n = (q[1] * q[1] + q[2] * q[2] + q[3] * q[3]).sqrt();
    if n < 1e-12 { return ([1.0, 0.0, 0.0], 2.0 * n); }
    ([q[1] / n, q[2] / n, q[3] / n], 2.0 * n.atan2(q[0]))
}

/// Shortest-arc rotation taking unit vector a to unit vector b.
pub fn from_two_vectors(a: [f64; 3], b: [f64; 3]) -> Q {
    let d = a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
    let c = [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]];
    let q = if d < -1.0 + 1e-12 {
        // antiparallel: 180 degrees about any axis perpendicular to a
        let p = if a[0].abs() < 0.9 { [0.0, -a[2], a[1]] } else { [a[2], 0.0, -a[0]] };
        [0.0, p[0], p[1], p[2]]
    } else { [1.0 + d, c[0], c[1], c[2]] };
    normalize(q)
}

pub fn normalize(q: Q) -> Q {
    let n = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]).sqrt();
    [q[0] / n, q[1] / n, q[2] / n, q[3] / n]
}
fn align(a: Q, b: Q) -> Q { // sign of b on a's side of the double cover
    if a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3] < 0.0 { [-b[0], -b[1], -b[2], -b[3]] } else { b }
}
pub fn nlerp(a: Q, b: Q, t: f64) -> Q {
    let b = align(a, b);
    normalize([0, 1, 2, 3].map(|i| a[i] + t * (b[i] - a[i])))
}
pub fn slerp(a: Q, b: Q, t: f64) -> Q {
    let b = align(a, b);
    let d = a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
    if d > 0.9995 { return nlerp(a, b, t); } // sin(theta) -> 0: the weights below become 0/0
    let th = d.acos();
    let (wa, wb) = (((1.0 - t) * th).sin() / th.sin(), (t * th).sin() / th.sin());
    [0, 1, 2, 3].map(|i| wa * a[i] + wb * b[i])
}

/// One quaternion per element (particles, bones): structure-of-arrays vectorises across elements.
pub fn rotate_batch(q: [&[f32]; 4], v: [&mut [f32]; 3]) {
    let [vx, vy, vz] = v;
    for i in 0..vx.len() {
        let (w, x, y, z) = (q[0][i], q[1][i], q[2][i], q[3][i]);
        let (px, py, pz) = (vx[i], vy[i], vz[i]);
        let (tx, ty, tz) = (2.0 * (y * pz - z * py), 2.0 * (z * px - x * pz), 2.0 * (x * py - y * px));
        vx[i] = px + w * tx + (y * tz - z * ty);
        vy[i] = py + w * ty + (z * tx - x * tz);
        vz[i] = pz + w * tz + (x * ty - y * tx);
    }
}

Asserted: matrix round trip (200k random, all four branches, 180-degree cases); to_matrix(a*b) == to_matrix(a)*to_matrix(b); axis-angle from 1e-9 to pi-1e-9; antiparallel vectors; constant slerp steps; rotate_batch equals the matrix result within 1e-5.

Complexity and constants

• All O(1); from_matrix is one branch and one sqrt.
• Measured on an Apple M4 Pro, f64, scalar, hot cache, throughput: to_matrix 0.8 ns, from_matrix 2.1 ns, nlerp 3 ns, slerp 17 ns (acos plus three sin); two runs agreed within 5%.
• nlerp speed ratio fastest/slowest, sampled: 1.02 at 30 degrees, 1.17 at 90, 1.59 at 150, 1.84 at 170; the ends are slower.
• One f32 quaternion per element, 1M elements: SoA rotate_batch 0.7 ns, AoS scalar 2.1 ns, AoS with a matrix built per element 3 ns.

Efficiency

• Cost: measured on an Apple M4 Pro, f64, scalar, hot cache: to_matrix 0.8 ns, from_matrix 2.1 ns, nlerp 3 ns, slerp 17 ns (above). With one f32 quaternion per element the loop is memory-bound: 28 B in and 12 B out per element in SoA.
• Levers:
  • nlerp instead of slerp — about 6x cheaper (above) — small angles or when constant angular speed is invisible
  • SoA component arrays — one instruction rotates 4 (NEON, SSE) or 8 (AVX) elements, about 3x over AoS scalar (measured above) — batches of quaternions
  • q * (1.5 - 0.5 |q|^2) renormalisation — no sqrt or division, relative error 3/8 e^2 for |q|^2 = 1 + e (measured above) — after many products
  • convert to a matrix once — a rotation applied to many points costs 9 mul per point instead of two quaternion products — large point sets
• Naive → standard → state of the art:
  • Euler angles with trigonometry per step · gimbal lock and 3 sin/cos pairs per matrix
  • unit quaternions, slerp, matrix for batches · slerp 17 ns, to_matrix 0.8 ns
  • SoA SIMD batches with nlerp or the cheap renormalisation · about 3x faster than AoS scalar (above)

Implementation notes

• Storage order: Eigen coeffs(), glam, ROS and many file formats use x,y,z,w; this page uses w first. Convert at the boundary only.
• Continuity: pick each new q with positive dot against the previous one, or filters and interpolators flip the long way.
• Averaging close orientations: sign-align, add, normalise; spread-out sets need the top eigenvector of sum q q^T (Markley et al.).
• Body rates: q <- normalize(q * exp(w dt / 2)); first-order q + q (0,w) dt/2 plus normalise has O(dt^2) error per step.
• Kabsch returns a matrix: pass it through from_matrix. Horn's method returns the quaternion as an eigenvector.

Pitfalls

• Euler angles: at pitch +-90 degrees only yaw -+ roll is defined and asin near 1 is inaccurate; use at the UI boundary.
• acos of a dot product rounded above 1 returns NaN; clamp (common in f32).
• Unnormalised q in to_matrix gives a scaled, sheared matrix.
• Lerp or average without sign alignment takes the long way, or q + (-q) gives zero.
• Active vs passive, or Hamilton vs JPL, mixed silently gives the inverse rotation.

Variants and alternatives

In production

• Games and 3-D content: Unity's Quaternion composes with * and interpolates with Slerp without gimbal lock (docs); glTF 2.0 stores node and animation rotations as (x, y, z, w) quaternions (spec).
• Robotics: ROS 2 uses quaternions for orientation in tf2 (tutorial).
• Drones and flight controllers: the PX4 EKF2 state vector starts with the attitude quaternion, North-East-Down to body (docs).
• Spacecraft attitude estimation: the error-state filter of Sola and the averaging method of Markley et al. (Sources).

Implementations

Sources

• Ken Shoemake, Animating rotation with quaternion curves, SIGGRAPH 1985. doi
• S. W. Shepperd, Quaternion from rotation matrix, J. Guidance and Control 1(3), 1978. doi
• Joan Sola, Quaternion kinematics for the error-state Kalman filter, 2017 (Hamilton vs JPL). arXiv
• F. Landis Markley, Yang Cheng, John L. Crassidis, Yaakov Oshman, Averaging quaternions, J. Guidance, Control, and Dynamics 30(4), 2007. doi
• Berthold K. P. Horn, Closed-form solution of absolute orientation using unit quaternions, JOSA A 4(4), 1987. doi