diff --git a/src/Math/Matrix.h b/src/Math/Matrix.h index 66235ef99..8de204b05 100644 --- a/src/Math/Matrix.h +++ b/src/Math/Matrix.h @@ -137,6 +137,8 @@ template class Matrix: public RectangularMatrix inverted() const { Matrix out(Zero); diff --git a/src/Math/Matrix3.h b/src/Math/Matrix3.h index 1a925a12b..a71854a7c 100644 --- a/src/Math/Matrix3.h +++ b/src/Math/Matrix3.h @@ -186,6 +186,24 @@ template class Matrix3: public Matrix<3, T> { inline Vector2& translation() { return (*this)[2].xy(); } inline constexpr Vector2 translation() const { return (*this)[2].xy(); } /**< @overload */ + /** + * @brief Inverted Euclidean transformation matrix + * + * Assumes that the matrix represents Euclidean transformation (i.e. + * only rotation and translation, no scaling) and creates inverted + * matrix from transposed rotation part and negated translation part. + * Significantly faster than the general algorithm in inverted(). + * @see rotationScaling() const, translation() const + */ + inline Matrix3 invertedEuclidean() const { + CORRADE_ASSERT((*this)(0, 2) == T(0) && (*this)(1, 2) == T(0) && (*this)(2, 2) == T(1), + "Math::Matrix3::invertedEuclidean(): unexpected values on last row", {}); + Matrix<2, T> inverseRotation = rotationScaling().transposed(); + CORRADE_ASSERT((inverseRotation*rotationScaling() == Matrix<2, T>()), + "Math::Matrix3::invertedEuclidean(): the matrix doesn't represent Euclidean transformation", {}); + return from(inverseRotation, inverseRotation*-translation()); + } + #ifndef DOXYGEN_GENERATING_OUTPUT inline Point2D operator*(const Point2D& other) const { return Matrix<3, T>::operator*(other); diff --git a/src/Math/Matrix4.h b/src/Math/Matrix4.h index f41458b03..d7f28f1a4 100644 --- a/src/Math/Matrix4.h +++ b/src/Math/Matrix4.h @@ -290,6 +290,24 @@ template class Matrix4: public Matrix<4, T> { inline Vector3& translation() { return (*this)[3].xyz(); } inline constexpr Vector3 translation() const { return (*this)[3].xyz(); } /**< @overload */ + /** + * @brief Inverted Euclidean transformation matrix + * + * Assumes that the matrix represents Euclidean transformation (i.e. + * only rotation and translation, no scaling) and creates inverted + * matrix from transposed rotation part and negated translation part. + * Significantly faster than the general algorithm in inverted(). + * @see rotationScaling() const, translation() const + */ + inline Matrix4 invertedEuclidean() const { + CORRADE_ASSERT((*this)(0, 3) == T(0) && (*this)(1, 3) == T(0) && (*this)(2, 3) == T(0) && (*this)(3, 3) == T(1), + "Math::Matrix4::invertedEuclidean(): unexpected values on last row", {}); + Matrix<3, T> inverseRotation = rotationScaling().transposed(); + CORRADE_ASSERT((inverseRotation*rotationScaling() == Matrix<3, T>()), + "Math::Matrix4::invertedEuclidean(): the matrix doesn't represent Euclidean transformation", {}); + return from(inverseRotation, inverseRotation*-translation()); + } + #ifndef DOXYGEN_GENERATING_OUTPUT inline Point3D operator*(const Point3D& other) const { return Matrix<4, T>::operator*(other); diff --git a/src/Math/Test/Matrix3Test.cpp b/src/Math/Test/Matrix3Test.cpp index 38dac55d3..b9219b57a 100644 --- a/src/Math/Test/Matrix3Test.cpp +++ b/src/Math/Test/Matrix3Test.cpp @@ -41,6 +41,7 @@ Matrix3Test::Matrix3Test() { &Matrix3Test::rotationScalingPart, &Matrix3Test::rotationPart, &Matrix3Test::vectorParts, + &Matrix3Test::invertedEuclidean, &Matrix3Test::debug, &Matrix3Test::configuration); } @@ -173,6 +174,33 @@ void Matrix3Test::vectorParts() { CORRADE_COMPARE(m.translation(), Vector2(-5.0f, 12.0f)); } +void Matrix3Test::invertedEuclidean() { + std::ostringstream o; + Error::setOutput(&o); + + Matrix3 m( + 3.0f, 5.0f, 8.0f, + 4.0f, 4.0f, 7.0f, + 7.0f, -1.0f, 8.0f + ); + CORRADE_COMPARE(m.invertedEuclidean(), Matrix3()); + CORRADE_COMPARE(o.str(), "Math::Matrix3::invertedEuclidean(): unexpected values on last row\n"); + + o.str(""); + CORRADE_COMPARE(Matrix3::scaling(Vector2(2.0f)).invertedEuclidean(), Matrix3()); + CORRADE_COMPARE(o.str(), "Math::Matrix3::invertedEuclidean(): the matrix doesn't represent Euclidean transformation\n"); + + Matrix3 actual = Matrix3::rotation(deg(-74.0f))* + Matrix3::reflection(Vector2(0.5f, -2.0f).normalized())* + Matrix3::translation({2.0f, -3.0f}); + Matrix3 expected = Matrix3::translation({-2.0f, 3.0f})* + Matrix3::reflection(Vector2(0.5f, -2.0f).normalized())* + Matrix3::rotation(deg(74.0f)); + + CORRADE_COMPARE(actual.invertedEuclidean(), expected); + CORRADE_COMPARE(actual.invertedEuclidean(), actual.inverted()); +} + void Matrix3Test::debug() { Matrix3 m( 3.0f, 5.0f, 8.0f, diff --git a/src/Math/Test/Matrix3Test.h b/src/Math/Test/Matrix3Test.h index fa658f74a..bea82a6f8 100644 --- a/src/Math/Test/Matrix3Test.h +++ b/src/Math/Test/Matrix3Test.h @@ -33,6 +33,7 @@ class Matrix3Test: public Corrade::TestSuite::Tester { void rotationScalingPart(); void rotationPart(); void vectorParts(); + void invertedEuclidean(); void debug(); void configuration(); diff --git a/src/Math/Test/Matrix4Test.cpp b/src/Math/Test/Matrix4Test.cpp index eac9539c8..0f3ee5be6 100644 --- a/src/Math/Test/Matrix4Test.cpp +++ b/src/Math/Test/Matrix4Test.cpp @@ -44,6 +44,7 @@ Matrix4Test::Matrix4Test() { &Matrix4Test::rotationScalingPart, &Matrix4Test::rotationPart, &Matrix4Test::vectorParts, + &Matrix4Test::invertedEuclidean, &Matrix4Test::debug, &Matrix4Test::configuration); } @@ -225,6 +226,34 @@ void Matrix4Test::vectorParts() { CORRADE_COMPARE(m.translation(), Vector3(-5.0f, 12.0f, 0.5f)); } +void Matrix4Test::invertedEuclidean() { + std::ostringstream o; + Error::setOutput(&o); + + Matrix4 m( + 3.0f, 5.0f, 8.0f, 4.0f, + 4.0f, 4.0f, 7.0f, 3.0f, + 7.0f, -1.0f, 8.0f, 0.0f, + 9.0f, 4.0f, 5.0f, 9.0f + ); + CORRADE_COMPARE(m.invertedEuclidean(), Matrix4()); + CORRADE_COMPARE(o.str(), "Math::Matrix4::invertedEuclidean(): unexpected values on last row\n"); + + o.str(""); + CORRADE_COMPARE(Matrix4::scaling(Vector3(2.0f)).invertedEuclidean(), Matrix4()); + CORRADE_COMPARE(o.str(), "Math::Matrix4::invertedEuclidean(): the matrix doesn't represent Euclidean transformation\n"); + + Matrix4 actual = Matrix4::rotation(deg(-74.0f), Vector3(-1.0f, 0.5f, 2.0f).normalized())* + Matrix4::reflection(Vector3(0.5f, -2.0f, 2.0f).normalized())* + Matrix4::translation({1.0f, 2.0f, -3.0f}); + Matrix4 expected = Matrix4::translation({-1.0f, -2.0f, 3.0f})* + Matrix4::reflection(Vector3(0.5f, -2.0f, 2.0f).normalized())* + Matrix4::rotation(deg(74.0f), Vector3(-1.0f, 0.5f, 2.0f).normalized()); + + CORRADE_COMPARE(actual.invertedEuclidean(), expected); + CORRADE_COMPARE(actual.invertedEuclidean(), actual.inverted()); +} + void Matrix4Test::debug() { Matrix4 m( 3.0f, 5.0f, 8.0f, 4.0f, diff --git a/src/Math/Test/Matrix4Test.h b/src/Math/Test/Matrix4Test.h index 3ea386b21..737778533 100644 --- a/src/Math/Test/Matrix4Test.h +++ b/src/Math/Test/Matrix4Test.h @@ -36,6 +36,7 @@ class Matrix4Test: public Corrade::TestSuite::Tester { void rotationScalingPart(); void rotationPart(); void vectorParts(); + void invertedEuclidean(); void debug(); void configuration();