Memo for myself about quaternions (when used to calculate rotation in 3D space).

A quaternion is an extension of the complex numbers. In the context of a spatial rotation, it's a vector of dimension 4 encoding a rotation angle in radians around an axis in 3D. The advantages of using quaternions over rotation matrices are: they use less memory, they are more numerically stable, and they are easier to use.

Structure to memorise a quaternion:

typedef struct CapyQuaternion { // Components (in x,y,z,w order) CapyVec comp; } CapyQuaternion;

A default quaternion corresponding to no rotation has values \((0, 0, 0, 1)\). Using the conventions: same handness for the rotation as for the coordinates system; matrix stored by row. Creating a quaternion from a rotation axis and an angle can be done as follow:

// Create a quaternion for a given rotation axis and angle (right-handed // rotation) // Input: // axis: the axis (normalised) // theta: the angle in radians // Output: // Return a CapyQuaternion equivalent to the rotation. CapyQuaternion CapyQuaternionCreateAxisAngle( CapyVec const* const axis, double const theta) { CapyQuaternion that = CapyQuaternionCreate(); double s = sin(0.5 * theta); loop(i, 3) that.comp.vals[i] = axis->vals[i] * s; that.comp.vals[3] = cos(0.5 * theta); return that; }

Converting a rotation matrix to a quaternion can be done as follow:

// Set the quaternion from a rotation matrix // Input: // mat: the rotation matrix (3x3) // Output: // The quaternion is updated. static void FromRotMat(CapyMat const* const mat) { CapyThatQuaternion; double sumDiag = mat->vals[0] + mat->vals[4] + mat->vals[8]; if(sumDiag > 0.0) { double s = 0.5 / sqrt(1.0 + sumDiag); that->comp.vals[0] = (mat->vals[7] - mat->vals[5]) * s; that->comp.vals[1] = (mat->vals[2] - mat->vals[6]) * s; that->comp.vals[2] = (mat->vals[3] - mat->vals[1]) * s; that->comp.vals[3] = 0.25 / s; } else { if(mat->vals[0] > mat->vals[4] && mat->vals[0] > mat->vals[8]) { double s = 2.0 * sqrt(1.0 + mat->vals[0] - mat->vals[4] - mat->vals[8]); that->comp.vals[0] = 0.25 * s; that->comp.vals[1] = (mat->vals[3] + mat->vals[1]) / s; that->comp.vals[2] = (mat->vals[2] + mat->vals[6]) / s; that->comp.vals[3] = (mat->vals[7] - mat->vals[5]) / s; } else if(mat->vals[4] > mat->vals[8]) { double s = 2.0 * sqrt(1.0 - mat->vals[0] + mat->vals[4] - mat->vals[8]); that->comp.vals[0] = (mat->vals[1] + mat->vals[3]) / s; that->comp.vals[1] = 0.25 * s; that->comp.vals[2] = (mat->vals[5] + mat->vals[7]) / s; that->comp.vals[3] = (mat->vals[2] - mat->vals[6]) / s; } else { double s = 2.0 * sqrt(1.0 - mat->vals[0] - mat->vals[4] + mat->vals[8]); that->comp.vals[0] = (mat->vals[6] + mat->vals[2]) / s; that->comp.vals[1] = (mat->vals[5] + mat->vals[7]) / s; that->comp.vals[2] = 0.25 * s; that->comp.vals[3] = (mat->vals[3] - mat->vals[1]) / s; } } CapyVecNormalise(&(that->comp)); }

Converting a quaternion to a rotation matrix can be done as follow:

// Convert the quaternion to a rotation matrix // Input: // mat: the rotation matrix (3x3) // Output: // The rotation matrix is updated. static void ToRotMat(CapyMat* const mat) { CapyThatQuaternion; double xx = that->comp.vals[0] * that->comp.vals[0]; double yy = that->comp.vals[1] * that->comp.vals[1]; double zz = that->comp.vals[2] * that->comp.vals[2]; double xy = that->comp.vals[0] * that->comp.vals[1]; double xz = that->comp.vals[0] * that->comp.vals[2]; double yz = that->comp.vals[1] * that->comp.vals[2]; double xw = that->comp.vals[0] * that->comp.vals[3]; double yw = that->comp.vals[1] * that->comp.vals[3]; double zw = that->comp.vals[2] * that->comp.vals[3]; mat->vals[0] = 1.0 - 2.0 * (yy + zz); mat->vals[1] = 2.0 * (xy - zw); mat->vals[2] = 2.0 * (xz + yw); mat->vals[3] = 2.0 * (xy + zw); mat->vals[4] = 1.0 - 2.0 * (xx + zz); mat->vals[5] = 2.0 * (yz - xw); mat->vals[6] = 2.0 * (xz - yw); mat->vals[7] = 2.0 * (yz + xw); mat->vals[8] = 1.0 - 2.0 * (xx + yy); }

Adding two quaternions can be done as follow:

// Add two quaternions. // Input: // qa: the quaternion to add // qb: the result quaternion // Output: // qb is updated with the quaternion representing the rotation of that // followed by the rotation of qa. qb can be 'that' or qa. static void Add( CapyQuaternion const* const qa, CapyQuaternion* const qb) { CapyThatQuaternion; CapyVec v = {.dim = 4, .vals = (double[4]){}}; v.vals[0] = qa->comp.vals[3] * that->comp.vals[0] + qa->comp.vals[0] * that->comp.vals[3] - qa->comp.vals[1] * that->comp.vals[2] + qa->comp.vals[2] * that->comp.vals[1]; v.vals[1] = qa->comp.vals[3] * that->comp.vals[1] + qa->comp.vals[0] * that->comp.vals[2] + qa->comp.vals[1] * that->comp.vals[3] - qa->comp.vals[2] * that->comp.vals[0]; v.vals[2] = qa->comp.vals[3] * that->comp.vals[2] - qa->comp.vals[0] * that->comp.vals[1] + qa->comp.vals[1] * that->comp.vals[0] + qa->comp.vals[2] * that->comp.vals[3]; v.vals[3] = qa->comp.vals[3] * that->comp.vals[3] - qa->comp.vals[0] * that->comp.vals[0] - qa->comp.vals[1] * that->comp.vals[1] - qa->comp.vals[2] * that->comp.vals[2]; loop(i, 4) qb->comp.vals[i] = v.vals[i]; CapyVecNormalise(&(qb->comp)); }

Substracting two quaternions can be done as follow:

// Substract two quaternions. // Input: // qa: the quaternion to substract // qb: the result quaternion // Output: // qb is updated with the quaternion representing the rotation of that // followed by the inverse rotation of qa. qb can be 'that' or qa. static void Sub( CapyQuaternion const* const qa, CapyQuaternion* const qb) { CapyThatQuaternion; CapyVec v = {.dim = 4, .vals = (double[4]){}}; v.vals[0] = qa->comp.vals[3] * that->comp.vals[0] + -qa->comp.vals[0] * that->comp.vals[3] - -qa->comp.vals[1] * that->comp.vals[2] + -qa->comp.vals[2] * that->comp.vals[1]; v.vals[1] = qa->comp.vals[3] * that->comp.vals[1] + -qa->comp.vals[0] * that->comp.vals[2] + -qa->comp.vals[1] * that->comp.vals[3] - -qa->comp.vals[2] * that->comp.vals[0]; v.vals[2] = qa->comp.vals[3] * that->comp.vals[2] - -qa->comp.vals[0] * that->comp.vals[1] + -qa->comp.vals[1] * that->comp.vals[0] + -qa->comp.vals[2] * that->comp.vals[3]; v.vals[3] = qa->comp.vals[3] * that->comp.vals[3] - -qa->comp.vals[0] * that->comp.vals[0] - -qa->comp.vals[1] * that->comp.vals[1] - -qa->comp.vals[2] * that->comp.vals[2]; loop(i, 4) qb->comp.vals[i] = v.vals[i]; CapyVecNormalise(&(qb->comp)); }

The conjugate of a quaternion (the inverse rotation) can be calculated as follow:

// Calculate the conjugate of the quaternion (ie the inverse rotation of // 'that') // Input: // q: the result conjugate // Output: // q is updated. q can be 'that' static void Conjugate(CapyQuaternion* const q) { CapyThatQuaternion; loop(i, 3) q->comp.vals[i] = -1.0 * that->comp.vals[i]; q->comp.vals[3] = that->comp.vals[3]; }

Applying a quaternion to a vector can be done as follow:

// Apply a quaternion to a vector (ie rotate the vector by the rotation // equivalent to the quaternion) // Input: // u: the vector to be rotated // v: the result vector // Output: // v is updated. v can be u. static void Apply( CapyVec const* const u, CapyVec* const v) { CapyThatQuaternion; CapyVec p = {.dim = 4, .vals = (double[4]){}}; loop(i, 3) p.vals[i] = u->vals[i]; p.vals[3] = 0.0; CapyVec w = {.dim = 4, .vals = (double[4]){}}; w.vals[0] = p.vals[3] * that->comp.vals[0] + p.vals[0] * that->comp.vals[3] - p.vals[1] * that->comp.vals[2] + p.vals[2] * that->comp.vals[1]; w.vals[1] = p.vals[3] * that->comp.vals[1] + p.vals[0] * that->comp.vals[2] + p.vals[1] * that->comp.vals[3] - p.vals[2] * that->comp.vals[0]; w.vals[2] = p.vals[3] * that->comp.vals[2] - p.vals[0] * that->comp.vals[1] + p.vals[1] * that->comp.vals[0] + p.vals[2] * that->comp.vals[3]; w.vals[3] = p.vals[3] * that->comp.vals[3] - p.vals[0] * that->comp.vals[0] - p.vals[1] * that->comp.vals[1] - p.vals[2] * that->comp.vals[2]; loop(i, 3) p.vals[i] = -1.0 * that->comp.vals[i]; p.vals[3] = that->comp.vals[3]; v->vals[0] = p.vals[3] * w.vals[0] + p.vals[0] * w.vals[3] - p.vals[1] * w.vals[2] + p.vals[2] * w.vals[1]; v->vals[1] = p.vals[3] * w.vals[1] + p.vals[0] * w.vals[2] + p.vals[1] * w.vals[3] - p.vals[2] * w.vals[0]; v->vals[2] = p.vals[3] * w.vals[2] - p.vals[0] * w.vals[1] + p.vals[1] * w.vals[0] + p.vals[2] * w.vals[3]; }

Applying the conjugate of a quaternion to a vector can be done as follow:

// Apply the conjugate of a quaternion to a vector (ie rotate the vector by // the opposite rotation equivalent to the quaternion) // Input: // u: the vector to be rotated // v: the result vector // Output: // v is updated. v can be u. static void ApplyInverse( CapyVec const* const u, CapyVec* const v) { CapyThatQuaternion; CapyVec p = {.dim = 4, .vals = (double[4]){}}; loop(i, 3) p.vals[i] = u->vals[i]; p.vals[3] = 0.0; CapyVec w = {.dim = 4, .vals = (double[4]){}}; w.vals[0] = p.vals[3] * -that->comp.vals[0] + p.vals[0] * that->comp.vals[3] - p.vals[1] * -that->comp.vals[2] + p.vals[2] * -that->comp.vals[1]; w.vals[1] = p.vals[3] * -that->comp.vals[1] + p.vals[0] * -that->comp.vals[2] + p.vals[1] * that->comp.vals[3] - p.vals[2] * -that->comp.vals[0]; w.vals[2] = p.vals[3] * -that->comp.vals[2] - p.vals[0] * -that->comp.vals[1] + p.vals[1] * -that->comp.vals[0] + p.vals[2] * that->comp.vals[3]; w.vals[3] = p.vals[3] * that->comp.vals[3] - p.vals[0] * -that->comp.vals[0] - p.vals[1] * -that->comp.vals[1] - p.vals[2] * -that->comp.vals[2]; loop(i, 3) p.vals[i] = that->comp.vals[i]; p.vals[3] = that->comp.vals[3]; v->vals[0] = p.vals[3] * w.vals[0] + p.vals[0] * w.vals[3] - p.vals[1] * w.vals[2] + p.vals[2] * w.vals[1]; v->vals[1] = p.vals[3] * w.vals[1] + p.vals[0] * w.vals[2] + p.vals[1] * w.vals[3] - p.vals[2] * w.vals[0]; v->vals[2] = p.vals[3] * w.vals[2] - p.vals[0] * w.vals[1] + p.vals[1] * w.vals[0] + p.vals[2] * w.vals[3]; }

The rotation angle of the quaternion can be obtained as follow:

// Get the angle of the quaternion // Output: // Return the angle in radians static double GetAngle(void) { CapyThatQuaternion; return 2.0 * acos(that->comp.vals[3]); }

The rotation axis of a quaternion can be calculated as follow:

// Get the rotation axis of the quaternion // Input: // axis: the result rotation axis // Output: // The result rotation axis is updated (null vector if no rotation) static void GetAxis(CapyVec* const axis) { CapyThatQuaternion; double s = sin(acos(that->comp.vals[3])); if(s > DBL_EPSILON) { double inv = 1.0 / s; loop(i, 3) axis->vals[i] = that->comp.vals[i] * inv; } else loop(i, 3) axis->vals[i] = 0.0; }

The rotation angle of a quaternion can be increased as follow:

// Increase the rotation angle of the quaternion. // Input: // theta: angle in radians // Output: // The quaternion is updated. static void AddAngle(double const theta) { CapyThatQuaternion; double phi = $(that, getAngle)(); phi += theta; CapyVec axis = {.dim = 3, .vals = (double[3]){}}; $(that, getAxis)(&axis); double s = sin(0.5 * phi); loop(i, 3) that->comp.vals[i] = axis.vals[i] * s; that->comp.vals[3] = cos(0.5 * phi); CapyVecNormalise(&(that->comp)); }

Unit test to check that the implementation is correct:

void Test() { loop(stepX, 11) loop(stepY, 11) loop(stepZ, 11) { CapyMat rotX = CapyMatCreateRotMat(0, CapyDegToRad(36.0 * (double)stepX)); CapyMat rotY = CapyMatCreateRotMat(1, CapyDegToRad(36.0 * (double)stepY)); CapyMat rotZ = CapyMatCreateRotMat(2, CapyDegToRad(36.0 * (double)stepZ)); CapyMat rotZY = CapyMatCreate(3, 3); CapyMatProdMat(&rotZ, &rotY, &rotZY); CapyMat rotZYX = CapyMatCreate(3, 3); CapyMatProdMat(&rotZY, &rotX, &rotZYX); CapyQuaternion quatFromMat = CapyQuaternionCreate(); $(&quatFromMat, fromRotMat)(&rotZYX); CapyVec axis = CapyVecCreate(3); $(&quatFromMat, getAxis)(&axis); double theta = $(&quatFromMat, getAngle)(); CapyQuaternion quat = CapyQuaternionCreateAxisAngle(&axis, theta); loop(i, 4) { assert(fabs(quatFromMat.comp.vals[i] - quat.comp.vals[i]) < 1e-9); } CapyMat rotMat = CapyMatCreate(3, 3); $(&quat, toRotMat)(&rotMat); loop(i, 9) assert(fabs(rotMat.vals[i] - rotZYX.vals[i]) < 1e-9); CapyVec v = CapyVecCreate(3); loop(i, 3) v.vals[i] = 1.0 + i; CapyVec u = CapyVecCreate(3); CapyVec w = CapyVecCreate(3); CapyMatProdVec(&rotZYX, &v, &u); $(&quat, apply)(&v, &w); loop(i, 3) assert(fabs(u.vals[i] - w.vals[i]) < 1e-9); $(&quat, applyInverse)(&w, &u); loop(i, 3) assert(fabs(v.vals[i] - u.vals[i]) < 1e-9); CapyQuaternion addQuat = CapyQuaternionCreate(); $(&quat, add)(&quatFromMat, &addQuat); CapyMatProdVec(&rotZYX, &w, &u); $(&addQuat, apply)(&v, &w); loop(i, 3) assert(fabs(u.vals[i] - w.vals[i]) < 1e-9); $(&addQuat, sub)(&quatFromMat, &addQuat); loop(i, 4) assert(fabs(addQuat.comp.vals[i] - quat.comp.vals[i]) < 1e-9); theta = $(&quat, getAngle)(); $(&quat, addAngle)(theta); $(&quat, apply)(&v, &w); CapyMatProdVec(&rotZYX, &v, &u); CapyMatProdVec(&rotZYX, &u, &v); loop(i, 3) assert(fabs(v.vals[i] - w.vals[i]) < 1e-9); CapyVecDestruct(&w); CapyVecDestruct(&u); CapyVecDestruct(&v); CapyMatDestruct(&rotMat); CapyQuaternionDestruct(&addQuat); CapyQuaternionDestruct(&quat); CapyVecDestruct(&axis); CapyQuaternionDestruct(&quatFromMat); CapyMatDestruct(&rotX); CapyMatDestruct(&rotY); CapyMatDestruct(&rotZ); CapyMatDestruct(&rotZY); CapyMatDestruct(&rotZYX); } }