matrix: add analytical 4x4 inverse
This commit is contained in:
parent
c086e2c9d7
commit
3ab2e8ed57
208
matrix.cpp
208
matrix.cpp
@ -52,7 +52,127 @@ matrix<T>::translate (T x, T y, T z) {
|
||||
template <typename T>
|
||||
matrix<T>
|
||||
matrix<T>::inverse (void) const {
|
||||
return matrix<T>(*this).invert ();
|
||||
matrix<T> 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<T>::inverse (void) const {
|
||||
template <typename T>
|
||||
matrix<T>&
|
||||
matrix<T>::invert (void) {
|
||||
auto m = *this;
|
||||
m.invert ();
|
||||
*this = m;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
template <typename T>
|
||||
matrix<T>
|
||||
matrix<T>::inverse_affine (void) const {
|
||||
return matrix<T>(*this).invert_affine ();
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
template <typename T>
|
||||
matrix<T>&
|
||||
matrix<T>::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<T>::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<T>::invert (void) {
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
template <typename T>
|
||||
T
|
||||
matrix<T>::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 <typename T>
|
||||
matrix<T>
|
||||
@ -150,10 +324,10 @@ matrix<T>::to_global (const util::point<3> &p) const {
|
||||
template <typename T>
|
||||
bool
|
||||
matrix<T>::is_affine (void) const {
|
||||
return exactly_equal (values[3][0], static_cast<T> (0)) &&
|
||||
exactly_equal (values[3][1], static_cast<T> (0)) &&
|
||||
exactly_equal (values[3][2], static_cast<T> (0)) &&
|
||||
exactly_equal (values[3][3], static_cast<T> (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});
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -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<T> inverse (void) const;
|
||||
matrix<T>& invert (void);
|
||||
matrix<T> inverse_affine (void) const;
|
||||
matrix<T>& invert_affine (void);
|
||||
|
||||
T det (void) const;
|
||||
|
||||
matrix<T> operator* (const matrix<T>&) const;
|
||||
|
||||
|
||||
@ -11,6 +11,7 @@ TEST_BIN = \
|
||||
json/types \
|
||||
maths/maths \
|
||||
maths/matrix \
|
||||
matrix \
|
||||
md2 \
|
||||
md4 \
|
||||
md5 \
|
||||
|
||||
66
test/matrix.cpp
Normal file
66
test/matrix.cpp
Normal file
@ -0,0 +1,66 @@
|
||||
#include "matrix.hpp"
|
||||
#include "vector.hpp"
|
||||
#include "debug.hpp"
|
||||
|
||||
#include <cstdlib>
|
||||
|
||||
int
|
||||
main (int, char **) {
|
||||
{
|
||||
// Identity matrix-vector multiplication
|
||||
auto v = util::vector<4> { 1, 2, 3, 4 };
|
||||
auto r = util::matrix<float>::IDENTITY * v;
|
||||
CHECK_EQ (r, v);
|
||||
}
|
||||
|
||||
{
|
||||
// Simple matrix-vector multiplication
|
||||
util::matrix<float> 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<float>::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<float> m { {
|
||||
{ 4, 1, 2, 3 },
|
||||
{ 2, 3, 4, 1 },
|
||||
{ 3, 4, 1, 2 },
|
||||
{ 1, 2, 3, 4 }
|
||||
} };
|
||||
|
||||
util::matrix<float> 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;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user