From 3ab2e8ed570537e4aee040c81bc49da7257e20bb Mon Sep 17 00:00:00 2001 From: Danny Robson Date: Tue, 19 Aug 2014 20:45:28 +1000 Subject: [PATCH] matrix: add analytical 4x4 inverse --- matrix.cpp | 208 +++++++++++++++++++++++++++++++++++++++++++---- matrix.hpp | 7 ++ test/Makefile.am | 1 + test/matrix.cpp | 66 +++++++++++++++ 4 files changed, 265 insertions(+), 17 deletions(-) create mode 100644 test/matrix.cpp diff --git a/matrix.cpp b/matrix.cpp index e9652191..84939a5f 100644 --- a/matrix.cpp +++ b/matrix.cpp @@ -52,7 +52,127 @@ matrix::translate (T x, T y, T z) { template matrix matrix::inverse (void) const { - return matrix(*this).invert (); + matrix m; + + T d = det (); + if (almost_zero (d)) + throw std::runtime_error ("non-singular matrix"); + auto v = values; + + m.values[0][0] = v[1][2] * v[2][3] * v[3][1] - + v[1][3] * v[2][2] * v[3][1] + + v[1][3] * v[2][1] * v[3][2] - + v[1][1] * v[2][3] * v[3][2] - + v[1][2] * v[2][1] * v[3][3] + + v[1][1] * v[2][2] * v[3][3]; + + m.values[0][1] = v[0][3] * v[2][2] * v[3][1] - + v[0][2] * v[2][3] * v[3][1] - + v[0][3] * v[2][1] * v[3][2] + + v[0][1] * v[2][3] * v[3][2] + + v[0][2] * v[2][1] * v[3][3] - + v[0][1] * v[2][2] * v[3][3]; + + m.values[0][2] = v[0][2] * v[1][3] * v[3][1] - + v[0][3] * v[1][2] * v[3][1] + + v[0][3] * v[1][1] * v[3][2] - + v[0][1] * v[1][3] * v[3][2] - + v[0][2] * v[1][1] * v[3][3] + + v[0][1] * v[1][2] * v[3][3]; + + m.values[0][3] = v[0][3] * v[1][2] * v[2][1] - + v[0][2] * v[1][3] * v[2][1] - + v[0][3] * v[1][1] * v[2][2] + + v[0][1] * v[1][3] * v[2][2] + + v[0][2] * v[1][1] * v[2][3] - + v[0][1] * v[1][2] * v[2][3]; + + m.values[1][0] = v[1][3] * v[2][2] * v[3][0] - + v[1][2] * v[2][3] * v[3][0] - + v[1][3] * v[2][0] * v[3][2] + + v[1][0] * v[2][3] * v[3][2] + + v[1][2] * v[2][0] * v[3][3] - + v[1][0] * v[2][2] * v[3][3]; + + m.values[1][1] = v[0][2] * v[2][3] * v[3][0] - + v[0][3] * v[2][2] * v[3][0] + + v[0][3] * v[2][0] * v[3][2] - + v[0][0] * v[2][3] * v[3][2] - + v[0][2] * v[2][0] * v[3][3] + + v[0][0] * v[2][2] * v[3][3]; + + m.values[1][2] = v[0][3] * v[1][2] * v[3][0] - + v[0][2] * v[1][3] * v[3][0] - + v[0][3] * v[1][0] * v[3][2] + + v[0][0] * v[1][3] * v[3][2] + + v[0][2] * v[1][0] * v[3][3] - + v[0][0] * v[1][2] * v[3][3]; + + m.values[1][3] = v[0][2] * v[1][3] * v[2][0] - + v[0][3] * v[1][2] * v[2][0] + + v[0][3] * v[1][0] * v[2][2] - + v[0][0] * v[1][3] * v[2][2] - + v[0][2] * v[1][0] * v[2][3] + + v[0][0] * v[1][2] * v[2][3]; + + m.values[2][0] = v[1][1] * v[2][3] * v[3][0] - + v[1][3] * v[2][1] * v[3][0] + + v[1][3] * v[2][0] * v[3][1] - + v[1][0] * v[2][3] * v[3][1] - + v[1][1] * v[2][0] * v[3][3] + + v[1][0] * v[2][1] * v[3][3]; + + m.values[2][1] = v[0][3] * v[2][1] * v[3][0] - + v[0][1] * v[2][3] * v[3][0] - + v[0][3] * v[2][0] * v[3][1] + + v[0][0] * v[2][3] * v[3][1] + + v[0][1] * v[2][0] * v[3][3] - + v[0][0] * v[2][1] * v[3][3]; + + m.values[2][2] = v[0][1] * v[1][3] * v[3][0] - + v[0][3] * v[1][1] * v[3][0] + + v[0][3] * v[1][0] * v[3][1] - + v[0][0] * v[1][3] * v[3][1] - + v[0][1] * v[1][0] * v[3][3] + + v[0][0] * v[1][1] * v[3][3]; + + m.values[2][3] = v[0][3] * v[1][1] * v[2][0] - + v[0][1] * v[1][3] * v[2][0] - + v[0][3] * v[1][0] * v[2][1] + + v[0][0] * v[1][3] * v[2][1] + + v[0][1] * v[1][0] * v[2][3] - + v[0][0] * v[1][1] * v[2][3]; + + m.values[3][0] = v[1][2] * v[2][1] * v[3][0] - + v[1][1] * v[2][2] * v[3][0] - + v[1][2] * v[2][0] * v[3][1] + + v[1][0] * v[2][2] * v[3][1] + + v[1][1] * v[2][0] * v[3][2] - + v[1][0] * v[2][1] * v[3][2]; + + m.values[3][1] = v[0][1] * v[2][2] * v[3][0] - + v[0][2] * v[2][1] * v[3][0] + + v[0][2] * v[2][0] * v[3][1] - + v[0][0] * v[2][2] * v[3][1] - + v[0][1] * v[2][0] * v[3][2] + + v[0][0] * v[2][1] * v[3][2]; + + m.values[3][2] = v[0][2] * v[1][1] * v[3][0] - + v[0][1] * v[1][2] * v[3][0] - + v[0][2] * v[1][0] * v[3][1] + + v[0][0] * v[1][2] * v[3][1] + + v[0][1] * v[1][0] * v[3][2] - + v[0][0] * v[1][1] * v[3][2]; + + m.values[3][3] = v[0][1] * v[1][2] * v[2][0] - + v[0][2] * v[1][1] * v[2][0] + + v[0][2] * v[1][0] * v[2][1] - + v[0][0] * v[1][2] * v[2][1] - + v[0][1] * v[1][0] * v[2][2] + + v[0][0] * v[1][1] * v[2][2]; + + m /= d; + return m; } @@ -60,10 +180,30 @@ matrix::inverse (void) const { template matrix& matrix::invert (void) { + auto m = *this; + m.invert (); + *this = m; + + return *this; +} + + +//----------------------------------------------------------------------------- +template +matrix +matrix::inverse_affine (void) const { + return matrix(*this).invert_affine (); +} + + +//----------------------------------------------------------------------------- +template +matrix& +matrix::invert_affine (void) { CHECK_HARD (is_affine ()); // inv ([ M b ] == [ inv(M) -inv(M).b ] - // [ 0 1 ]) [ 0 1 + // [ 0 1 ]) [ 0 1 ] // Invert the 3x3 M T A = (values[1][1] * values[2][2] - values[1][2] * values[2][1]); @@ -78,18 +218,18 @@ matrix::invert (void) { T H = (values[0][2] * values[1][0] - values[0][0] * values[1][2]); T K = (values[0][0] * values[1][1] - values[0][1] * values[1][0]); - T det = values[0][0] * A + values[0][1] * B + values[0][2] * C; - CHECK_NEQ (det, 0.0); + T d = values[0][0] * A + values[0][1] * B + values[0][2] * C; + CHECK_NEQ (d, 0.0); - values[0][0] = A / det; - values[0][1] = D / det; - values[0][2] = G / det; - values[1][0] = B / det; - values[1][1] = E / det; - values[1][2] = H / det; - values[2][0] = C / det; - values[2][1] = F / det; - values[2][2] = K / det; + values[0][0] = A / d; + values[0][1] = D / d; + values[0][2] = G / d; + values[1][0] = B / d; + values[1][1] = E / d; + values[1][2] = H / d; + values[2][0] = C / d; + values[2][1] = F / d; + values[2][2] = K / d; // Multiply the b T b0 = - values[0][0] * values[0][3] - values[0][1] * values[1][3] - values[0][2] * values[2][3]; @@ -104,6 +244,40 @@ matrix::invert (void) { } +//----------------------------------------------------------------------------- +template +T +matrix::det (void) const { + return values[0][3] * values[1][2] * values[2][1] * values[3][0] - + values[0][2] * values[1][3] * values[2][1] * values[3][0] - + values[0][3] * values[1][1] * values[2][2] * values[3][0] + + values[0][1] * values[1][3] * values[2][2] * values[3][0] + + values[0][2] * values[1][1] * values[2][3] * values[3][0] - + values[0][1] * values[1][2] * values[2][3] * values[3][0] - + + values[0][3] * values[1][2] * values[2][0] * values[3][1] + + values[0][2] * values[1][3] * values[2][0] * values[3][1] + + values[0][3] * values[1][0] * values[2][2] * values[3][1] - + values[0][0] * values[1][3] * values[2][2] * values[3][1] - + values[0][2] * values[1][0] * values[2][3] * values[3][1] + + values[0][0] * values[1][2] * values[2][3] * values[3][1] + + + values[0][3] * values[1][1] * values[2][0] * values[3][2] - + values[0][1] * values[1][3] * values[2][0] * values[3][2] - + values[0][3] * values[1][0] * values[2][1] * values[3][2] + + values[0][0] * values[1][3] * values[2][1] * values[3][2] + + values[0][1] * values[1][0] * values[2][3] * values[3][2] - + values[0][0] * values[1][1] * values[2][3] * values[3][2] - + + values[0][2] * values[1][1] * values[2][0] * values[3][3] + + values[0][1] * values[1][2] * values[2][0] * values[3][3] + + values[0][2] * values[1][0] * values[2][1] * values[3][3] - + values[0][0] * values[1][2] * values[2][1] * values[3][3] - + values[0][1] * values[1][0] * values[2][2] * values[3][3] + + values[0][0] * values[1][1] * values[2][2] * values[3][3]; +} + + //----------------------------------------------------------------------------- template matrix @@ -150,10 +324,10 @@ matrix::to_global (const util::point<3> &p) const { template bool matrix::is_affine (void) const { - return exactly_equal (values[3][0], static_cast (0)) && - exactly_equal (values[3][1], static_cast (0)) && - exactly_equal (values[3][2], static_cast (0)) && - exactly_equal (values[3][3], static_cast (1)); + return exactly_equal (values[3][0], T {0}) && + exactly_equal (values[3][1], T {0}) && + exactly_equal (values[3][2], T {0}) && + exactly_equal (values[3][3], T {1}); } diff --git a/matrix.hpp b/matrix.hpp index 7b957be5..ead9e3b4 100644 --- a/matrix.hpp +++ b/matrix.hpp @@ -29,11 +29,18 @@ namespace util { struct matrix { T values[4][4]; + static const size_t rows = 4; + static const size_t cols = 4; + void scale (T x, T y, T z); void translate (T x, T y, T z); matrix inverse (void) const; matrix& invert (void); + matrix inverse_affine (void) const; + matrix& invert_affine (void); + + T det (void) const; matrix operator* (const matrix&) const; diff --git a/test/Makefile.am b/test/Makefile.am index 448fad0e..6abc3b37 100644 --- a/test/Makefile.am +++ b/test/Makefile.am @@ -11,6 +11,7 @@ TEST_BIN = \ json/types \ maths/maths \ maths/matrix \ + matrix \ md2 \ md4 \ md5 \ diff --git a/test/matrix.cpp b/test/matrix.cpp new file mode 100644 index 00000000..420e2969 --- /dev/null +++ b/test/matrix.cpp @@ -0,0 +1,66 @@ +#include "matrix.hpp" +#include "vector.hpp" +#include "debug.hpp" + +#include + +int +main (int, char **) { + { + // Identity matrix-vector multiplication + auto v = util::vector<4> { 1, 2, 3, 4 }; + auto r = util::matrix::IDENTITY * v; + CHECK_EQ (r, v); + } + + { + // Simple matrix-vector multiplication + util::matrix m { { + { 1, 2, 3, 4 }, + { 5, 6, 7, 8 }, + { 9, 10, 11, 12 }, + { 13, 14, 15, 16 } + } }; + + util::vector<4> v { 1, 2, 3, 4 }; + + auto r = m * v; + + CHECK_EQ (r.x, 30); + CHECK_EQ (r.y, 70); + CHECK_EQ (r.z, 110); + CHECK_EQ (r.w, 150); + } + + { + // Ensure identity inverts to identity + auto m = util::matrix::IDENTITY.inverse (); + for (size_t r = 0; r < m.rows; ++r) + for (size_t c = 0; c < m.cols; ++c) + if (r == c) + CHECK_EQ (m.values[r][c], 1); + else + CHECK_EQ (m.values[r][c], 0); + } + + { + // Simple inversion test + util::matrix m { { + { 4, 1, 2, 3 }, + { 2, 3, 4, 1 }, + { 3, 4, 1, 2 }, + { 1, 2, 3, 4 } + } }; + + util::matrix r { { + { 11, 1, 1, -9 }, + { -9, 1, 11, 1 }, + { 1, 11, -9, 1 }, + { 1, -9, 1, 11 } + } }; + + CHECK_EQ (m.inverse (), r / 40.f); + } + + return EXIT_SUCCESS; +}