diff --git a/headers/vmath.hpp/vmath_ext.hpp b/headers/vmath.hpp/vmath_ext.hpp index b9765af..6906715 100644 --- a/headers/vmath.hpp/vmath_ext.hpp +++ b/headers/vmath.hpp/vmath_ext.hpp @@ -942,6 +942,33 @@ namespace vmath_hpp } } + template < typename T > + [[nodiscard]] qua qrotate(const qua& q, const mat& m) { + return q * qrotate(m); + } + + template < typename T > + [[nodiscard]] qua qrotate(const vec& from, const vec& to) { + /// REFERENCE: + /// http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final + + const T n = sqrt(length2(from) * length2(to)); + const T s = dot(from, to) + n; + + if ( s < T(0.000001) * n ) { + return abs(from.x) > abs(from.z) + ? normalize(qua{vec{-from.y, from.x, T(0)}, T(0)}) + : normalize(qua{vec{T(0), -from.z, from.y}, T(0)}); + } + + return normalize(qua{cross(from, to), s}); + } + + template < typename T > + [[nodiscard]] qua qrotate(const qua& q, const vec& from, const vec& to) { + return q * qrotate(from, to); + } + template < typename T > [[nodiscard]] qua qrotate(T angle, const vec& axis) { /// REFERENCE: diff --git a/untests/vmath_ext_tests.cpp b/untests/vmath_ext_tests.cpp index f8addd8..683ed86 100644 --- a/untests/vmath_ext_tests.cpp +++ b/untests/vmath_ext_tests.cpp @@ -16,6 +16,10 @@ namespace { using namespace vmath_hpp; using namespace vmath_tests; + + constexpr float pi = radians(180.f); + constexpr float pi_2 = radians(90.f); + constexpr float pi_4 = radians(45.f); } TEST_CASE("vmath/ext") { @@ -220,10 +224,6 @@ TEST_CASE("vmath/ext") { } SECTION("matrix rotate") { - constexpr float pi = radians(180.f); - constexpr float pi_2 = radians(90.f); - constexpr float pi_4 = radians(45.f); - REQUIRE(float4(0.f,1.f,0.f,1.f) * rotate_x(pi_2) == uapprox4(0.f,0.f,1.f,1.f)); REQUIRE(float4(0.f,0.f,1.f,1.f) * rotate_y(pi_2) == uapprox4(1.f,0.f,0.f,1.f)); REQUIRE(float4(1.f,0.f,0.f,1.f) * rotate_z(pi_2) == uapprox4(0.f,1.f,0.f,1.f)); @@ -339,11 +339,46 @@ TEST_CASE("vmath/ext") { STATIC_REQUIRE(perpendicular(float3(2.f, 2.f, 2.f), float3(0.f, 0.f, 1.f)) == uapprox3(2.f, 2.f, 0.f)); } - SECTION("quaternion qrotate") { - constexpr float pi = radians(180.f); - constexpr float pi_2 = radians(90.f); - constexpr float pi_4 = radians(45.f); + SECTION("quaternion/qrotate(m)") { + REQUIRE(all(approx( + vec{4.f,3.f,2.f,1.f} * rotate(qrotate(float3x3(rotate(0.f, vec{1.f,2.f,3.f})))), + vec{4.f,3.f,2.f,1.f} * rotate(0.f, vec{1.f,2.f,3.f}), 0.001f))); + REQUIRE(all(approx( + vec{4.f,3.f,2.f,1.f} * rotate(qrotate(float3x3(rotate(radians(12.5f), vec{1.f,2.f,3.f})))), + vec{4.f,3.f,2.f,1.f} * rotate(radians(12.5f), vec{1.f,2.f,3.f}), 0.001f))); + REQUIRE(all(approx( + vec{4.f,3.f,2.f,1.f} * rotate(qrotate(float3x3(rotate(radians(-190.5f), vec{1.f,2.f,3.f})))), + vec{4.f,3.f,2.f,1.f} * rotate(radians(-190.5f), vec{1.f,2.f,3.f}), 0.001f))); + } + SECTION("quaternion/qrotate(q, m)") { + REQUIRE(all(approx( + vec{4.f,3.f,2.f} * qrotate( + qrotate(float3x3(rotate(0.f, vec{1.f,2.f,3.f}))), + float3x3(rotate(0.f, vec{3.f,2.f,1.f}))), + vec{4.f,3.f,2.f} * + float3x3(rotate(0.f, vec{1.f,2.f,3.f})) * + float3x3(rotate(0.f, vec{3.f,2.f,1.f}))))); + } + + SECTION("quaternion/qrotate(from, to)") { + REQUIRE(+unit3_x * qrotate(-unit3_x, +unit3_x) == uapprox3(-unit3_x)); + REQUIRE(-unit3_y * qrotate(+unit3_y, -unit3_y) == uapprox3(+unit3_y)); + REQUIRE(+unit3_z * qrotate(-unit3_z, +unit3_z) == uapprox3(-unit3_z)); + REQUIRE(vec{1.f,2.f,3.f} * qrotate(vec{1.f,2.f,3.f}, vec{-2.f,1.f,3.f}) == uapprox3(-2.f,1.f,3.f)); + REQUIRE(vec{-2.f,1.f,3.f} * qrotate(vec{-2.f,1.f,3.f}, vec{1.f,2.f,3.f}) == uapprox3(1.f,2.f,3.f)); + } + + SECTION("quaternion/qrotate(q, from, to)") { + REQUIRE(vec{1.f,2.f,3.f} * + inverse(qrotate(float3x3(rotate(radians(12.f), {2.f,2.f,2.f})))) * + qrotate( + qrotate(float3x3(rotate(radians(12.f), {2.f,2.f,2.f}))), + vec{1.f,2.f,3.f}, + vec{-2.f,1.f,3.f}) == uapprox3(vec{-2.f,1.f,3.f})); + } + + SECTION("quaternion/qrotate(angle, axis)") { 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)))); @@ -352,22 +387,24 @@ TEST_CASE("vmath/ext") { 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)); - REQUIRE(float3(0.f,1.f,0.f) * qrotate_x(qrotate_x(pi_4),pi_4) == uapprox3(0.f,0.f,1.f)); - REQUIRE(float3(0.f,0.f,1.f) * qrotate_y(qrotate_y(pi_4),pi_4) == uapprox3(1.f,0.f,0.f)); - REQUIRE(float3(1.f,0.f,0.f) * qrotate_z(qrotate_z(pi_4),pi_4) == uapprox3(0.f,1.f,0.f)); - REQUIRE(float3(2.f,3.f,4.f) * qrotate(pi,{0.f,0.f,1.f}) == uapprox3(-2.f,-3.f,4.f)); REQUIRE(float3(2.f,3.f,4.f) * qrotate(pi,float3{0.f,0.f,1.f}) == uapprox3(-2.f,-3.f,4.f)); - REQUIRE(float3(2.f,3.f,4.f) * qrotate(qrotate(pi_2,{0.f,0.f,1.f}),pi_2,{0.f,0.f,1.f}) == uapprox3(-2.f,-3.f,4.f)); - REQUIRE(float3(2.f,3.f,4.f) * qrotate(qrotate(pi_2,float3{0.f,0.f,1.f}),pi_2,float3{0.f,0.f,1.f}) == uapprox3(-2.f,-3.f,4.f)); - REQUIRE(qrotate_x(12.3f) == qrotate(12.3f, unit3_x * 2.f)); 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") { + SECTION("quaternion/qrotate(q, angle, axis)") { + REQUIRE(float3(0.f,1.f,0.f) * qrotate_x(qrotate_x(pi_4),pi_4) == uapprox3(0.f,0.f,1.f)); + REQUIRE(float3(0.f,0.f,1.f) * qrotate_y(qrotate_y(pi_4),pi_4) == uapprox3(1.f,0.f,0.f)); + REQUIRE(float3(1.f,0.f,0.f) * qrotate_z(qrotate_z(pi_4),pi_4) == uapprox3(0.f,1.f,0.f)); + + REQUIRE(float3(2.f,3.f,4.f) * qrotate(qrotate(pi_2,{0.f,0.f,1.f}),pi_2,{0.f,0.f,1.f}) == uapprox3(-2.f,-3.f,4.f)); + REQUIRE(float3(2.f,3.f,4.f) * qrotate(qrotate(pi_2,float3{0.f,0.f,1.f}),pi_2,float3{0.f,0.f,1.f}) == uapprox3(-2.f,-3.f,4.f)); + } + + SECTION("quaternion/qlook_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)))))));