diff --git a/headers/vmath.hpp/vmath_qua_fun.hpp b/headers/vmath.hpp/vmath_qua_fun.hpp index 4da69d3..7225e3c 100644 --- a/headers/vmath.hpp/vmath_qua_fun.hpp +++ b/headers/vmath.hpp/vmath_qua_fun.hpp @@ -161,6 +161,43 @@ namespace vmath_hpp namespace vmath_hpp { + template < typename T > + [[nodiscard]] qua lerp(const qua& xs, const qua& ys, T a) { + return qua(lerp(vec{xs}, vec{ys}, a)); + } + + template < typename T > + [[nodiscard]] qua lerp(const qua& xs, const qua& ys, T xs_a, T ys_a) { + return qua(lerp(vec{xs}, vec{ys}, xs_a, ys_a)); + } + + template < typename T > + [[nodiscard]] qua nlerp(const qua& unit_xs, const qua& unit_ys, T a) { + const T xs_scale = T(1) - a; + const T ys_scale = a * sign(dot(unit_xs, unit_ys)); + return normalize(lerp(unit_xs, unit_ys, xs_scale, ys_scale)); + } + + template < typename T > + [[nodiscard]] qua slerp(const qua& unit_xs, const qua& unit_ys, T a) { + const T raw_cos_theta = dot(unit_xs, unit_ys); + const T raw_cos_theta_sign = sign(raw_cos_theta); + + // half degree linear threshold: cos((pi / 180) * 0.25) + if ( const T cos_theta = raw_cos_theta * raw_cos_theta_sign; cos_theta < T(0.99999) ) { + const T theta = acos(cos_theta); + const T rsin_theta = rsqrt(T(1) - sqr(cos_theta)); + const T xs_scale = sin((T(1) - a) * theta) * rsin_theta; + const T ys_scale = sin(a * theta) * raw_cos_theta_sign * rsin_theta; + return lerp(unit_xs, unit_ys, xs_scale, ys_scale); + } else { + // use linear interpolation for small angles + const T xs_scale = T(1) - a; + const T ys_scale = a * raw_cos_theta_sign; + return normalize(lerp(unit_xs, unit_ys, xs_scale, ys_scale)); + } + } + template < typename T > [[nodiscard]] vec isnan(const qua& xs) { return isnan(vec{xs}); diff --git a/untests/vmath_qua_fun_tests.cpp b/untests/vmath_qua_fun_tests.cpp index c367be8..7025534 100644 --- a/untests/vmath_qua_fun_tests.cpp +++ b/untests/vmath_qua_fun_tests.cpp @@ -73,9 +73,76 @@ TEST_CASE("vmath/qua_fun") { } SECTION("Common Functions") { - REQUIRE_FALSE(any(isnan(fqua(1,1,1,1)))); - REQUIRE_FALSE(any(isinf(fqua(1,1,1,1)))); - REQUIRE(all(isfinite(fqua(1,1,1,1)))); + { + REQUIRE(all(approx( + qrotate_z(radians(5.f)), + nlerp(qrotate_z(radians(5.f)), qrotate_z(radians(15.f)), 0.f), + 0.00001f))); + REQUIRE(all(approx( + qrotate_z(radians(6.f)), + nlerp(qrotate_z(radians(5.f)), qrotate_z(radians(15.f)), 0.1f), + 0.00001f))); + REQUIRE(all(approx( + qrotate_z(radians(10.f)), + nlerp(qrotate_z(radians(5.f)), qrotate_z(radians(15.f)), 0.5f), + 0.00001f))); + REQUIRE(all(approx( + qrotate_z(radians(15.f)), + nlerp(qrotate_z(radians(5.f)), qrotate_z(radians(15.f)), 1.f), + 0.00001f))); + + REQUIRE(all(approx( + qrotate_z(radians(315.f)), + nlerp(qrotate_z(radians(270.f)), qrotate_z(radians(0.f)), 0.5f)))); + REQUIRE(all(approx( + qrotate_z(radians(290.f)), + nlerp(qrotate_z(radians(220.f)), qrotate_z(radians(0.f)), 0.5f)))); + } + { + REQUIRE(all(approx( + qrotate_z(radians(5.f)), + slerp(qrotate_z(radians(5.f)), qrotate_z(radians(15.f)), 0.f), + 0.00001f))); + REQUIRE(all(approx( + qrotate_z(radians(6.f)), + slerp(qrotate_z(radians(5.f)), qrotate_z(radians(15.f)), 0.1f), + 0.00001f))); + REQUIRE(all(approx( + qrotate_z(radians(10.f)), + slerp(qrotate_z(radians(5.f)), qrotate_z(radians(15.f)), 0.5f), + 0.00001f))); + REQUIRE(all(approx( + qrotate_z(radians(15.f)), + slerp(qrotate_z(radians(5.f)), qrotate_z(radians(15.f)), 1.f), + 0.00001f))); + + REQUIRE(all(approx( + qrotate_z(radians(0.f)), + slerp(qrotate_z(radians(0.f)), qrotate_z(radians(0.f)), 0.5f)))); + REQUIRE(all(approx( + qrotate_z(radians(0.25f)), + slerp(qrotate_z(radians(0.f)), qrotate_z(radians(0.5f)), 0.5f)))); + + REQUIRE(all(approx( + qrotate_z(radians(-45.f)), + slerp(qrotate_z(radians(0.f)), qrotate_z(radians(270.f)), 0.5f)))); + REQUIRE(all(approx( + qrotate_z(radians(-70.f)), + slerp(qrotate_z(radians(0.f)), qrotate_z(radians(220.f)), 0.5f)))); + + REQUIRE(all(approx( + qrotate_z(radians(315.f)), + slerp(qrotate_z(radians(270.f)), qrotate_z(radians(0.f)), 0.5f)))); + REQUIRE(all(approx( + qrotate_z(radians(290.f)), + slerp(qrotate_z(radians(220.f)), qrotate_z(radians(0.f)), 0.5f)))); + } + + { + REQUIRE_FALSE(any(isnan(fqua(1,1,1,1)))); + REQUIRE_FALSE(any(isinf(fqua(1,1,1,1)))); + REQUIRE(all(isfinite(fqua(1,1,1,1)))); + } } SECTION("Geometric Functions") {