A small quaternion library in C

Lesson 11 · Code

Everything from this tutorial in two small files of plain C: float math, no malloc, no dependencies. It runs anywhere, from a desktop program to a small microcontroller. The same few lines translate easily to Python, C# or JavaScript.

11.1What is inside

One type, quat_t with w, x, y, z, and one function for each lesson:

• quat_identity: no turn, [1, 0, 0, 0]. quat_norm: the length of q, lesson 4.
• quat_mul, quat_conj, quat_normalize: lesson 5 and lesson 7.
• quat_from_axis_angle: lesson 4.
• quat_rotate, quat_rotate_inv: object frame to world and back, lesson 6.
• quat_from_euler, quat_to_euler, quat_to_matrix: lesson 8.
• quat_integrate: lesson 9. quat_slerp: lesson 10.

11.2The files

The header: two small types and the functions.

/* quaternion.h · Steppeschool, Quaternions Visually
 * Small quaternion library for 3D rotations. Plain C, float math, no dependencies.
 * Convention: q = [w, x, y, z], Hamilton product, v_world = q ⊗ v_object ⊗ q*.
 * Right-handed axes. Euler angles: ZYX (yaw about z, then pitch about y, then roll about x), in radians.
 */
#ifndef QUATERNION_H
#define QUATERNION_H

typedef struct { float w, x, y, z; } quat_t;
typedef struct { float x, y, z; } vec3_t;

quat_t quat_identity(void);
quat_t quat_mul(quat_t a, quat_t b);              /* a ⊗ b: first b, then a (world axes) */
quat_t quat_conj(quat_t q);                       /* [w, -x, -y, -z]: the opposite turn  */
float  quat_norm(quat_t q);
quat_t quat_normalize(quat_t q);
quat_t quat_from_axis_angle(vec3_t axis, float angle);
vec3_t quat_rotate(quat_t q, vec3_t v);           /* object -> world: q ⊗ v ⊗ q*         */
vec3_t quat_rotate_inv(quat_t q, vec3_t v);       /* world -> object: q* ⊗ v ⊗ q         */
quat_t quat_from_euler(float roll, float pitch, float yaw);
void   quat_to_euler(quat_t q, float *roll, float *pitch, float *yaw);
void   quat_to_matrix(quat_t q, float R[3][3]);
quat_t quat_integrate(quat_t q, vec3_t omega_rad_s, float dt);  /* one time step of a spinning object */
quat_t quat_slerp(quat_t a, quat_t b, float t);

#endif

The code. Every function is one formula from the lessons.

/* quaternion.c · Steppeschool, Quaternions Visually */
#include "quaternion.h"
#include <math.h>

quat_t quat_identity(void) { quat_t q = {1.0f, 0.0f, 0.0f, 0.0f}; return q; }

quat_t quat_mul(quat_t a, quat_t b)
{
    quat_t r;
    r.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
    r.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;
    r.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x;
    r.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w;
    return r;
}

quat_t quat_conj(quat_t q) { quat_t r = {q.w, -q.x, -q.y, -q.z}; return r; }

float quat_norm(quat_t q) { return sqrtf(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z); }

quat_t quat_normalize(quat_t q)
{
    float n = quat_norm(q);
    if (n < 1e-9f) return quat_identity();      /* never divide by zero */
    quat_t r = {q.w / n, q.x / n, q.y / n, q.z / n};
    return r;
}

quat_t quat_from_axis_angle(vec3_t a, float angle)
{
    float n = sqrtf(a.x * a.x + a.y * a.y + a.z * a.z);
    if (n < 1e-9f) return quat_identity();
    float s = sinf(angle * 0.5f) / n;
    quat_t q = {cosf(angle * 0.5f), a.x * s, a.y * s, a.z * s};
    return q;
}

/* v' = v + 2w (u × v) + 2 u × (u × v), with u = (x, y, z). Same as q ⊗ v ⊗ q*, fewer multiplications. */
vec3_t quat_rotate(quat_t q, vec3_t v)
{
    float tx = 2.0f * (q.y * v.z - q.z * v.y);   /* t = 2 (u × v) */
    float ty = 2.0f * (q.z * v.x - q.x * v.z);
    float tz = 2.0f * (q.x * v.y - q.y * v.x);
    vec3_t r;
    r.x = v.x + q.w * tx + (q.y * tz - q.z * ty);
    r.y = v.y + q.w * ty + (q.z * tx - q.x * tz);
    r.z = v.z + q.w * tz + (q.x * ty - q.y * tx);
    return r;
}

vec3_t quat_rotate_inv(quat_t q, vec3_t v) { return quat_rotate(quat_conj(q), v); }

