diff --git a/src/Math/DualQuaternion.h b/src/Math/DualQuaternion.h index 2b132864c..e65df7dfb 100644 --- a/src/Math/DualQuaternion.h +++ b/src/Math/DualQuaternion.h @@ -87,9 +87,10 @@ template class DualQuaternion: public Dual> { /** * @brief Construct dual quaternion from vector * - * @f[ + * To be used in transformations later. @f[ * \hat q = [\boldsymbol 0, 1] + \epsilon [\boldsymbol v, 0] * @f] + * @see transformPointNormalized() * @todoc Remove workaround when Doxygen is predictable */ #ifdef DOXYGEN_GENERATING_OUTPUT @@ -211,6 +212,21 @@ template class DualQuaternion: public Dual> { return quaternionConjugated(); } + /** + * @brief Rotate and translate point with normalized dual quaternion + * + * Expects that the dual quaternion is normalized. @f[ + * v' = qv \overline{\hat q^*} = q ([\boldsymbol 0, 1] + \epsilon [\boldsymbol v, 0]) \overline{\hat q^*} + * @f] + * @see Quaternion::rotateVectorNormalized() + */ + inline Vector3 transformPointNormalized(const Vector3& vector) const { + CORRADE_ASSERT(MathTypeTraits>::equals(norm(), Dual(1)), + "Math::DualQuaternion::transformPointNormalized(): dual quaternion must be normalized", + Vector3(std::numeric_limits::quiet_NaN())); + return ((*this)*DualQuaternion(vector)*conjugated()).dual().vector(); + } + MAGNUM_DUAL_SUBCLASS_IMPLEMENTATION(DualQuaternion, Quaternion) private: diff --git a/src/Math/Quaternion.h b/src/Math/Quaternion.h index e26ce38df..3d57aa838 100644 --- a/src/Math/Quaternion.h +++ b/src/Math/Quaternion.h @@ -403,6 +403,7 @@ template class Quaternion { * quaternions. @f[ * v' = qvq^{-1} = q [\boldsymbol v, 0] q^{-1} * @f] + * @see DualQuaternion::transformPointNormalized() */ inline Vector3 rotateVector(const Vector3& vector) const { return ((*this)*Quaternion(vector)*inverted()).vector(); @@ -415,7 +416,7 @@ template class Quaternion { * normalized. @f[ * v' = qvq^{-1} = qvq^* = q [\boldsymbol v, 0] q^* * @f] - * @see DualQuaternion::transformVectorNormalized() + * @see DualQuaternion::transformPointNormalized() */ inline Vector3 rotateVectorNormalized(const Vector3& vector) const { CORRADE_ASSERT(MathTypeTraits::equals(dot(), T(1)), diff --git a/src/Math/Test/DualQuaternionTest.cpp b/src/Math/Test/DualQuaternionTest.cpp index 8a653804d..382586469 100644 --- a/src/Math/Test/DualQuaternionTest.cpp +++ b/src/Math/Test/DualQuaternionTest.cpp @@ -18,6 +18,7 @@ #include "Math/Constants.h" #include "Math/DualQuaternion.h" +#include "Math/Matrix4.h" namespace Magnum { namespace Math { namespace Test { @@ -38,14 +39,17 @@ class DualQuaternionTest: public Corrade::TestSuite::Tester { void rotation(); void translation(); + void transformPointNormalized(); void debug(); }; typedef Math::Dual Dual; +typedef Math::Matrix4 Matrix4; typedef Math::DualQuaternion DualQuaternion; typedef Math::Quaternion Quaternion; typedef Math::Vector3 Vector3; +typedef Math::Vector4 Vector4; DualQuaternionTest::DualQuaternionTest() { addTests(&DualQuaternionTest::construct, @@ -61,6 +65,7 @@ DualQuaternionTest::DualQuaternionTest() { &DualQuaternionTest::rotation, &DualQuaternionTest::translation, + &DualQuaternionTest::transformPointNormalized, &DualQuaternionTest::debug); } @@ -138,6 +143,28 @@ void DualQuaternionTest::translation() { CORRADE_COMPARE(q.translation(), vec); } +void DualQuaternionTest::transformPointNormalized() { + DualQuaternion a = DualQuaternion::translation({-1.0f, 2.0f, 3.0f})*DualQuaternion::rotation(deg(23.0f), Vector3::xAxis()); + DualQuaternion b = DualQuaternion::rotation(deg(23.0f), Vector3::xAxis())*DualQuaternion::translation({-1.0f, 2.0f, 3.0f}); + Matrix4 m = Matrix4::translation({-1.0f, 2.0f, 3.0f})*Matrix4::rotationX(deg(23.0f)); + Matrix4 n = Matrix4::rotationX(deg(23.0f))*Matrix4::translation({-1.0f, 2.0f, 3.0f}); + Vector3 v(0.0f, -3.6f, 0.7f); + + std::ostringstream o; + Corrade::Utility::Error::setOutput(&o); + Vector3 notTransformed = (a*Dual(2)).transformPointNormalized(v); + CORRADE_VERIFY(notTransformed != notTransformed); + CORRADE_COMPARE(o.str(), "Math::DualQuaternion::transformPointNormalized(): dual quaternion must be normalized\n"); + + Vector3 transformedA = a.transformPointNormalized(v); + CORRADE_COMPARE(transformedA, (m*Vector4(v, 1.0f)).xyz()); + CORRADE_COMPARE(transformedA, Vector3(-1.0f, -1.58733f, 2.237721f)); + + Vector3 transformedB = b.transformPointNormalized(v); + CORRADE_COMPARE(transformedB, (n*Vector4(v, 1.0f)).xyz()); + CORRADE_COMPARE(transformedB, Vector3(-1.0f, -2.918512f, 2.780698f)); +} + void DualQuaternionTest::debug() { std::ostringstream o;