1 files changed, 40 insertions, 4 deletions

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}; }