From 9bae02d86f1b9eac1545bc4654582b6ecc0a3b8d Mon Sep 17 00:00:00 2001
From: philipp schoenberger <philipp.schoenberger@ndsatcom.com>
Date: Tue, 30 Jun 2015 15:48:11 +0200
Subject: [PATCH] add more simple inverse function capable to invert all matrix
 sizes

---
 lwrserv/include/mat.h     | 294 +++++++++-----------------------------
 lwrserv/test/test_mat.cpp | 263 ++++++++++++++++++++++++++++++++++
 2 files changed, 328 insertions(+), 229 deletions(-)

diff --git a/lwrserv/include/mat.h b/lwrserv/include/mat.h
index d2bd9d6..221ebae 100644
--- a/lwrserv/include/mat.h
+++ b/lwrserv/include/mat.h
@@ -1,9 +1,20 @@
 #ifndef _MAT_H_
 #define _MAT_H_
 #include <iostream>
+#include <math.h>
 #include "vec.h"
 
-template <class T, unsigned SIZE> class CVector;
+template <class T, unsigned SIZE> class Vec;
+
+template <class T, unsigned SIZE> class Mat;
+// some common vector classes (abbr. names)
+typedef Mat<float, 2> Mat2f;
+typedef Mat<float, 3> Mat3f;
+typedef Mat<float, 4> Mat4f;
+
+typedef Mat<double, 2> Mat2d;
+typedef Mat<double, 3> Mat3d;
+typedef Mat<double, 4> Mat4d;
 
 // template square matrix class for SIMPLE data types 
 template <class T, unsigned SIZE> class Mat 
@@ -36,6 +47,29 @@ public:
             }
         }
     }
+    Mat<T, SIZE> (const T init_val,const T eye_val)
+    {
+        for (unsigned int j=0; j<SIZE; j++)
+        {
+            for (unsigned int i=0; i<SIZE; i++)
+            {
+                if (j == i )
+                    m_aatData[i][j] = eye_val;
+                else
+                    m_aatData[i][j] = init_val;
+            }
+        }
+    }
+    Mat<T, SIZE> (const T init_val)
+    {
+        for (unsigned int j=0; j<SIZE; j++)
+        {
+            for (unsigned int i=0; i<SIZE; i++)
+            {
+                m_aatData[i][j] = init_val;
+            }
+        }
+    }
 
     Mat<T, SIZE> (const Mat<T, SIZE> &mat)
     {
@@ -66,10 +100,6 @@ public:
         return m_aatData[i][j];
     }
 
-    // CMatrix<T, SIZE>operator + (const Matrix<T, SIZE> &mat)
-    // CMatrix<T, SIZE>operator - (const Matrix<T, SIZE> &mat)
-    // ...
-
     Mat<T, SIZE> operator + (const T &scalar)
     {
         Mat<T, SIZE> buf;
@@ -78,6 +108,7 @@ public:
                 buf(i,j) += m_aatData[i][j] + scalar;
         return buf;
     }
+
     Mat<T, SIZE> operator - (const T &scalar)
     {
         Mat<T, SIZE> buf;
@@ -86,6 +117,7 @@ public:
                 buf(i,j) += m_aatData[i][j] - scalar;
         return buf;
     }
+
     Mat<T, SIZE> operator * (const T &scalar)
     {
         Mat<T, SIZE> buf;
@@ -94,6 +126,7 @@ public:
                 buf(i,j) += m_aatData[i][j] * scalar;
         return buf;
     }
+
     Mat<T, SIZE> operator / (const T &scalar)
     {
         Mat<T, SIZE> buf;
@@ -124,23 +157,30 @@ public:
     }
 #endif
 
