From 3b2e49a00d867316b626723e617814fc8ac20855 Mon Sep 17 00:00:00 2001 From: Squareys Date: Mon, 5 Oct 2015 23:09:09 +0200 Subject: [PATCH 1/7] Math: Add test for DualQuaternion screw interpolation (ScLERP). A test driven approach to this abstract math stuff. Signed-off-by: Squareys --- src/Magnum/Math/Test/DualQuaternionTest.cpp | 25 +++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/Magnum/Math/Test/DualQuaternionTest.cpp b/src/Magnum/Math/Test/DualQuaternionTest.cpp index 69a54ea61..856ca6e53 100644 --- a/src/Magnum/Math/Test/DualQuaternionTest.cpp +++ b/src/Magnum/Math/Test/DualQuaternionTest.cpp @@ -86,6 +86,8 @@ struct DualQuaternionTest: Corrade::TestSuite::Tester { void transformPoint(); void transformPointNormalized(); + void sclerp(); + void debug(); }; @@ -125,6 +127,8 @@ DualQuaternionTest::DualQuaternionTest() { &DualQuaternionTest::transformPoint, &DualQuaternionTest::transformPointNormalized, + &DualQuaternionTest::sclerp, + &DualQuaternionTest::debug}); } @@ -383,6 +387,27 @@ void DualQuaternionTest::debug() { CORRADE_COMPARE(o.str(), "DualQuaternion({{1, 2, 3}, -4}, {{0.5, -3.1, 3.3}, 2})\n"); } +void DualQuaternionTest::sclerp() { + + const DualQuaternion from = DualQuaternion::translation(Vector3{20.0f, .0f, .0f})*DualQuaternion::rotation(180.0_degf, Vector3{.0f, 1.0f, .0f}); + const DualQuaternion to = DualQuaternion::translation(Vector3{42.0f, 42.0f, 42.0f})*DualQuaternion::rotation(75.0_degf, Vector3{1.0f, .0f, .0f}); + + constexpr DualQuaternion expected1{Quaternion{{.23296291314453416f, .9238795325112867f, .0f}, .303603179340959f}, + Quaternion{{2.235619101917766f, 2.8169719855488395f, 10.722240915237789f}, -10.287636336847847f}}; + constexpr DualQuaternion expected2{Quaternion{{.4437679833315842f, .6845471059286887f, .0f}, .5783296955322937f}, + Quaternion{{5.764394870292371f, 11.161306653193549f, 9.671267015501789f}, -17.634394590712066f}}; + constexpr DualQuaternion expected3{Quaternion{{.5979785904506439f, .18738131458572468f, .0f}, .7793008714910992f}, + Quaternion{{13.409627907069353f, 25.452124456683414f, 5.681581047706807f}, -16.409481115504978f}}; + + const DualQuaternion interp1 = Math::sclerp(from, to, 0.25f); + const DualQuaternion interp2 = Math::sclerp(from, to, 0.52f); + const DualQuaternion interp3 = Math::sclerp(from, to, 0.88f); + + CORRADE_COMPARE(interp1, expected1); + CORRADE_COMPARE(interp2, expected2); + CORRADE_COMPARE(interp3, expected3); +} + }}} CORRADE_TEST_MAIN(Magnum::Math::Test::DualQuaternionTest) From f8199f35ac6e623eab952393f52da78add482c42 Mon Sep 17 00:00:00 2001 From: Squareys Date: Wed, 7 Oct 2015 22:02:02 +0200 Subject: [PATCH 2/7] Math: Implement DualQuaternion screw interpolation (ScLERP). Signed-off-by: Squareys --- src/Magnum/Math/DualQuaternion.h | 52 ++++++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) diff --git a/src/Magnum/Math/DualQuaternion.h b/src/Magnum/Math/DualQuaternion.h index 154afb96f..c45173a29 100644 --- a/src/Magnum/Math/DualQuaternion.h +++ b/src/Magnum/Math/DualQuaternion.h @@ -29,6 +29,8 @@ * @brief Class @ref Magnum::Math::DualQuaternion */ +#include + #include "Magnum/Math/Dual.h" #include "Magnum/Math/Matrix4.h" #include "Magnum/Math/Quaternion.h" @@ -39,6 +41,56 @@ namespace Implementation { template struct DualQuaternionConverter; } +/** @relatesalso Dual quaternion +@brief Screw linear interpolation of two dual quaternions +@param from First quaternion +@param to Second quaternion +@param t Interpolation phase (from range @f$ [0; 1] @f$) + +Expects that the real parts of both dual quaternions are normalized. @f[ +\begin{array}{l} + {\hat q}_{ScLERP} = \hat p (\hat p^* \hat q)^t = + cos \left( t \frac {\hat a} 2 \right) + \hat {\boldsymbol n} \cdot sin \left( t \frac {\hat a} 2 \right) \\ + \hat d = p^*q = [l, m] \\ + \hat a = [\hat a_R, \hat a_D]; \hat a_R = 2 \cdot acos \left( l_S \right); \hat a_D = -2m_S \frac 1 {|l_V|} \\ + \hat {\boldsymbol n} = [\hat l_V \frac 1 {|l_V|}, + \left( m_V - \hat {\boldsymbol n}_R \frac {\hat a_D l_S} 2 \right)\frac 1 {|l_V|}] +\end{array} +@f] +@see @ref Quaternion::isNormalized(), @ref slerp(const Quaternion&, const Quaternion&, T), + @ref lerp(const T&, const T&, U) +*/ +template inline DualQuaternion sclerp(const DualQuaternion& from, const DualQuaternion& to, const T t) { + CORRADE_ASSERT(from.real().isNormalized() && to.real().isNormalized(), + "Math::sclerp(): real parts of quaternions must be normalized", {}); + + const T dotResult = dot(from.real().vector(), to.real().vector()); + + /* Multiplying with -1.0f ensures shortest path when dot < 0. */ + const DualQuaternion diff = from.quaternionConjugated()*((dotResult < .0) ? -to : to); + + const Vector3 diffReal = diff.real().vector(); + const Vector3 diffDual = diff.dual().vector(); + + const T invr = 1/std::sqrt(diffReal.dot()); + + T angle = 2*acos(diff.real().scalar()); + T pitch = -2*diff.dual().scalar()*invr; + const Vector3 direction = diffReal*invr; + const Vector3 moment = (diffDual - (direction*(pitch*diff.real().scalar()*.5f)))*invr; + + angle *= t; + pitch *= t; + + const T sinAngle = sin(.5f*angle); + const T cosAngle = cos(.5f*angle); + + const Vector3 v = direction*sinAngle; + const Vector3 v2 = moment*sinAngle + direction*(pitch*.5f*cosAngle); + + return from*DualQuaternion{Quaternion{v, cosAngle}, Quaternion{v2, -pitch*.5f*sinAngle}}; +} + /** @brief Dual quaternion @tparam T Underlying data type From 0bfe2abed48dd3bcc8d4874277c7e33553c0cefc Mon Sep 17 00:00:00 2001 From: Squareys Date: Wed, 7 Oct 2015 22:06:46 +0200 Subject: [PATCH 3/7] Math: Optimize ScLERP implementation. Optimized with simple code tricks, some very complex math (like `2*0.5=1`), and principal of locality. Things the compiler would probably do for me anyway. Was able to remove about 6 useless float multiplications. Signed-off-by: Squareys --- src/Magnum/Math/DualQuaternion.h | 24 ++++++++++-------------- 1 file changed, 10 insertions(+), 14 deletions(-) diff --git a/src/Magnum/Math/DualQuaternion.h b/src/Magnum/Math/DualQuaternion.h index c45173a29..441015d05 100644 --- a/src/Magnum/Math/DualQuaternion.h +++ b/src/Magnum/Math/DualQuaternion.h @@ -69,26 +69,22 @@ template inline DualQuaternion sclerp(const DualQuaternion& from, /* Multiplying with -1.0f ensures shortest path when dot < 0. */ const DualQuaternion diff = from.quaternionConjugated()*((dotResult < .0) ? -to : to); - const Vector3 diffReal = diff.real().vector(); - const Vector3 diffDual = diff.dual().vector(); + const T angle = acos(diff.real().scalar())*t; + const T sinAngle = sin(angle); + const T cosAngle = cos(angle); + const Vector3& diffReal = diff.real().vector(); const T invr = 1/std::sqrt(diffReal.dot()); - - T angle = 2*acos(diff.real().scalar()); - T pitch = -2*diff.dual().scalar()*invr; const Vector3 direction = diffReal*invr; - const Vector3 moment = (diffDual - (direction*(pitch*diff.real().scalar()*.5f)))*invr; - - angle *= t; - pitch *= t; - - const T sinAngle = sin(.5f*angle); - const T cosAngle = cos(.5f*angle); const Vector3 v = direction*sinAngle; - const Vector3 v2 = moment*sinAngle + direction*(pitch*.5f*cosAngle); - return from*DualQuaternion{Quaternion{v, cosAngle}, Quaternion{v2, -pitch*.5f*sinAngle}}; + T pitch = -diff.dual().scalar()*invr; + const Vector3 moment = (diff.dual().vector() - (direction*(pitch*diff.real().scalar())))*invr; + pitch *= t; + const Vector3 v2 = moment*sinAngle + direction*(pitch*cosAngle); + + return from*DualQuaternion{Quaternion{v, cosAngle}, Quaternion{v2, -pitch*sinAngle}}; } /** From 030c095c85449755b24c3c549ff95608051e4fe5 Mon Sep 17 00:00:00 2001 From: Squareys Date: Fri, 9 Oct 2015 00:12:17 +0200 Subject: [PATCH 4/7] Math: Add inline comments to reference the TeX formula from code. Should help people understand the code and counteracts the unreadability caused by the optimization commit at least a bit. Signed-off-by: Squareys --- src/Magnum/Math/DualQuaternion.h | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/src/Magnum/Math/DualQuaternion.h b/src/Magnum/Math/DualQuaternion.h index 441015d05..59fe3dbea 100644 --- a/src/Magnum/Math/DualQuaternion.h +++ b/src/Magnum/Math/DualQuaternion.h @@ -67,21 +67,32 @@ template inline DualQuaternion sclerp(const DualQuaternion& from, const T dotResult = dot(from.real().vector(), to.real().vector()); /* Multiplying with -1.0f ensures shortest path when dot < 0. */ + /* diff = \hat d = p^*q */ const DualQuaternion diff = from.quaternionConjugated()*((dotResult < .0) ? -to : to); + /* angle = t \hat a_D */ const T angle = acos(diff.real().scalar())*t; + /* precompute sin/cos for manifold use */ const T sinAngle = sin(angle); const T cosAngle = cos(angle); - const Vector3& diffReal = diff.real().vector(); - const T invr = 1/std::sqrt(diffReal.dot()); - const Vector3 direction = diffReal*invr; + /* merely a shortcut */ + const Vector3& m = diff.real().vector(); + /* invr = \frac 1 {|l_V|} */ + const T invr = 1/m.length(); + /* direction = \hat {\boldsymbol n} = \hat l_V \frac 1 {|l_V|} */ + const Vector3 direction = m*invr; + /* Vector of real part of q_{ScLERP} + = \hat {\boldsymbol n}_R \cdot sin \left( t \frac {\hat a_R} 2 \right)*/ const Vector3 v = direction*sinAngle; + /* pitch = \frac {\hat a_D} 2 = m_S \frac 1 {|l_V|} */ T pitch = -diff.dual().scalar()*invr; + /* moment = \hat {\boldsymbol n}_D */ const Vector3 moment = (diff.dual().vector() - (direction*(pitch*diff.real().scalar())))*invr; pitch *= t; + /* Vector of dual part of q_{ScLERP} */ const Vector3 v2 = moment*sinAngle + direction*(pitch*cosAngle); return from*DualQuaternion{Quaternion{v, cosAngle}, Quaternion{v2, -pitch*sinAngle}}; From a5066622cb58040265a54b7b38bf303aaa08e3e5 Mon Sep 17 00:00:00 2001 From: Squareys Date: Sat, 10 Oct 2015 16:22:24 +0200 Subject: [PATCH 5/7] Math: Cleanup ScLERP implementation. - Use explicit conversion to `T` - Use `std::` for `acos, cos, sin` to avoid use of double only functions - Do not mutate variables in math code to avoid confusion Signed-off-by: Squareys --- src/Magnum/Math/DualQuaternion.h | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/Magnum/Math/DualQuaternion.h b/src/Magnum/Math/DualQuaternion.h index 59fe3dbea..6a9061645 100644 --- a/src/Magnum/Math/DualQuaternion.h +++ b/src/Magnum/Math/DualQuaternion.h @@ -66,20 +66,20 @@ template inline DualQuaternion sclerp(const DualQuaternion& from, const T dotResult = dot(from.real().vector(), to.real().vector()); - /* Multiplying with -1.0f ensures shortest path when dot < 0. */ + /* Multiplying with -1 ensures shortest path when dot < 0 */ /* diff = \hat d = p^*q */ - const DualQuaternion diff = from.quaternionConjugated()*((dotResult < .0) ? -to : to); + const DualQuaternion diff = from.quaternionConjugated()*((dotResult < T(0)) ? -to : to); /* angle = t \hat a_D */ - const T angle = acos(diff.real().scalar())*t; + const T angle = std::acos(diff.real().scalar())*t; /* precompute sin/cos for manifold use */ - const T sinAngle = sin(angle); - const T cosAngle = cos(angle); + const T sinAngle = std::sin(angle); + const T cosAngle = std::cos(angle); /* merely a shortcut */ const Vector3& m = diff.real().vector(); /* invr = \frac 1 {|l_V|} */ - const T invr = 1/m.length(); + const T invr = m.lengthInverted(); /* direction = \hat {\boldsymbol n} = \hat l_V \frac 1 {|l_V|} */ const Vector3 direction = m*invr; @@ -88,14 +88,14 @@ template inline DualQuaternion sclerp(const DualQuaternion& from, const Vector3 v = direction*sinAngle; /* pitch = \frac {\hat a_D} 2 = m_S \frac 1 {|l_V|} */ - T pitch = -diff.dual().scalar()*invr; + const T pitch = -diff.dual().scalar()*invr; /* moment = \hat {\boldsymbol n}_D */ const Vector3 moment = (diff.dual().vector() - (direction*(pitch*diff.real().scalar())))*invr; - pitch *= t; + const T pitchT = pitch*t; /* Vector of dual part of q_{ScLERP} */ - const Vector3 v2 = moment*sinAngle + direction*(pitch*cosAngle); + const Vector3 v2 = moment*sinAngle + direction*(pitchT*cosAngle); - return from*DualQuaternion{Quaternion{v, cosAngle}, Quaternion{v2, -pitch*sinAngle}}; + return from*DualQuaternion{Quaternion{v, cosAngle}, Quaternion{v2, -pitchT*sinAngle}}; } /** From 8d329e1ea0cf8e4c9fd984cbc9e2890dbacffdec Mon Sep 17 00:00:00 2001 From: Squareys Date: Sat, 10 Oct 2015 16:25:47 +0200 Subject: [PATCH 6/7] Math: Super minor formatting fix. Signed-off-by: Squareys --- src/Magnum/Math/Quaternion.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Magnum/Math/Quaternion.h b/src/Magnum/Math/Quaternion.h index b3617931b..c05e0993e 100644 --- a/src/Magnum/Math/Quaternion.h +++ b/src/Magnum/Math/Quaternion.h @@ -89,7 +89,7 @@ Expects that both quaternions are normalized. @f[ @f] @see @ref Quaternion::isNormalized(), @ref slerp(const Quaternion&, const Quaternion&, T), @ref lerp(const T&, const T&, U) - */ +*/ template inline Quaternion lerp(const Quaternion& normalizedA, const Quaternion& normalizedB, T t) { CORRADE_ASSERT(normalizedA.isNormalized() && normalizedB.isNormalized(), "Math::lerp(): quaternions must be normalized", {}); From 3b5ba3f21fb249f336ac28b97568aea4fee7d0bb Mon Sep 17 00:00:00 2001 From: Squareys Date: Sat, 10 Oct 2015 18:01:35 +0200 Subject: [PATCH 7/7] Math: Use a more readable notation for inline comments. This should be alot easier to read than the TeX code before. Signed-off-by: Squareys --- src/Magnum/Math/DualQuaternion.h | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/src/Magnum/Math/DualQuaternion.h b/src/Magnum/Math/DualQuaternion.h index 6a9061645..3e12626c6 100644 --- a/src/Magnum/Math/DualQuaternion.h +++ b/src/Magnum/Math/DualQuaternion.h @@ -67,29 +67,28 @@ template inline DualQuaternion sclerp(const DualQuaternion& from, const T dotResult = dot(from.real().vector(), to.real().vector()); /* Multiplying with -1 ensures shortest path when dot < 0 */ - /* diff = \hat d = p^*q */ + /* diff = d = p^*q */ const DualQuaternion diff = from.quaternionConjugated()*((dotResult < T(0)) ? -to : to); - /* angle = t \hat a_D */ + /* angle = t*a_D */ const T angle = std::acos(diff.real().scalar())*t; - /* precompute sin/cos for manifold use */ + /* Precompute sin/cos for manifold use */ const T sinAngle = std::sin(angle); const T cosAngle = std::cos(angle); - /* merely a shortcut */ + /* Merely a shortcut */ const Vector3& m = diff.real().vector(); /* invr = \frac 1 {|l_V|} */ const T invr = m.lengthInverted(); - /* direction = \hat {\boldsymbol n} = \hat l_V \frac 1 {|l_V|} */ + /* direction = n = l_V * 1/|l_V| */ const Vector3 direction = m*invr; - /* Vector of real part of q_{ScLERP} - = \hat {\boldsymbol n}_R \cdot sin \left( t \frac {\hat a_R} 2 \right)*/ + /* Vector of real part of q_{ScLERP} = n_R*sin(t*a_R/2)*/ const Vector3 v = direction*sinAngle; - /* pitch = \frac {\hat a_D} 2 = m_S \frac 1 {|l_V|} */ + /* pitch = a_D/2 = m_S*1/|l_V| */ const T pitch = -diff.dual().scalar()*invr; - /* moment = \hat {\boldsymbol n}_D */ + /* moment = n_D */ const Vector3 moment = (diff.dual().vector() - (direction*(pitch*diff.real().scalar())))*invr; const T pitchT = pitch*t; /* Vector of dual part of q_{ScLERP} */