Add slerp and qslerp.

4 files changed, 136 insertions, 14 deletions

diff --git a/include/math/defs.h b/include/math/defs.h
index f67ee97..f572d8f 100644
--- a/include/math/defs.h
+++ b/include/math/defs.h
@@ -14,7 +14,7 @@ typedef double R;
 #define R_MIN DBL_MIN
 #define R_MAX DBL_MAX
 #else // floats
-typedef float       R;
+typedef float   R;
 #define R_MIN FLT_MIN
 #define R_MAX FLT_MAX
 #endif
@@ -29,13 +29,15 @@ typedef float       R;
 #define TO_DEG (180.0 / PI)
 
 #ifdef MATH_USE_DOUBLE
-static inline double min(R a, R b) { return fmin(a, b); }
-static inline double max(R a, R b) { return fmax(a, b); }
-static inline double rlog2(R a) { return log2(a); }
+static inline R min(R a, R b) { return fmin(a, b); }
+static inline R max(R a, R b) { return fmax(a, b); }
+static inline R rlog2(R a) { return log2(a); }
+static inline R rmod(R a, R m) { return fmodf(a, m); }
 #else // floats
-static inline float min(R a, R b) { return fminf(a, b); }
-static inline float max(R a, R b) { return fmaxf(a, b); }
-static inline float rlog2(R a) { return log2f(a); }
+static inline R min(R a, R b) { return fminf(a, b); }
+static inline R max(R a, R b) { return fmaxf(a, b); }
+static inline R rlog2(R a) { return log2f(a); }
+static inline R rmod(R a, R m) { return fmod(a, m); }
 #endif
 
 static inline R rabs(R x) { return x >= 0.0 ? x : -x; }
@@ -51,3 +53,4 @@ static inline R sign(R x) {
   }
 }
 static inline R lerp(R a, R b, R t) { return a + (b - a) * t; }
+static inline R R_eq(R a, R b, R eps) { return rabs(a - b) <= eps; }
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));
+}
diff --git a/include/math/spatial3.h b/include/math/spatial3.h
index 8de38bf..9065972 100644
--- a/include/math/spatial3.h
+++ b/include/math/spatial3.h
@@ -118,7 +118,7 @@ static inline void spatial3_set_transform(Spatial3* spatial, mat4 transform) {
 static inline void spatial3_set_forward(Spatial3* spatial, vec3 forward) {
   spatial->f = vec3_normalize(forward);
   // Use aux vector to define right vector orthogonal to forward.
-  if (vec3_eq(vec3_abs(spatial->f), up3())) {
+  if (vec3_eq(vec3_abs(spatial->f), up3(), 1e-9)) {
     spatial->r = vec3_normalize(vec3_cross(spatial->f, forward3()));
   } else {
     spatial->r = vec3_normalize(vec3_cross(spatial->f, up3()));
diff --git a/include/math/vec3.h b/include/math/vec3.h
index 641c02f..d8e1248 100644
--- a/include/math/vec3.h
+++ b/include/math/vec3.h
@@ -51,14 +51,14 @@ static inline vec3 vec3_div(vec3 a, vec3 b) {
   return (vec3){a.x / b.x, a.y / b.y, a.z / b.z};
 }
 
-/// Scale a vector by a scalar value.
+/// Scale the vector.
 static inline vec3 vec3_scale(vec3 v, R s) {
   return (vec3){v.x * s, v.y * s, v.z * s};
 }
 
 /// Compare two vectors for equality.
-static inline bool vec3_eq(vec3 a, vec3 b) {
-  return a.x == b.x && a.y == b.y && a.z == b.z;
+static inline bool vec3_eq(vec3 a, vec3 b, R eps) {
+  return R_eq(a.x, b.x, eps) && R_eq(a.y, b.y, eps) && R_eq(a.z, b.z, eps);
 }
 
 /// Return the absolute value of the vector.
@@ -67,7 +67,9 @@ static inline vec3 vec3_abs(vec3 v) {
 }
 
 /// Compare two vectors for inequality.
-static inline bool vec3_ne(vec3 a, vec3 b) { return !(vec3_eq(a, b)); }
+static inline bool vec3_ne(vec3 a, vec3 b, R eps) {
+  return !(vec3_eq(a, b, eps));
+}
 
 /// Return the vector's squared magnitude.
 static inline R vec3_norm2(vec3 v) { return v.x * v.x + v.y * v.y + v.z * v.z; }
@@ -123,6 +125,40 @@ static inline vec3 vec3_pow(vec3 v, R p) {
   return (vec3){pow(v.x, p), pow(v.y, p), pow(v.z, p)};
 }
 
+/// Linearly interpolate two vectors.
+static inline vec3 vec3_lerp(vec3 a, vec3 b, R t) {
+  return (vec3){
+      .x = a.x + t * (b.x - a.x),
+      .y = a.y + t * (b.y - a.y),
+      .z = a.z + t * (b.z - a.z)};
+}
+
+/// Interpolate two unit vectors using spherical linear interpolation.
+///
+/// Note: You might want to normalize the result.
+static inline vec3 vec3_slerp(vec3 a, vec3 b, R t) {
+  assert(0.0 <= t);
+  assert(t <= 1.0);
+  const R eps = 1e-5;
+  (void)eps;
+  assert(R_eq(vec3_norm2(a), 1.0, eps));
+  assert(R_eq(vec3_norm2(b), 1.0, eps));
+  R dot = vec3_dot(a, b);
+  // For numerical stability, perform linear interpolation when the two vectors
+  // 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.0 - dot * dot);
+    ta                = sin((1.0 - t) * theta) / sin_theta;
+    tb                = sin(t * theta) / sin_theta;
+  } else { // Linear interpolation.
+    ta = 1.0 - t;
+    tb = t;
+  }
+  return vec3_add(vec3_scale(a, ta), vec3_scale(b, tb));
+}
+
 /// The (1, 0, 0) vector.
 static inline vec3 right3() { return (vec3){1.0, 0.0, 0.0}; }