-    Mat<T,SIZE> determinant()
+    T determinant()
     {
-        T buf;
+        T buf = 0;
+        if (SIZE == 1)
+            return m_aatData[0][0];
+        if ( SIZE == 2)
+            return m_aatData[0][0] * m_aatData[1][1] - m_aatData[1][0]*m_aatData[0][1];
 
-        for (unsigned int j=0; j<SIZE; j++) // SPALTE j
+
+        for (unsigned int i=0; i<SIZE; i++) // SPALTE j
         {
-            unsigned int i = j;
             T multBuff;
-            multBuff = m_aatData[i][j];
-            multBuff *= m_aatData[(i+1)%SIZE][(i+1)%SIZE];
-            multBuff *= m_aatData[(i+2)%SIZE][(i+2)%SIZE];
+            multBuff = m_aatData[0][i];
+            multBuff *= m_aatData[1][(i+1)%SIZE];
+            multBuff *= m_aatData[2][(i+2)%SIZE];
+            std::cout <<"+("<< i << ", " << (i+1)%SIZE << ", " << (i+2)%SIZE << ") ";
             buf += multBuff;
-            multBuff = m_aatData[i][j];
-            multBuff *= m_aatData[(i-1)%SIZE][(i-1)%SIZE];
-            multBuff *= m_aatData[(i-2)%SIZE][(i-2)%SIZE];
+            multBuff = m_aatData[0][i];
+            multBuff *= m_aatData[1][(i+SIZE-1)%SIZE];
+            multBuff *= m_aatData[2][(i+SIZE-2)%SIZE];
+            std::cout <<"-("<< i <<", " << (i+SIZE-1)%SIZE << ", " << (i+SIZE-2)%SIZE << ") ";
             buf -= multBuff;
         }
+        return buf;
     }
 
     Mat<T,SIZE> norm()
@@ -158,231 +198,27 @@ public:
         Mat<T, SIZE> buf;
         for (unsigned int i=0; i<SIZE; i++) // ZEILE i
             for (unsigned int j=0; j<SIZE; j++) // Spalte j
-                buf(i,j) += m_aatData[j][i];
+                buf.m_aatData[i][j] = m_aatData[j][i];
+        return buf;
     }
