diff --git a/headers/vmath.hpp/vmath_ext.hpp b/headers/vmath.hpp/vmath_ext.hpp index e9860b7..da06640 100644 --- a/headers/vmath.hpp/vmath_ext.hpp +++ b/headers/vmath.hpp/vmath_ext.hpp @@ -229,6 +229,32 @@ namespace vmath_hpp // rotate + template < typename T > + [[nodiscard]] mat rotate(const qua& q) { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/ + + const auto [qv, qs] = normalize(q); + + const T xx = qv.x * qv.x; + const T yy = qv.y * qv.y; + const T zz = qv.z * qv.z; + + const T xy = qv.x * qv.y; + const T xz = qv.x * qv.z; + const T yz = qv.y * qv.z; + + const T xw = qv.x * qs; + const T yw = qv.y * qs; + const T zw = qv.z * qs; + + return { + T(1) - (T(2) * (yy + zz)), T(2) * (xy + zw), T(2) * (xz - yw), 0, + T(2) * (xy - zw), T(1) - (T(2) * (xx + zz)), T(2) * (yz + xw), 0, + T(2) * (xz + yw), T(2) * (yz - xw), T(1) - (T(2) * (xx + yy)), 0, + 0, 0, 0, 1}; + } + template < typename T > [[nodiscard]] mat rotate(T angle, const vec& axis) { const auto [s, c] = sincos(angle); diff --git a/untests/vmath_ext_tests.cpp b/untests/vmath_ext_tests.cpp index 89c877b..2f318ca 100644 --- a/untests/vmath_ext_tests.cpp +++ b/untests/vmath_ext_tests.cpp @@ -315,6 +315,10 @@ TEST_CASE("vmath/ext") { constexpr float pi_2 = radians(90.f); constexpr float pi_4 = radians(45.f); + REQUIRE(all(approx( + rotate(12.3f, float3(1.f,2.f,3.f)), + rotate(qrotate(12.3f, float3(1.f,2.f,3.f)) * 2.f)))); + REQUIRE(float3(0.f,1.f,0.f) * qrotate_x(pi_2) == uapprox3(0.f,0.f,1.f)); REQUIRE(float3(0.f,0.f,1.f) * qrotate_y(pi_2) == uapprox3(1.f,0.f,0.f)); REQUIRE(float3(1.f,0.f,0.f) * qrotate_z(pi_2) == uapprox3(0.f,1.f,0.f));