1 files changed, 85 insertions, 2 deletions

diff --git a/include/math/quat.h b/include/math/quat.h
index b284567..a154044 100644
--- a/include/math/quat.h
+++ b/include/math/quat.h
@@ -4,23 +4,38 @@
 #include "mat4.h"
 #include "vec3.h"
 
+#include <assert.h>
+
 /// A quaternion.
 typedef struct quat {
-  R w, x, y, z;
+  R x, y, z, w;
 } quat;
 
+/// Return the unit/identity quaternion.
 static inline quat qunit() {
-  return (quat){.x = 0.0, .y = 0.0, .z = 0.0, .w = 0.0};
+  return (quat){.x = 0.0, .y = 0.0, .z = 0.0, .w = 1.0};
 }
 
+/// Construct a quaternion.
 static inline quat qmake(R x, R y, R z, R w) {
   return (quat){.x = x, .y = y, .z = z, .w = w};
 }
 
+/// Construct a quaternion from an array.
 static inline quat quat_from_array(const R xyzw[4]) {
   return (quat){.x = xyzw[0], .y = xyzw[1], .z = xyzw[2], .w = xyzw[3]};
 }
 
+/// Construct a quaternion from a 3D point.
+static inline quat quat_from_vec3(vec3 p) {
+  return (quat){.x = p.x, .y = p.y, .z = p.z, .w = 0.0};
+}
+
+/// Get a 3D point back from a quaternion.
+static inline vec3 vec3_from_quat(quat q) {
+  return (vec3){.x = q.x, .y = q.y, .z = q.z};
+}
+
 /// Construct a rotation quaternion.
 static inline quat qmake_rot(R angle, R x, R y, R z) {
   const R a   = angle * 0.5;
@@ -34,6 +49,11 @@ static inline quat qmake_rot(R angle, R x, R y, R z) {
   return (quat){x / mag, y / mag, z / mag, w};
 }
 
+/// Add two quaternions.
+static inline quat qadd(quat a, quat b) {
+  return (quat){.x = a.x + b.x, .y = a.y + b.y, .z = a.z + b.z, .w = a.w + b.w};
+}
+
 /// Multiply two quaternions.
 static inline quat qmul(quat q1, quat q2) {
   const R x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
@@ -61,6 +81,37 @@ static inline vec3 qrot(quat q, vec3 v) {
   return vec3_make(u.x, u.y, u.z);
 }
 
+/// Negate the quaternion.
+static inline quat qneg(quat q) {
+  return (quat){.x = -q.x, .y = -q.y, .z = -q.z, .w = -q.w};
+}
+
+/// Return the dot product of two quaternions.
+static inline R qdot(quat a, quat b) {
+  return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+/// Return the quaternion's magnitude.
+static inline R qnorm(quat q) {
+  return sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
+}
+
+/// Return the quaternion's squared magnitude.
+static inline R qnorm2(quat q) {
+  return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
+}
+
+/// Normalize the quaternion.
+static inline quat qnormalize(quat q) {
+  const R n = qnorm(q);
+  return (quat){.x = q.x / n, .y = q.y / n, .z = q.z / n, .w = q.w / n};
+}
+
+/// Scale the quaternion.
+static inline quat qscale(quat q, R s) {
+  return (quat){.x = s * q.x, .y = s * q.y, .z = s * q.z, .w = s * q.w};
+}
+
 /// Get a 4x4 rotation matrix from a quaternion.
 static inline mat4 mat4_from_quat(quat q) {
   const R xx = q.x * q.x;
@@ -77,3 +128,35 @@ static inline mat4 mat4_from_quat(quat q) {
       1 - 2 * xx - 2 * zz, 2 * yz - 2 * wx, 0, 2 * xz - 2 * wy, 2 * yz + 2 * wx,
       1 - 2 * xx - 2 * yy, 0, 0, 0, 0, 1);
 }
+
+/// Interpolate two unit quaternions using spherical linear interpolation.
+///
+/// Note: You might want to normalize the result.
+static inline quat qslerp(quat a, quat b, R t) {
+  assert(0.0 <= t);
+  assert(t <= 1.0);
+  const R eps = 1e-5;
+  (void)eps;
+  assert(R_eq(qnorm2(a), 1.0, eps));
+  assert(R_eq(qnorm2(b), 1.0, eps));
+  R dot = qdot(a, b);
+  // Make the rotation path follow the "short way", i.e., ensure that:
+  // -90 <= angle <= 90
+  if (dot < 0.0) {
+    dot = -dot;
+    b   = qneg(b);
+  }
+  // For numerical stability, perform linear interpolation when the two
+  // quaternions are close to each other.
+  R ta, tb;
+  if (1.0 - dot > 1e-6) {
+    const R theta     = acos(dot);
+    const R sin_theta = sqrt(1 - dot * dot);
+    ta                = sin(theta * (1.0 - t)) / sin_theta;
+    tb                = sin(theta * t) / sin_theta;
+  } else { // Linear interpolation.
+    ta = 1.0 - t;
+    tb = t;
+  }
+  return qadd(qscale(a, ta), qscale(b, tb));
+}