-
-    Mat<float, 4> inv()
+    Mat<T,SIZE> inv()
     {
-        Mat<float,4> invOut;
-
-        long indxc[4], indxr[4], ipiv[4];
-        long i, icol, irow, j, ir, ic;
-        long big, dum, pivinv,temp, bb;
-
-        ipiv[0] = -1;
-        ipiv[1] = -1;
-        ipiv[2] = -1;
-        ipiv[3] = -1;
-
-        invOut = *this;
-
-
-        for (i =0; i < 4 ; i++) {
-            big =0.0f;
-            for (j =0; j < 4 ; j++) {
-                if (ipiv[j] != 0) {
-                    if (ipiv[0] == - 1) {
-                        if ((bb = (float) fabs(invOut(j,0))) > big) {
-                            big = bb;
-                            irow = j;
-                            icol = 0;
-                            }
-                    } else if (ipiv[0] > 0) {
-                        goto end;
-                    }
-
-                    if (ipiv[1] == - 1) {
-                        if ((bb = (float) fabs(invOut(j,1))) > big) {
-                            big = bb;
-                            irow = j;
-                            icol = 1;
-                        }
-                    } else if (ipiv[1] > 0) {
-                        goto end;
-                    }
-
-                    if (ipiv[2] == - 1) {
-                        if ((bb = (float) fabs(invOut(j,2))) > big) {
-                            big = bb;
-                            irow = j;
-                            icol = 2;
-                        }
-                    } else if (ipiv[2] > 0) {
-                        goto end;
-                    }
+        T det = determinant();
 
-                    if (ipiv[3] == - 1) {
-                        if ((bb = (float) fabs(invOut(j,3))) > big) {
-                            big = bb;
-                            irow = j;
-                            icol = 3;
-                        }
-                    } else if (ipiv[3] > 0) {
-                        goto end;
-                    }
-                }
-            }
-            ++(ipiv[icol]);
-
-            if (irow != icol) {
-                temp = invOut(irow,0);
-                invOut(irow, 0) = invOut(icol, 0);
-                invOut(icol, 0) = temp;
-
-                temp = invOut(irow,1);
-                invOut(irow, 1) = invOut(icol, 1);
-                invOut(icol,1) = temp;
-
-                temp = invOut(irow,2);
-                invOut(irow,2) = invOut(icol,2);
-                invOut(icol,2) = temp;
-
-                temp = invOut(irow,3);
-                invOut(irow,3) = invOut(icol,3);
-                invOut(icol,3) = temp;
-            }
-
-            indxr[i] = irow;
-            indxc[i] = icol;
-
-            if (invOut(icol,icol) ==0.0) {
-                goto end;
-            }
-
-            pivinv =1.0f / invOut(icol,icol);
-            invOut(icol,icol) =1.0f;
-            invOut(icol,0) *= pivinv;
-            invOut(icol,1) *= pivinv;
-            invOut(icol,2) *= pivinv;
-            invOut(icol,3) *= pivinv;
-
-            if (icol != 0) {
-                dum = invOut(0,icol);
-                invOut(0,icol) =0.0f;
-                invOut(0,0) -= invOut(icol,0) * dum;
-                invOut(0,1) -= invOut(icol,1) * dum;
-                invOut(0,2) -= invOut(icol,2) * dum;
-                invOut(0,3) -= invOut(icol,3) * dum;
-            }
-
-            if (icol != 1) {
-                dum = invOut(1,icol);
-                invOut(1,icol) =0.0f;
-                invOut(1,0) -= invOut(icol,0) * dum;
-                invOut(1,1) -= invOut(icol,1) * dum;
-                invOut(1,2) -= invOut(icol,2) * dum;
-                invOut(1,3) -= invOut(icol,3) * dum;
-            }
-
-            if (icol != 2) {
-                dum = invOut(2,icol);
-                invOut(2,icol) =0.0f;
-                invOut(2,0) -= invOut(icol,0) * dum;
-                invOut(2,1) -= invOut(icol,1) * dum;
-                invOut(2,2) -= invOut(icol,2) * dum;
-                invOut(2,3) -= invOut(icol,3) * dum;
-            }
-
-            if (icol != 3) {
-                dum = invOut(3,icol);
-                invOut(3,icol) =0.0f;
-                invOut(3,0) -= invOut(icol,0) * dum;
-                invOut(3,1) -= invOut(icol,1) * dum;
-                invOut(3,2) -= invOut(icol,2) * dum;
-                invOut(3,3) -= invOut(icol,3) * dum;
-            }
-        }
-        if (indxr[3] != indxc[3]) {
-            ir = indxr[3];
-            ic = indxc[3];
-            temp = invOut(0,ir);
-            invOut(0,ir) = invOut(0,ic);
-            invOut(0,ic) = temp;
-            temp = invOut(1,ir);
-            invOut(1,ir) = invOut(1,ic);
-            invOut(1,ic) = temp;
-            temp = invOut(2,ir);
-            invOut(2,ir) = invOut(2,ic);
-            invOut(2,ic) = temp;
-            temp = invOut(3,ir);
-            invOut(3,ir) = invOut(3,ic);
-            invOut(3,ic) = temp;
-        }
-        if (indxr[2] != indxc[2]) {
-            ir = indxr[2];
-            ic = indxc[2];
-
-            temp = invOut(0,ir);
-            invOut(0,ir) = invOut(0,ic);
-            invOut(0,ic) = temp;
-
-            temp = invOut(1,ir);
-            invOut(1,ir) = invOut(1,ic);
-            invOut(1,ic) = temp;
-
-            temp = invOut(2,ir);
-            invOut(2,ir) = invOut(2,ic);
-            invOut(2,ic) = temp;
-
-            temp = invOut(3,ir);
-            invOut(3,ir) = invOut(3,ic);
-            invOut(3,ic) = temp;
-        }
-        if (indxr[1] != indxc[1]) {
-            ir = indxr[1];
-            ic = indxc[1];
-
-            temp = invOut(0,ir);
-            invOut(0,ir) = invOut(0,ic);
-            invOut(0,ic) = temp;
-
-            temp = invOut(1,ir);
-            invOut(1,ir) = invOut(1,ic);
-            invOut(1,ic) = temp;
-
-            temp = invOut(2,ir);
-            invOut(2,ir) = invOut(2,ic);
-            invOut(2,ic) = temp;
-
-            temp = invOut(3,ir);
-            invOut(3,ir) = invOut(3,ic);
-            invOut(3,ic) = temp;
+        if (det== 0)
+        {
+            return *this;
         }
-        if (indxr[0] != indxc[0]) {
-            ir = indxr[0];
-            ic = indxc[0];
-
-            temp = invOut(0,ir);
-            invOut(0,ir) = invOut(0,ic);
-            invOut(0,ic) = temp;
 
-            temp = invOut(1,ir);
-            invOut(1,ir) = invOut(1,ic);
-            invOut(1,ic) = temp;
+        Mat<float,4> thisTransposed = this->transpose();
+        thisTransposed/det;
 
-            temp = invOut(2,ir);
-            invOut(2,ir) = invOut(2,ic);
-            invOut(2,ic) = temp;
-
-            temp = invOut(3,ir);
-            invOut(3,ir) = invOut(3,ic);
-            invOut(3,ic) = temp;
-        }
-end:
-        return invOut;
+        return thisTransposed;
     }
 
 private:
     T m_aatData[SIZE][SIZE];
 };