quat_t quat_from_euler(float roll, float pitch, float yaw)
{
    float cr = cosf(roll * 0.5f),  sr = sinf(roll * 0.5f);
    float cp = cosf(pitch * 0.5f), sp = sinf(pitch * 0.5f);
    float cy = cosf(yaw * 0.5f),   sy = sinf(yaw * 0.5f);
    quat_t q;
    q.w = cr * cp * cy + sr * sp * sy;
    q.x = sr * cp * cy - cr * sp * sy;
    q.y = cr * sp * cy + sr * cp * sy;
    q.z = cr * cp * sy - sr * sp * cy;
    return q;
}

void quat_to_euler(quat_t q, float *roll, float *pitch, float *yaw)
{
    float s = 2.0f * (q.w * q.y - q.z * q.x);
    if (s > 1.0f)  s = 1.0f;                     /* rounding can push it to 1.0000001: asinf would give NaN */
    if (s < -1.0f) s = -1.0f;
    if (s > 0.99999f || s < -0.99999f) {         /* gimbal lock: roll and yaw share one axis, so we put it all in yaw */
        *roll  = 0.0f;
        *pitch = (s > 0.0f) ? 1.5707963f : -1.5707963f;
        *yaw   = ((s > 0.0f) ? -2.0f : 2.0f) * atan2f(q.x, q.w);
        return;
    }
    *roll  = atan2f(2.0f * (q.w * q.x + q.y * q.z), 1.0f - 2.0f * (q.x * q.x + q.y * q.y));
    *pitch = asinf(s);
    *yaw   = atan2f(2.0f * (q.w * q.z + q.x * q.y), 1.0f - 2.0f * (q.y * q.y + q.z * q.z));
}

void quat_to_matrix(quat_t q, float R[3][3])
{
    float w = q.w, x = q.x, y = q.y, z = q.z;
    R[0][0] = 1 - 2 * (y * y + z * z); R[0][1] = 2 * (x * y - w * z);     R[0][2] = 2 * (x * z + w * y);
    R[1][0] = 2 * (x * y + w * z);     R[1][1] = 1 - 2 * (x * x + z * z); R[1][2] = 2 * (y * z - w * x);
    R[2][0] = 2 * (x * z - w * y);     R[2][1] = 2 * (y * z + w * x);     R[2][2] = 1 - 2 * (x * x + y * y);
}

/* q_dot = 1/2 q ⊗ [0, ω]. One small step, then normalise. ω in rad/s about the object's own axes, dt in seconds. */
quat_t quat_integrate(quat_t q, vec3_t omega, float dt)
{
    float h = 0.5f * dt;
    quat_t r;
    r.w = q.w + h * (-q.x * omega.x - q.y * omega.y - q.z * omega.z);
    r.x = q.x + h * ( q.w * omega.x + q.y * omega.z - q.z * omega.y);
    r.y = q.y + h * ( q.w * omega.y - q.x * omega.z + q.z * omega.x);
    r.z = q.z + h * ( q.w * omega.z + q.x * omega.y - q.y * omega.x);
    return quat_normalize(r);
}

quat_t quat_slerp(quat_t a, quat_t b, float t)
{
    float d = a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z;
    if (d < 0.0f) { b.w = -b.w; b.x = -b.x; b.y = -b.y; b.z = -b.z; d = -d; }   /* take the short way */
    float ka, kb;
    if (d > 0.9995f) { ka = 1.0f - t; kb = t; }                                 /* almost equal: plain blend */
    else {
        float th = acosf(d), s = sinf(th);
        ka = sinf((1.0f - t) * th) / s;
        kb = sinf(t * th) / s;
    }
    quat_t r = {ka * a.w + kb * b.w, ka * a.x + kb * b.x, ka * a.y + kb * b.y, ka * a.z + kb * b.z};
    return quat_normalize(r);
}

Tests you can run before you use it.

/* test_quaternion.c · build and run:  gcc test_quaternion.c quaternion.c -lm -o test && ./test */
#include <stdio.h>
#include <math.h>
#include "quaternion.h"

#define DEG (3.14159265f / 180.0f)
static int fails = 0;
static void check(const char *name, float got, float want)
{
    int ok = fabsf(got - want) < 1e-3f;
    if (!ok) fails++;
    printf("%-34s %9.4f  (want %9.4f)  %s\n", name, got, want, ok ? "ok" : "FAIL");
}

