Add slerp and qslerp.

2 files changed, 153 insertions, 0 deletions

diff --git a/test/quat_test.c b/test/quat_test.c
new file mode 100644
index 0000000..83519c3
--- /dev/null
+++ b/test/quat_test.c
@@ -0,0 +1,85 @@
+#include <math/quat.h>
+
+#include <math/float.h>
+
+#include "test.h"
+
+#include <stdio.h>
+
+static const float eps = 1e-7;
+
+static inline void print_quat(quat q) {
+  printf("{ %f, %f, %f, %f }\n", q.x, q.y, q.z, q.w);
+}
+
+static inline void print_vec3(vec3 v) {
+  printf("{ %f, %f, %f }\n", v.x, v.y, v.z);
+}
+
+/// Slerp between two vectors forming an acute angle.
+TEST_CASE(quat_slerp_acute_angle) {
+  const R angle1 = 0;
+  const R angle2 = PI / 4;
+  const R t      = 0.5;
+
+  const quat a = qmake_rot(angle1, 0, 0, 1);
+  const quat b = qmake_rot(angle2, 0, 0, 1);
+
+  const quat c      = qslerp(a, b, t);
+  const vec3 result = qrot(c, vec3_make(1, 0, 0));
+
+  const R    angle3   = lerp(angle1, angle2, t);
+  const vec3 expected = vec3_make(cos(angle3), sin(angle3), 0.0);
+  TEST_TRUE(vec3_eq(result, expected, eps));
+}
+
+/// Slerp between two vectors forming an obtuse angle (negative dot product).
+///
+/// The interpolation must follow the shortest path between both vectors.
+TEST_CASE(quat_slerp_obtuse_angle) {
+  const R angle1 = 0;
+  const R angle2 = 3 * PI / 4;
+  const R t      = 0.5;
+
+  const quat a = qmake_rot(angle1, 0, 0, 1);
+  const quat b = qmake_rot(angle2, 0, 0, 1);
+
+  const quat c      = qslerp(a, b, t);
+  const vec3 result = qrot(c, vec3_make(1, 0, 0));
+
+  const R    angle3   = lerp(angle1, angle2, t);
+  const vec3 expected = vec3_make(cos(angle3), sin(angle3), 0.0);
+  TEST_TRUE(vec3_eq(result, expected, eps));
+}
+
+/// Slerp between two vectors forming a reflex angle.
+///
+/// The interpolation must follow the shortest path between both vectors.
+TEST_CASE(quat_slerp_reflex_angle) {
+  const R angle1 = 0;
+  const R angle2 = 5 * PI / 4;
+  const R t      = 0.5;
+
+  const quat a = qmake_rot(angle1, 0, 0, 1);
+  const quat b = qmake_rot(angle2, 0, 0, 1);
+
+  const quat c      = qslerp(a, b, t);
+  const vec3 result = qrot(c, vec3_make(1, 0, 0));
+
+  // Because it's a reflex angle, we expect the rotation to follow the short
+  // path from 'a' down clockwise to 'b'. Could add +PI to the result of lerp(),
+  // but that adds more error than negating cos and sin.
+  const R    angle3   = lerp(angle1, angle2, t);
+  const vec3 expected = vec3_make(-cos(angle3), -sin(angle3), 0.0);
+  TEST_TRUE(vec3_eq(result, expected, eps));
+}
+
+TEST_CASE(quat_mat4_from_quat) {
+  const R    angle = PI / 8;
+  const quat q     = qmake_rot(angle, 0, 0, 1);
+
+  const mat4 m = mat4_from_quat(q);
+  const vec3 p = mat4_mul_vec3(m, vec3_make(1, 0, 0), /*w=*/1);
+
+  TEST_TRUE(vec3_eq(p, vec3_make(cos(angle), sin(angle), 0), eps));
+}
diff --git a/test/vec3_test.c b/test/vec3_test.c
new file mode 100644
index 0000000..886fee3
--- /dev/null
+++ b/test/vec3_test.c
@@ -0,0 +1,68 @@
+#include <math/vec3.h>
+
+#include <math/float.h>
+
+#include "test.h"
+
+#include <stdio.h>
+
+static const float eps = 1e-7;
+
+static inline void print_vec3(vec3 v) {
+  printf("{ %f, %f, %f }\n", v.x, v.y, v.z);
+}
+
+/// Slerp between two vectors forming an acute angle.
+TEST_CASE(vec3_slerp_acute_angle) {
+  const R angle1 = 0;
+  const R angle2 = PI / 4;
+  const R t      = 0.5;
+
+  const vec3 a = vec3_make(cos(angle1), sin(angle1), 0);
+  const vec3 b = vec3_make(cos(angle2), sin(angle2), 0);
+
+  const vec3 result = vec3_slerp(a, b, t);
+
+  const R    angle3   = lerp(angle1, angle2, t);
+  const vec3 expected = vec3_make(cos(angle3), sin(angle3), 0.0);
+  TEST_TRUE(vec3_eq(result, expected, eps));
+}
+
+/// Slerp between two vectors forming an obtuse angle (negative dot product).
+///
+/// The interpolation must follow the shortest path between both vectors.
+TEST_CASE(vec3_slerp_obtuse_angle) {
+  const R angle1 = 0;
+  const R angle2 = 3 * PI / 4;
+  const R t      = 0.5;
+
+  const vec3 a = vec3_make(cos(angle1), sin(angle1), 0);
+  const vec3 b = vec3_make(cos(angle2), sin(angle2), 0);
+
+  const vec3 result = vec3_slerp(a, b, t);
+
+  const R    angle3   = lerp(angle1, angle2, t);
+  const vec3 expected = vec3_make(cos(angle3), sin(angle3), 0.0);
+  TEST_TRUE(vec3_eq(result, expected, eps));
+}
+
+/// Slerp between two vectors forming a reflex angle.
+///
+/// The interpolation must follow the shortest path between both vectors.
+TEST_CASE(vec3_slerp_reflex_angle) {
+  const R angle1 = 0;
+  const R angle2 = 5 * PI / 4;
+  const R t      = 0.5;
+
+  const vec3 a = vec3_make(cos(angle1), sin(angle1), 0);
+  const vec3 b = vec3_make(cos(angle2), sin(angle2), 0);
+
+  const vec3 result = vec3_slerp(a, b, t);
+
+  // slerp goes from a to b following the shortest path, which is down from a
+  // towards b. The resulting angle is therefore +PI of the angle we get from
+  // lerping the two input angles.
+  const R    angle3   = lerp(angle1, angle2, t) + PI;
+  const vec3 expected = vec3_make(cos(angle3), sin(angle3), 0.0);
+  TEST_TRUE(vec3_eq(result, expected, 1e-5));
+}