-// some common vector classes (abbr. names)
-typedef Mat<float, 2> Mat2f;
-typedef Mat<float, 3> Mat3f;
-typedef Mat<float, 4> Mat4f;
-
-typedef Mat<double, 2> Mat2d;
-typedef Mat<double, 3> Mat3d;
-typedef Mat<double, 4> Mat4d;
 
 
 template <class T, unsigned SIZE>
diff --git a/lwrserv/test/test_mat.cpp b/lwrserv/test/test_mat.cpp
index 73220d2..b102b85 100644
--- a/lwrserv/test/test_mat.cpp
+++ b/lwrserv/test/test_mat.cpp
@@ -8,6 +8,7 @@
 #define TESTSIZE 4
 int testMat [TESTSIZE][TESTSIZE];
 int testVec [TESTSIZE];
+float tolerance = 0.00001f;
 
 TEST_GROUP(Matrix)
 {
@@ -29,6 +30,268 @@ TEST_GROUP(Matrix)
     {
     }
 };
+
+TEST(Matrix, vectorInvRotX)
+{
+    Mat<float, TESTSIZE> a;
+    for(int x = 0 ; x < TESTSIZE ; ++x)
+    {
+        for (int y = 0; y <TESTSIZE ; ++y)
+        {
+            float val = 0.0f;
+            if (x == y )
+                val = 1.0f;
+            a(x,x) = val;
+        }
+    }
+    
+    a(0,0) = 1.0f;
+    a(1,1) = cos(45.0f *M_PI/180.0f);
+    a(1,2) = -sin(45.0f *M_PI/180.0f);
+    a(2,2) = cos(45.0f *M_PI/180.0f);
+    a(2,1) = sin(45.0f *M_PI/180.0f);
+    
+
+
+    float deg = 45.0f*M_PI/180.0f;
+    DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
+    DOUBLES_EQUAL(cos(deg),a(1,1), tolerance);
+    DOUBLES_EQUAL(-sin(deg),a(1,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
+    DOUBLES_EQUAL(sin(deg),a(2,1), tolerance);
+    DOUBLES_EQUAL(cos(deg),a(2,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
+    DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
+    DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
+    
+    a = a.inv();
+    
+    deg = -45.0f*M_PI/180.0f;
+    DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
+    DOUBLES_EQUAL(cos(deg),a(1,1), tolerance);
+    DOUBLES_EQUAL(-sin(deg),a(1,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
+    DOUBLES_EQUAL(sin(deg),a(2,1), tolerance);
+    DOUBLES_EQUAL(cos(deg),a(2,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
+    DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
+    DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
+}
+
+TEST(Matrix, vectorInvRotY)
+{
+    Mat<float, TESTSIZE> a(0.0f);
+    
+    float deg = 45.0f*M_PI/180.0f;
+    a(1,1) = 1.0f;
+    a(0,0) = cos(deg);
+    a(0,2) = sin(deg);
+    a(2,2) = cos(deg);
+    a(2,0) = -sin(deg);
+    a(3,3) = 1.0f;
+    
+    std::cout << a;
+
+    a = a.inv();
+    
+    deg = -45.0f*M_PI/180.0f;
+    DOUBLES_EQUAL(cos(deg),a(0,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
+    DOUBLES_EQUAL(sin(deg),a(0,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
+    DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
+    DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
+    
+    DOUBLES_EQUAL(-sin(deg),a(2,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
+    DOUBLES_EQUAL(cos(deg),a(2,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
+    DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
+    DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
+}
+
+TEST(Matrix, matrixInvEye)
+{
+    Mat<float, TESTSIZE> a(0.0f , 1.0f);
+    
+    // do the invert
+    a = a.inv();
+    DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
+    DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
+    DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
+    DOUBLES_EQUAL(1.0f,a(2,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
+    DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
+    DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
+}
+TEST(Matrix, matrixEye)
+{
+    Mat<float, TESTSIZE> a(0.0f , 1.0f);
+    
+    DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
+    DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
+    DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
+    DOUBLES_EQUAL(1.0f,a(2,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
+    DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
+    DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
+}
+TEST(Matrix, matrixDeterminantEye)
+{
+    //eye matrix
+    Mat<float, TESTSIZE> a(0.0f, 1.0f);
+
+    DOUBLES_EQUAL(1.0f,a.determinant(),tolerance);
+}
+TEST(Matrix, matrixDeterminantEye3)
+{
+    //eye matrix
+    Mat<float, 3> a(0.0f, 1.0f);
+
+    DOUBLES_EQUAL(1.0f,a.determinant(),tolerance);
+}
+TEST(Matrix, matrixDeterminantSimple)
+{
+    Mat<float, 1> a;
+    a(0,0) = 50.0f;
+
+    DOUBLES_EQUAL(50.0f,a.determinant(), tolerance);
+}
+TEST(Matrix, matrixDeterminantSimple2)
+{
+    Mat<float, 2> a;
+    a(0,0) = 10.0f;
+    a(1,1) = 5.0f;
+    DOUBLES_EQUAL(50.0f,a.determinant(), tolerance);
+}
+
+TEST(Matrix, matrixDeterminantSimple3)
+{
+    Mat<float, 2> a;
+    a(0,0) = 10.0f;
+    a(0,1) = 5.0f;
+    a(1,0) = 10.0f;
+    a(1,1) = 5.0f;
+    DOUBLES_EQUAL(0.0f,a.determinant(), tolerance);
+}
+
+TEST(Matrix, matrixDeterminantSimple4)
+{
+    Mat<float, 2> a;
+    a(0,0) = 10.0f;
+    a(0,1) = 5.0f;
+    a(1,0) = 5.0f;
+    a(1,1) = 10.0f;
+    DOUBLES_EQUAL(75.0f,a.determinant(), tolerance);
+}
+
+TEST(Matrix, matrixTransposeEye)
+{
+    Mat<float, TESTSIZE> a;
+    for(int x = 0 ; x < TESTSIZE ; ++x)
+    {
+        for (int y = 0; y <TESTSIZE ; ++y)
+        {
+            float val = 0.0f;
+            if (x == y )
+                val = 1.0f;
+            a(x,y) = val;
+        }
+    }
+    
+    DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
+    DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
+    DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
+    DOUBLES_EQUAL(1.0f,a(2,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
+    DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
+    DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
+
+    a = a.inv();
+
+    // should stille be the same
+    DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
+    DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
+    DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
+    DOUBLES_EQUAL(1.0f,a(2,2), tolerance);
+    DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
+    
+    DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
+    DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
+    DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
+    DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
+}
 TEST(Matrix, vectorMultiply)
 {
     Mat<int, TESTSIZE> a = testMat;