Skip to content

Commit

Permalink
Matrix4.inverse() now computes the general case 4x4 inverse (used to …
Browse files Browse the repository at this point in the history 
…only work for affine transform matrices)
  • Loading branch information
@aslze
aslze committed Nov 23, 2023
1 parent dd77134 commit 60d639e
Showing 1 changed file with 76 additions and 28 deletions.
104 changes: 76 additions & 28 deletions include/asl/Matrix4.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ template<class T>
class Matrix4_
{
T a[4][4]; // row-major

public:
bool isRowMajor() const { return true; }
/**
Expand All @@ -53,12 +53,15 @@ class Matrix4_
/**
Returns the element at row `i`, column `j`.
*/
T& operator()(int i, int j) { return a[i][j]; }
T& operator()(int i, int j) { return a[i][j]; }
const T& operator()(int i, int j) const { return a[i][j]; }

T& at(int i, int j) {return a[i][j];}
T& at(int i, int j) { return a[i][j]; }
const T& at(int i, int j) const { return a[i][j]; }


T* data() { return &a[0][0]; }
const T* data() const { return &a[0][0]; }

template<class T2>
Matrix4_(const Matrix4_<T2>& m)
{
Expand Down Expand Up @@ -341,14 +344,14 @@ class Matrix4_
T norm() const { return sqrt(normSq()); }
};

typedef Matrix4_<float> Matrix4;
typedef Matrix4_<float> Matrix4;
typedef Matrix4_<double> Matrix4d;

/**
Returns an orthonormal approximation of this transform matrix
\ingroup Math3D
*/
template <class T>
template<class T>
asl::Matrix4_<T> orthonormalize(const asl::Matrix4_<T>& m)
{
Vec3_<T> v1 = m.column3(0).normalized();
Expand Down Expand Up @@ -378,9 +381,9 @@ template<class T>
Matrix4_<T> Matrix4_<T>::operator+(const Matrix4_<T>& B) const
{
Matrix4_<T> C;
for(int i=0; i<4; i++)
for(int j=0; j<4; j++)
C(i,j) = a[i][j] + B(i,j);
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
C(i, j) = a[i][j] + B(i, j);
return C;
}

Expand Down Expand Up @@ -408,17 +411,17 @@ template<class T>
Matrix4_<T> Matrix4_<T>::operator*(T t) const
{
Matrix4_<T> C;
for(int i=0; i<4; i++)
for(int j=0; j<4; j++)
C(i,j) = t * a[i][j];
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
C(i, j) = t * a[i][j];
return C;
}

template<class T>
Matrix4_<T>& Matrix4_<T>::operator*=(T t)
{
for (int i = 0; i<4; i++)
for (int j = 0; j<4; j++)
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
a[i][j] *= t;
return *this;
}
Expand Down Expand Up @@ -589,17 +592,52 @@ inline Vec3_<T> Matrix4_<T>::operator%(const Vec3_<T>& p) const
template<class T>
Matrix4_<T> Matrix4_<T>::inverse() const
{
T d = a[0][0] * (a[1][1] * a[2][2] - a[1][2] * a[2][1]) + a[1][0] * (a[0][2] * a[2][1] - a[0][1] * a[2][2]) + a[2][0] * (a[0][1] * a[1][2] - a[0][2] * a[1][1]);
T x = a[0][1] * (a[1][3] * a[2][2] - a[1][2] * a[2][3]) + a[1][1] * (a[0][2] * a[2][3] - a[0][3] * a[2][2]) + a[2][1] * (a[0][3] * a[1][2] - a[0][2] * a[1][3]);
T y = a[0][0] * (a[1][2] * a[2][3] - a[1][3] * a[2][2]) + a[1][0] * (a[0][3] * a[2][2] - a[0][2] * a[2][3]) + a[2][0] * (a[0][2] * a[1][3] - a[0][3] * a[1][2]);
T z = a[0][0] * (a[1][3] * a[2][1] - a[1][1] * a[2][3]) + a[1][0] * (a[0][1] * a[2][3] - a[0][3] * a[2][1]) + a[2][0] * (a[0][3] * a[1][1] - a[0][1] * a[1][3]);

Matrix4_<T> m(
a[1][1] * a[2][2] - a[1][2] * a[2][1], a[0][2] * a[2][1] - a[0][1] * a[2][2], a[0][1] * a[1][2] - a[0][2] * a[1][1], x,
a[1][2] * a[2][0] - a[1][0] * a[2][2], a[0][0] * a[2][2] - a[0][2] * a[2][0], a[0][2] * a[1][0] - a[0][0] * a[1][2], y,
a[1][0] * a[2][1] - a[1][1] * a[2][0], a[0][1] * a[2][0] - a[0][0] * a[2][1], a[0][0] * a[1][1] - a[0][1] * a[1][0], z);
m *= T(1)/d;
m(3, 3) = 1;
T d =
a[0][0] * (a[1][1] * (a[2][2] * a[3][3] - a[2][3] * a[3][2]) + a[1][2] * (a[2][3] * a[3][1] - a[2][1] * a[3][3]) +
a[1][3] * (a[2][1] * a[3][2] - a[2][2] * a[3][1])) +
a[0][1] * (a[1][0] * (a[2][3] * a[3][2] - a[2][2] * a[3][3]) + a[1][2] * (a[2][0] * a[3][3] - a[2][3] * a[3][0]) +
a[1][3] * (a[2][2] * a[3][0] - a[2][0] * a[3][2])) +
a[0][2] * (a[1][0] * (a[2][1] * a[3][3] - a[2][3] * a[3][1]) + a[1][1] * (a[2][3] * a[3][0] - a[2][0] * a[3][3]) +
a[1][3] * (a[2][0] * a[3][1] - a[2][1] * a[3][0])) +
a[0][3] * (a[1][0] * (a[2][2] * a[3][1] - a[2][1] * a[3][2]) + a[1][1] * (a[2][0] * a[3][2] - a[2][2] * a[3][0]) +
a[1][2] * (a[2][1] * a[3][0] - a[2][0] * a[3][1]));

Matrix4_<T> m(a[1][1] * (a[2][2] * a[3][3] - a[2][3] * a[3][2]) + a[1][2] * (a[2][3] * a[3][1] - a[2][1] * a[3][3]) +
a[1][3] * (a[2][1] * a[3][2] - a[2][2] * a[3][1]),
a[0][1] * (a[2][3] * a[3][2] - a[2][2] * a[3][3]) + a[0][2] * (a[2][1] * a[3][3] - a[2][3] * a[3][1]) +
a[0][3] * (a[2][2] * a[3][1] - a[2][1] * a[3][2]),
a[0][1] * (a[1][2] * a[3][3] - a[1][3] * a[3][2]) + a[0][2] * (a[1][3] * a[3][1] - a[1][1] * a[3][3]) +
a[0][3] * (a[1][1] * a[3][2] - a[1][2] * a[3][1]),
a[0][1] * (a[1][3] * a[2][2] - a[1][2] * a[2][3]) + a[0][2] * (a[1][1] * a[2][3] - a[1][3] * a[2][1]) +
a[0][3] * (a[1][2] * a[2][1] - a[1][1] * a[2][2]),

a[1][0] * (a[2][3] * a[3][2] - a[2][2] * a[3][3]) + a[1][2] * (a[2][0] * a[3][3] - a[2][3] * a[3][0]) +
a[1][3] * (a[2][2] * a[3][0] - a[2][0] * a[3][2]),
a[0][0] * (a[2][2] * a[3][3] - a[2][3] * a[3][2]) + a[0][2] * (a[2][3] * a[3][0] - a[2][0] * a[3][3]) +
a[0][3] * (a[2][0] * a[3][2] - a[2][2] * a[3][0]),
a[0][0] * (a[1][3] * a[3][2] - a[1][2] * a[3][3]) + a[0][2] * (a[1][0] * a[3][3] - a[1][3] * a[3][0]) +
a[0][3] * (a[1][2] * a[3][0] - a[1][0] * a[3][2]),
a[0][0] * (a[1][2] * a[2][3] - a[1][3] * a[2][2]) + a[0][2] * (a[1][3] * a[2][0] - a[1][0] * a[2][3]) +
a[0][3] * (a[1][0] * a[2][2] - a[1][2] * a[2][0]),

a[1][0] * (a[2][1] * a[3][3] - a[2][3] * a[3][1]) + a[1][1] * (a[2][3] * a[3][0] - a[2][0] * a[3][3]) +
a[1][3] * (a[2][0] * a[3][1] - a[2][1] * a[3][0]),
a[0][0] * (a[2][3] * a[3][1] - a[2][1] * a[3][3]) + a[0][1] * (a[2][0] * a[3][3] - a[2][3] * a[3][0]) +
a[0][3] * (a[2][1] * a[3][0] - a[2][0] * a[3][1]),
a[0][0] * (a[1][1] * a[3][3] - a[1][3] * a[3][1]) + a[0][1] * (a[1][3] * a[3][0] - a[1][0] * a[3][3]) +
a[0][3] * (a[1][0] * a[3][1] - a[1][1] * a[3][0]),
a[0][0] * (a[1][3] * a[2][1] - a[1][1] * a[2][3]) + a[0][1] * (a[1][0] * a[2][3] - a[1][3] * a[2][0]) +
a[0][3] * (a[1][1] * a[2][0] - a[1][0] * a[2][1]),

a[1][0] * (a[2][2] * a[3][1] - a[2][1] * a[3][2]) + a[1][1] * (a[2][0] * a[3][2] - a[2][2] * a[3][0]) +
a[1][2] * (-a[2][0] * a[3][1] + a[2][1] * a[3][0]),
a[0][0] * (a[2][1] * a[3][2] - a[2][2] * a[3][1]) + a[0][1] * (a[2][2] * a[3][0] - a[2][0] * a[3][2]) +
a[0][2] * (a[2][0] * a[3][1] - a[2][1] * a[3][0]),
a[0][0] * (a[1][2] * a[3][1] - a[1][1] * a[3][2]) + a[0][1] * (a[1][0] * a[3][2] - a[1][2] * a[3][0]) +
a[0][2] * (-a[1][0] * a[3][1] + a[1][1] * a[3][0]),
a[0][0] * (a[1][1] * a[2][2] - a[1][2] * a[2][1]) + a[0][1] * (a[1][2] * a[2][0] - a[1][0] * a[2][2]) +
a[0][2] * (a[1][0] * a[2][1] - a[1][1] * a[2][0]));
m *= T(1) / d;
return m;
}

Expand Down Expand Up @@ -640,10 +678,20 @@ inline Vec3_<T> Matrix4_<T>::axisAngle() const
}

template<class T>
T Matrix4_<T>::det() const
T Matrix4_<T>::det() const
{
return a[0][0]*a[1][1]*a[2][2]-a[0][0]*a[2][1]*a[1][2]-a[1][0]*a[0][1]*a[2][2]+
a[1][0]*a[2][1]*a[0][2]+a[2][0]*a[0][1]*a[1][2]-a[2][0]*a[1][1]*a[0][2];
return a[0][0] *
(a[1][1] * (a[2][2] * a[3][3] - a[2][3] * a[3][2]) + a[1][2] * (a[2][3] * a[3][1] - a[2][1] * a[3][3]) +
a[1][3] * (a[2][1] * a[3][2] - a[2][2] * a[3][1])) +
a[0][1] *
(a[1][0] * (a[2][3] * a[3][2] - a[2][2] * a[3][3]) + a[1][2] * (a[2][0] * a[3][3] - a[2][3] * a[3][0]) +
a[1][3] * (a[2][2] * a[3][0] - a[2][0] * a[3][2])) +
a[0][2] *
(a[1][0] * (a[2][1] * a[3][3] - a[2][3] * a[3][1]) + a[1][1] * (a[2][3] * a[3][0] - a[2][0] * a[3][3]) +
a[1][3] * (a[2][0] * a[3][1] - a[2][1] * a[3][0])) +
a[0][3] *
(a[1][0] * (a[2][2] * a[3][1] - a[2][1] * a[3][2]) + a[1][1] * (a[2][0] * a[3][2] - a[2][2] * a[3][0]) +
a[1][2] * (a[2][1] * a[3][0] - a[2][0] * a[3][1]));
}

template<class T>
Expand Down

0 comments on commit 60d639e

Please sign in to comment.