int main(void)
{
    /* 90 deg about z: the x axis turns to the y axis */
    quat_t qz = quat_from_axis_angle((vec3_t){0, 0, 1}, 90 * DEG);
    vec3_t v = quat_rotate(qz, (vec3_t){1, 0, 0});
    check("90 about z: x -> y, x part", v.x, 0);
    check("90 about z: x -> y, y part", v.y, 1);

    /* order matters: 90 about x, then 90 about z */
    quat_t qx = quat_from_axis_angle((vec3_t){1, 0, 0}, 90 * DEG);
    quat_t ab = quat_mul(qz, qx), ba = quat_mul(qx, qz);
    printf("qz*qx = [%.3f %.3f %.3f %.3f]\n", ab.w, ab.x, ab.y, ab.z);
    printf("qx*qz = [%.3f %.3f %.3f %.3f]\n", ba.w, ba.x, ba.y, ba.z);
    check("order matters (y parts differ)", ab.y - ba.y, 1.0f);

    /* Euler -> q -> Euler */
    float r, p, y;
    quat_t qe = quat_from_euler(20 * DEG, 30 * DEG, 45 * DEG);
    quat_to_euler(qe, &r, &p, &y);
    check("euler round trip: roll", r / DEG, 20);
    check("euler round trip: pitch", p / DEG, 30);
    check("euler round trip: yaw", y / DEG, 45);

    /* at pitch 90 only yaw - roll is defined: we get roll 0 and yaw 45 - 20 = 25 */
    quat_to_euler(quat_from_euler(20 * DEG, 90 * DEG, 45 * DEG), &r, &p, &y);
    check("gimbal lock: roll", r / DEG, 0);
    check("gimbal lock: yaw", y / DEG, 25);

    /* matrix and rotate agree */
    float R[3][3];
    quat_to_matrix(qe, R);
    vec3_t a = quat_rotate(qe, (vec3_t){0.3f, -0.5f, 0.8f});
    check("matrix row 1 = rotate x", R[0][0] * 0.3f - R[0][1] * 0.5f + R[0][2] * 0.8f, a.x);

    /* spin at 90 deg/s about z for 1 s, 200 steps: yaw is 90 */
    quat_t q = quat_identity();
    for (int i = 0; i < 200; i++) q = quat_integrate(q, (vec3_t){0, 0, 90 * DEG}, 0.005f);
    quat_to_euler(q, &r, &p, &y);
    check("spin 1 s at 90 deg/s: yaw", y / DEG, 90);
    check("spin: |q| stays 1", quat_norm(q), 1);

    /* slerp halfway between 0 and 90 deg yaw is 45 deg yaw */
    quat_t h = quat_slerp(quat_identity(), qz, 0.5f);
    quat_to_euler(h, &r, &p, &y);
    check("slerp halfway: yaw", y / DEG, 45);

    printf(fails ? "\n%d test(s) FAILED\n" : "\nall tests passed\n", fails);
    return fails != 0;
}

11.3Run the tests

Put the three files in one folder and run:

gcc test_quaternion.c quaternion.c -lm -o test
./test

You should see this:

90 about z: x -> y, x part            0.0000  (want    0.0000)  ok
90 about z: x -> y, y part            1.0000  (want    1.0000)  ok
qz*qx = [0.500 0.500 0.500 0.500]
qx*qz = [0.500 0.500 -0.500 0.500]
order matters (y parts differ)        1.0000  (want    1.0000)  ok
euler round trip: roll               20.0000  (want   20.0000)  ok
euler round trip: pitch              30.0000  (want   30.0000)  ok
euler round trip: yaw                45.0000  (want   45.0000)  ok
gimbal lock: roll                     0.0000  (want    0.0000)  ok
gimbal lock: yaw                     25.0000  (want   25.0000)  ok
matrix row 1 = rotate x               0.9147  (want    0.9147)  ok
spin 1 s at 90 deg/s: yaw            89.9996  (want   90.0000)  ok
spin: |q| stays 1                     1.0000  (want    1.0000)  ok
slerp halfway: yaw                   45.0000  (want   45.0000)  ok

all tests passed

11.4Use it in your loop

Here is an object that spins, updated 200 times per second. The turn rates rate_x, rate_y, rate_z are in degrees per second, about the object's own axes. They can come from a sensor, a simulation or a controller.

#include "quaternion.h"
#define DEG_TO_RAD 0.0174533f
#define TIME_STEP  0.005f               /* 200 steps per second */

quat_t q = {1.0f, 0.0f, 0.0f, 0.0f};  /* start: no turn */

void every_step(float rate_x, float rate_y, float rate_z)
{
    vec3_t rate = {rate_x * DEG_TO_RAD, rate_y * DEG_TO_RAD, rate_z * DEG_TO_RAD};
    q = quat_integrate(q, rate, TIME_STEP);

    vec3_t front = quat_rotate(q, (vec3_t){1, 0, 0});   /* where the object points, in the world */

    float roll, pitch, yaw;              /* only for showing to a human */
    quat_to_euler(q, &roll, &pitch, &yaw);
}

That's the whole tutorial. You can now read a quaternion, multiply two of them, turn a vector, convert to angles and matrices, follow a spinning object and blend two orientations. If one of these still feels unclear, go back to its lesson and play with the lab once more.

+Sources and further reading

• J. Solà, "Quaternion kinematics for the error-state Kalman filter", 2017
  chapter 3 compares the conventions used by different libraries