diff --git a/headers/vmath.hpp/vmath_ext.hpp b/headers/vmath.hpp/vmath_ext.hpp index affb519..e23b103 100644 --- a/headers/vmath.hpp/vmath_ext.hpp +++ b/headers/vmath.hpp/vmath_ext.hpp @@ -761,6 +761,62 @@ namespace vmath_hpp { // qrotate + template < typename T > + [[nodiscard]] qua qrotate(const mat& m) { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ + + const T wv = m[0][0] + m[1][1] + m[2][2]; + const T xv = m[0][0] - m[1][1] - m[2][2]; + const T yv = m[1][1] - m[0][0] - m[2][2]; + const T zv = m[2][2] - m[0][0] - m[1][1]; + + struct pmv { + const T v{}; + const int i{}; + }; + + const auto [mv,mi] = max({ + {wv, 0}, + {xv, 1}, + {yv, 2}, + {zv, 3}, + }, [](auto&& l, auto&& r){ + return l.v < r.v; + }); + + const T qv = T(0.5) * sqrt(T(1) + mv); + const T rqv = T(0.25) * rcp(qv); + + switch ( mi ) { + default: + case 0: + return { + (m[1][2] - m[2][1]) * rqv, + (m[2][0] - m[0][2]) * rqv, + (m[0][1] - m[1][0]) * rqv, + qv}; + case 1: + return { + qv, + (m[1][0] + m[0][1]) * rqv, + (m[2][0] + m[0][2]) * rqv, + (m[1][2] - m[2][1]) * rqv}; + case 2: + return { + (m[1][0] + m[0][1]) * rqv, + qv, + (m[2][1] + m[1][2]) * rqv, + (m[2][0] - m[0][2]) * rqv}; + case 3: + return { + (m[2][0] + m[0][2]) * rqv, + (m[2][1] + m[1][2]) * rqv, + qv, + (m[0][1] - m[1][0]) * rqv}; + } + } + template < typename T > [[nodiscard]] qua qrotate(T angle, const vec& axis) { /// REFERENCE: @@ -808,4 +864,30 @@ namespace vmath_hpp [[nodiscard]] qua qrotate_z(const qua& q, T angle) { return qrotate(q, angle, unit3_z); } + + // look_at + + template < typename T > + [[nodiscard]] qua qlook_at_lh(const vec& dir, const vec& up) { + const vec az = normalize(dir); + const vec ax = normalize(cross(up, az)); + const vec ay = cross(az, ax); + + return qrotate(mat{ + ax.x, ay.x, az.x, + ax.y, ay.y, az.y, + ax.z, ay.z, az.z}); + } + + template < typename T > + [[nodiscard]] qua qlook_at_rh(const vec& dir, const vec& up) { + const vec az = normalize(-dir); + const vec ax = normalize(cross(up, az)); + const vec ay = cross(az, ax); + + return qrotate(mat{ + ax.x, ay.x, az.x, + ax.y, ay.y, az.y, + ax.z, ay.z, az.z}); + } } diff --git a/untests/vmath_ext_tests.cpp b/untests/vmath_ext_tests.cpp index 2f318ca..f76d74f 100644 --- a/untests/vmath_ext_tests.cpp +++ b/untests/vmath_ext_tests.cpp @@ -337,4 +337,13 @@ TEST_CASE("vmath/ext") { REQUIRE(qrotate_y(12.3f) == qrotate(12.3f, unit3_y * 2.f)); REQUIRE(qrotate_z(12.3f) == qrotate(12.3f, unit3_z * 2.f)); } + + SECTION("quaternion look_at") { + REQUIRE(all(approx( + qlook_at_lh(float3(1.f,2.f,3.f), float3(0,1,0)), + qrotate(float3x3(look_at_lh(float3(), float3(1.f,2.f,3.f), float3(0,1,0))))))); + REQUIRE(all(approx( + qlook_at_rh(float3(1.f,2.f,3.f), float3(0,1,0)), + qrotate(float3x3(look_at_rh(float3(), float3(1.f,2.f,3.f), float3(0,1,0))))))); + } }