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>
|
template <typename T>
|
||||||
matrix<T>
|
matrix<T>
|
||||||
matrix<T>::inverse (void) const {
|
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>
|
template <typename T>
|
||||||
matrix<T>&
|
matrix<T>&
|
||||||
matrix<T>::invert (void) {
|
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 ());
|
CHECK_HARD (is_affine ());
|
||||||
|
|
||||||
// inv ([ M b ] == [ inv(M) -inv(M).b ]
|
// inv ([ M b ] == [ inv(M) -inv(M).b ]
|
||||||
// [ 0 1 ]) [ 0 1
|
// [ 0 1 ]) [ 0 1 ]
|
||||||
|
|
||||||
// Invert the 3x3 M
|
// Invert the 3x3 M
|
||||||
T A = (values[1][1] * values[2][2] - values[1][2] * values[2][1]);
|
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 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 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;
|
T d = values[0][0] * A + values[0][1] * B + values[0][2] * C;
|
||||||
CHECK_NEQ (det, 0.0);
|
CHECK_NEQ (d, 0.0);
|
||||||
|
|
||||||
values[0][0] = A / det;
|
values[0][0] = A / d;
|
||||||
values[0][1] = D / det;
|
values[0][1] = D / d;
|
||||||
values[0][2] = G / det;
|
values[0][2] = G / d;
|
||||||
values[1][0] = B / det;
|
values[1][0] = B / d;
|
||||||
values[1][1] = E / det;
|
values[1][1] = E / d;
|
||||||
values[1][2] = H / det;
|
values[1][2] = H / d;
|
||||||
values[2][0] = C / det;
|
values[2][0] = C / d;
|
||||||
values[2][1] = F / det;
|
values[2][1] = F / d;
|
||||||
values[2][2] = K / det;
|
values[2][2] = K / d;
|
||||||
|
|
||||||
// Multiply the b
|
// Multiply the b
|
||||||
T b0 = - values[0][0] * values[0][3] - values[0][1] * values[1][3] - values[0][2] * values[2][3];
|
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>
|
template <typename T>
|
||||||
matrix<T>
|
matrix<T>
|
||||||
@ -150,10 +324,10 @@ matrix<T>::to_global (const util::point<3> &p) const {
|
|||||||
template <typename T>
|
template <typename T>
|
||||||
bool
|
bool
|
||||||
matrix<T>::is_affine (void) const {
|
matrix<T>::is_affine (void) const {
|
||||||
return exactly_equal (values[3][0], static_cast<T> (0)) &&
|
return exactly_equal (values[3][0], T {0}) &&
|
||||||
exactly_equal (values[3][1], static_cast<T> (0)) &&
|
exactly_equal (values[3][1], T {0}) &&
|
||||||
exactly_equal (values[3][2], static_cast<T> (0)) &&
|
exactly_equal (values[3][2], T {0}) &&
|
||||||
exactly_equal (values[3][3], static_cast<T> (1));
|
exactly_equal (values[3][3], T {1});
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -29,11 +29,18 @@ namespace util {
|
|||||||
struct matrix {
|
struct matrix {
|
||||||
T values[4][4];
|
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 scale (T x, T y, T z);
|
||||||
void translate (T x, T y, T z);
|
void translate (T x, T y, T z);
|
||||||
|
|
||||||
matrix<T> inverse (void) const;
|
matrix<T> inverse (void) const;
|
||||||
matrix<T>& invert (void);
|
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;
|
matrix<T> operator* (const matrix<T>&) const;
|
||||||
|
|
||||||
|
|||||||
@ -11,6 +11,7 @@ TEST_BIN = \
|
|||||||
json/types \
|
json/types \
|
||||||
maths/maths \
|
maths/maths \
|
||||||
maths/matrix \
|
maths/matrix \
|
||||||
|
matrix \
|
||||||
md2 \
|
md2 \
|
||||||
md4 \
|
md4 \
|
||||||
md5 \
|
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