From da93b2ad6aafe3237c402b29fa889e61129eeab6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Vladim=C3=ADr=20Vondru=C5=A1?= Date: Fri, 15 Mar 2013 00:30:13 +0100 Subject: [PATCH] Math: ability to create Quaternion and DualQuaternion from matrix. --- src/Math/DualQuaternion.h | 17 ++++++++- src/Math/Quaternion.h | 54 +++++++++++++++++++++++++++- src/Math/Test/DualQuaternionTest.cpp | 8 +++++ src/Math/Test/QuaternionTest.cpp | 21 +++++++++-- 4 files changed, 96 insertions(+), 4 deletions(-) diff --git a/src/Math/DualQuaternion.h b/src/Math/DualQuaternion.h index 6a5653224..9d3e79902 100644 --- a/src/Math/DualQuaternion.h +++ b/src/Math/DualQuaternion.h @@ -79,6 +79,21 @@ template class DualQuaternion: public Dual> { return {{}, {vector/T(2), T(0)}}; } + /** + * @brief Create dual quaternion from transformation matrix + * + * Expects that the matrix represents rigid transformation. + * @see toMatrix(), Quaternion::fromMatrix(), + * Matrix4::isRigidTransformation() + */ + inline static DualQuaternion fromMatrix(const Matrix4& matrix) { + CORRADE_ASSERT(matrix.isRigidTransformation(), + "Math::DualQuaternion::fromMatrix(): the matrix doesn't represent rigid transformation", {}); + + Quaternion q = Implementation::quaternionFromMatrix(matrix.rotationScaling()); + return {q, Quaternion(matrix.translation()/2)*q}; + } + /** * @brief Default constructor * @@ -158,7 +173,7 @@ template class DualQuaternion: public Dual> { /** * @brief Convert dual quaternion to transformation matrix * - * @see Quaternion::toMatrix() + * @see fromMatrix(), Quaternion::toMatrix() */ Matrix4 toMatrix() const { return Matrix4::from(this->real().toMatrix(), translation()); diff --git a/src/Math/Quaternion.h b/src/Math/Quaternion.h index 5103de897..aa5614810 100644 --- a/src/Math/Quaternion.h +++ b/src/Math/Quaternion.h @@ -38,6 +38,45 @@ namespace Magnum { namespace Math { +#ifndef DOXYGEN_GENERATING_OUTPUT +namespace Implementation { + +/* No assertions fired, for internal use */ +template inline Quaternion quaternionFromMatrix(const Matrix<3, T>& m) { + const Vector<3, T> diagonal = m.diagonal(); + const T trace = diagonal.sum(); + + /* Diagonal is positive */ + if(trace > T(0)) { + const T s = std::sqrt(trace + T(1)); + const T t = T(0.5)/s; + return {Vector3(m[1][2] - m[2][1], + m[2][0] - m[0][2], + m[0][1] - m[1][0])*t, s*T(0.5)}; + } + + /* Diagonal is negative */ + std::size_t i = 0; + if(diagonal[1] > diagonal[0]) i = 1; + if(diagonal[2] > diagonal[i]) i = 2; + + const std::size_t j = (i + 1) % 3; + const std::size_t k = (i + 2) % 3; + + const T s = std::sqrt(diagonal[i] - diagonal[j] - diagonal[k] + T(1)); + const T t = (s == T(0) ? T(0) : T(0.5)/s); + + Vector3 vec; + vec[i] = s*T(0.5); + vec[j] = (m[i][j] + m[j][i])*t; + vec[k] = (m[i][k] + m[k][i])*t; + + return {vec, (m[j][k] - m[k][j])*t}; +} + +} +#endif + /** @brief %Quaternion @tparam T Underlying data type @@ -134,6 +173,18 @@ template class Quaternion { return {normalizedAxis*std::sin(T(angle)/2), std::cos(T(angle)/2)}; } + /** + * @brief Create quaternion from rotation matrix + * + * Expects that the matrix is orthogonal (i.e. pure rotation). + * @see toMatrix(), DualComplex::fromMatrix(), Matrix::isOrthogonal() + */ + inline static Quaternion fromMatrix(const Matrix<3, T>& matrix) { + CORRADE_ASSERT(matrix.isOrthogonal(), + "Math::Quaternion::fromMatrix(): the matrix is not orthogonal", {}); + return Implementation::quaternionFromMatrix(matrix); + } + /** * @brief Default constructor * @@ -213,7 +264,8 @@ template class Quaternion { /** * @brief Convert quaternion to rotation matrix * - * @see DualQuaternion::toMatrix(), Matrix4::from(const Matrix<3, T>&, const Vector3&) + * @see fromMatrix(), DualQuaternion::toMatrix(), + * Matrix4::from(const Matrix<3, T>&, const Vector3&) */ Matrix<3, T> toMatrix() const { return { diff --git a/src/Math/Test/DualQuaternionTest.cpp b/src/Math/Test/DualQuaternionTest.cpp index 75300f1d6..cb5011d94 100644 --- a/src/Math/Test/DualQuaternionTest.cpp +++ b/src/Math/Test/DualQuaternionTest.cpp @@ -234,6 +234,14 @@ void DualQuaternionTest::matrix() { /* Verify that negated dual quaternion gives the same transformation */ CORRADE_COMPARE(q.toMatrix(), m); CORRADE_COMPARE((-q).toMatrix(), m); + + std::ostringstream o; + Error::setOutput(&o); + DualQuaternion::fromMatrix(m*2); + CORRADE_COMPARE(o.str(), "Math::DualQuaternion::fromMatrix(): the matrix doesn't represent rigid transformation\n"); + + DualQuaternion p = DualQuaternion::fromMatrix(m); + CORRADE_COMPARE(p, q); } void DualQuaternionTest::transformPoint() { diff --git a/src/Math/Test/QuaternionTest.cpp b/src/Math/Test/QuaternionTest.cpp index 6577ea5ab..9f9bcc865 100644 --- a/src/Math/Test/QuaternionTest.cpp +++ b/src/Math/Test/QuaternionTest.cpp @@ -282,12 +282,29 @@ void QuaternionTest::angle() { } void QuaternionTest::matrix() { - Quaternion q = Quaternion::rotation(Deg(37.0f), Vector3(1.0f/Constants::sqrt3())); - Matrix3 m = Matrix4::rotation(Deg(37.0f), Vector3(1.0f/Constants::sqrt3())).rotationScaling(); + Vector3 axis = Vector3(1.0f, -3.0f, 5.0f).normalized(); + + Quaternion q = Quaternion::rotation(Deg(37.0f), axis); + Matrix3 m = Matrix4::rotation(Deg(37.0f), axis).rotationScaling(); /* Verify that negated quaternion gives the same rotation */ CORRADE_COMPARE(q.toMatrix(), m); CORRADE_COMPARE((-q).toMatrix(), m); + + std::ostringstream o; + Error::setOutput(&o); + Quaternion::fromMatrix(m*2); + CORRADE_COMPARE(o.str(), "Math::Quaternion::fromMatrix(): the matrix is not orthogonal\n"); + + /* Trace > 0 */ + CORRADE_VERIFY(m.trace() > 0.0f); + CORRADE_COMPARE(Quaternion::fromMatrix(m), q); + + /* Trace < 0 */ + Matrix3 m2 = Matrix4::rotation(Deg(130.0f), axis).rotationScaling(); + Quaternion q2 = Quaternion::rotation(Deg(130.0f), axis); + CORRADE_VERIFY(m2.trace() < 0.0f); + CORRADE_COMPARE(Quaternion::fromMatrix(m2), q2); } void QuaternionTest::lerp() {