From fc175dbb72f80764431c1fe82ab8996b114831da Mon Sep 17 00:00:00 2001 From: 3gg <3gg@shellblade.net> Date: Sat, 4 Feb 2023 11:07:21 -0800 Subject: Add slerp and qslerp. --- CMakeLists.txt | 4 ++- include/math/defs.h | 17 ++++++---- include/math/quat.h | 87 +++++++++++++++++++++++++++++++++++++++++++++++-- include/math/spatial3.h | 2 +- include/math/vec3.h | 44 ++++++++++++++++++++++--- test/quat_test.c | 85 +++++++++++++++++++++++++++++++++++++++++++++++ test/vec3_test.c | 68 ++++++++++++++++++++++++++++++++++++++ 7 files changed, 292 insertions(+), 15 deletions(-) create mode 100644 test/quat_test.c create mode 100644 test/vec3_test.c diff --git a/CMakeLists.txt b/CMakeLists.txt index a3e4638..916dbfa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,7 +15,9 @@ target_link_libraries(math INTERFACE # Test. add_executable(math_test - test/mat4_test.c) + test/mat4_test.c + test/quat_test.c + test/vec3_test.c) target_link_libraries(math_test math) 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 + /// 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}; } 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 + +#include + +#include "test.h" + +#include + +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 + +#include + +#include "test.h" + +#include + +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)); +} -- cgit v1.2.3