diff --git a/util/Matrix.cc b/util/Matrix.cc
index 790276c..5d53153 100644
--- a/util/Matrix.cc
+++ b/util/Matrix.cc
@@ -1,6 +1,11 @@
 
 #include "Matrix.h"
 
+Matrix::Matrix()
+{
+    m_inverse_calculated = false;
+}
+
 Matrix Matrix::identity()
 {
     Matrix res;
@@ -47,6 +52,119 @@ double Matrix::determinant()
          - m_matrix[0][3]*m_matrix[1][2]*m_matrix[2][0]*m_matrix[3][1];
 }
 
+void Matrix::calculateInverse()
+{
+    m_inverse_calculated = true;
+    m_inverse_valid = false;
+    double det = determinant();
+    if (det == 0.0)
+        return;
+    m_inverse[0][0] = m_matrix[1][1]*m_matrix[2][2]*m_matrix[3][3]
+                    + m_matrix[1][2]*m_matrix[2][3]*m_matrix[3][1]
+                    + m_matrix[1][3]*m_matrix[2][1]*m_matrix[3][2]
+                    - m_matrix[1][1]*m_matrix[2][3]*m_matrix[3][2]
+                    - m_matrix[1][2]*m_matrix[2][1]*m_matrix[3][3]
+                    - m_matrix[1][3]*m_matrix[2][2]*m_matrix[3][1];
+    m_inverse[0][1] = m_matrix[0][1]*m_matrix[2][3]*m_matrix[3][2]
+                    + m_matrix[0][2]*m_matrix[2][1]*m_matrix[3][3]
+                    + m_matrix[0][3]*m_matrix[2][2]*m_matrix[3][1]
+                    - m_matrix[0][1]*m_matrix[2][2]*m_matrix[3][3]
+                    - m_matrix[0][2]*m_matrix[2][3]*m_matrix[3][1]
+                    - m_matrix[0][3]*m_matrix[2][1]*m_matrix[3][2];
+    m_inverse[0][2] = m_matrix[0][1]*m_matrix[1][2]*m_matrix[3][3]
+                    + m_matrix[0][2]*m_matrix[1][3]*m_matrix[3][1]
+                    + m_matrix[0][3]*m_matrix[1][1]*m_matrix[3][2]
+                    - m_matrix[0][1]*m_matrix[1][3]*m_matrix[3][2]
+                    - m_matrix[0][2]*m_matrix[1][1]*m_matrix[3][3]
+                    - m_matrix[0][3]*m_matrix[1][2]*m_matrix[3][1];
+    m_inverse[0][3] = m_matrix[0][1]*m_matrix[1][3]*m_matrix[2][2]
+                    + m_matrix[0][2]*m_matrix[1][1]*m_matrix[2][3]
+                    + m_matrix[0][3]*m_matrix[1][2]*m_matrix[2][1]
+                    - m_matrix[0][1]*m_matrix[1][2]*m_matrix[2][3]
+                    - m_matrix[0][2]*m_matrix[1][3]*m_matrix[2][1]
+                    - m_matrix[0][3]*m_matrix[1][1]*m_matrix[2][2];
+    m_inverse[1][0] = m_matrix[1][0]*m_matrix[2][3]*m_matrix[3][2]
+                    + m_matrix[1][2]*m_matrix[2][0]*m_matrix[3][3]
+                    + m_matrix[1][3]*m_matrix[2][2]*m_matrix[3][0]
+                    - m_matrix[1][0]*m_matrix[2][2]*m_matrix[3][3]
+                    - m_matrix[1][2]*m_matrix[2][3]*m_matrix[3][0]
+                    - m_matrix[1][3]*m_matrix[2][0]*m_matrix[3][2];
+    m_inverse[1][1] = m_matrix[0][0]*m_matrix[2][2]*m_matrix[3][3]
+                    + m_matrix[0][2]*m_matrix[2][3]*m_matrix[3][0]
+                    + m_matrix[0][3]*m_matrix[2][0]*m_matrix[3][2]
+                    - m_matrix[0][0]*m_matrix[2][3]*m_matrix[3][2]
+                    - m_matrix[0][2]*m_matrix[2][0]*m_matrix[3][3]
+                    - m_matrix[0][3]*m_matrix[2][2]*m_matrix[3][0];
+    m_inverse[1][2] = m_matrix[0][0]*m_matrix[1][3]*m_matrix[3][2]
+                    + m_matrix[0][2]*m_matrix[1][0]*m_matrix[3][3]
+                    + m_matrix[0][3]*m_matrix[1][2]*m_matrix[3][0]
+                    - m_matrix[0][0]*m_matrix[1][2]*m_matrix[3][3]
+                    - m_matrix[0][2]*m_matrix[1][3]*m_matrix[3][0]
+                    - m_matrix[0][3]*m_matrix[1][0]*m_matrix[3][2];
+    m_inverse[1][3] = m_matrix[0][0]*m_matrix[1][2]*m_matrix[2][3]
+                    + m_matrix[0][2]*m_matrix[1][3]*m_matrix[2][0]
+                    + m_matrix[0][3]*m_matrix[1][0]*m_matrix[2][2]
+                    - m_matrix[0][0]*m_matrix[1][3]*m_matrix[2][2]
+                    - m_matrix[0][2]*m_matrix[1][0]*m_matrix[2][3]
+                    - m_matrix[0][3]*m_matrix[1][2]*m_matrix[2][0];
+    m_inverse[2][0] = m_matrix[1][0]*m_matrix[2][1]*m_matrix[3][3]
+                    + m_matrix[1][1]*m_matrix[2][3]*m_matrix[3][0]
+                    + m_matrix[1][3]*m_matrix[2][0]*m_matrix[3][1]
+                    - m_matrix[1][0]*m_matrix[2][3]*m_matrix[3][1]
+                    - m_matrix[1][1]*m_matrix[2][0]*m_matrix[3][3]
+                    - m_matrix[1][3]*m_matrix[2][1]*m_matrix[3][0];
+    m_inverse[2][1] = m_matrix[0][0]*m_matrix[2][3]*m_matrix[3][1]
+                    + m_matrix[0][1]*m_matrix[2][0]*m_matrix[3][3]
+                    + m_matrix[0][3]*m_matrix[2][1]*m_matrix[3][0]
+                    - m_matrix[0][0]*m_matrix[2][1]*m_matrix[3][3]
+                    - m_matrix[0][1]*m_matrix[2][3]*m_matrix[3][0]
+                    - m_matrix[0][3]*m_matrix[2][0]*m_matrix[3][1];
+    m_inverse[2][2] = m_matrix[0][0]*m_matrix[1][1]*m_matrix[3][3]
+                    + m_matrix[0][1]*m_matrix[1][3]*m_matrix[3][0]
+                    + m_matrix[0][3]*m_matrix[1][0]*m_matrix[3][1]
+                    - m_matrix[0][0]*m_matrix[1][3]*m_matrix[3][1]
+                    - m_matrix[0][1]*m_matrix[1][0]*m_matrix[3][3]
+                    - m_matrix[0][3]*m_matrix[1][1]*m_matrix[3][0];
+    m_inverse[2][3] = m_matrix[0][0]*m_matrix[1][3]*m_matrix[2][1]
+                    + m_matrix[0][1]*m_matrix[1][0]*m_matrix[2][3]
+                    + m_matrix[0][3]*m_matrix[1][1]*m_matrix[2][0]
+                    - m_matrix[0][0]*m_matrix[1][1]*m_matrix[2][3]
+                    - m_matrix[0][1]*m_matrix[1][3]*m_matrix[2][0]
+                    - m_matrix[0][3]*m_matrix[1][0]*m_matrix[2][1];
+    m_inverse[3][0] = m_matrix[1][0]*m_matrix[2][2]*m_matrix[3][1]
+                    + m_matrix[1][1]*m_matrix[2][0]*m_matrix[3][2]
+                    + m_matrix[1][2]*m_matrix[2][1]*m_matrix[3][0]
+                    - m_matrix[1][0]*m_matrix[2][1]*m_matrix[3][2]
+                    - m_matrix[1][1]*m_matrix[2][2]*m_matrix[3][0]
+                    - m_matrix[1][2]*m_matrix[2][0]*m_matrix[3][1];
+    m_inverse[3][1] = m_matrix[0][0]*m_matrix[2][1]*m_matrix[3][2]
+                    + m_matrix[0][1]*m_matrix[2][2]*m_matrix[3][0]
+                    + m_matrix[0][2]*m_matrix[2][0]*m_matrix[3][1]
+                    - m_matrix[0][0]*m_matrix[2][2]*m_matrix[3][1]
+                    - m_matrix[0][1]*m_matrix[2][0]*m_matrix[3][2]
+                    - m_matrix[0][2]*m_matrix[2][1]*m_matrix[3][0];
+    m_inverse[3][2] = m_matrix[0][0]*m_matrix[1][2]*m_matrix[3][1]
+                    + m_matrix[0][1]*m_matrix[1][0]*m_matrix[3][2]
+                    + m_matrix[0][2]*m_matrix[1][1]*m_matrix[3][0]
+                    - m_matrix[0][0]*m_matrix[1][1]*m_matrix[3][2]
+                    - m_matrix[0][1]*m_matrix[1][2]*m_matrix[3][0]
+                    - m_matrix[0][2]*m_matrix[1][0]*m_matrix[3][1];
+    m_inverse[3][3] = m_matrix[0][0]*m_matrix[1][1]*m_matrix[2][2]
+                    + m_matrix[0][1]*m_matrix[1][2]*m_matrix[2][0]
+                    + m_matrix[0][2]*m_matrix[1][0]*m_matrix[2][1]
+                    - m_matrix[0][0]*m_matrix[1][2]*m_matrix[2][1]
+                    - m_matrix[0][1]*m_matrix[1][0]*m_matrix[2][2]
+                    - m_matrix[0][2]*m_matrix[1][1]*m_matrix[2][0];
+    for (int i = 0; i < 4; i++)
+    {
+        for (int j = 0; j < 4; j++)
+        {
+            m_inverse[i][j] /= det;
+        }
+    }
+    m_inverse_valid = true;
+}
+
 Matrix operator*(const Matrix & m1, const Matrix & m2)
 {
     Matrix res;
diff --git a/util/Matrix.h b/util/Matrix.h
index 3c3c8f6..7023a70 100644
--- a/util/Matrix.h
+++ b/util/Matrix.h
@@ -7,6 +7,7 @@
 class Matrix
 {
     public:
+        Matrix();
         typedef double Matrix_row_t[4];
         Matrix_row_t & operator[](int idx) { return m_matrix[idx]; }
         const Matrix_row_t & operator[](int idx) const { return m_matrix[idx]; }
@@ -15,6 +16,10 @@ class Matrix
 
     protected:
         double m_matrix[4][4];
+        double m_inverse[4][4];
+        bool m_inverse_calculated;
+        bool m_inverse_valid;
+        void calculateInverse();
 };
 
 Matrix operator*(const Matrix & m1, const Matrix & m2);