Merge pull request #1297 from marauder2k9-torque/matrix-templated

Template Matrix class

Engine/source/T3D/gameBase/gameConnection.h

@@ -52,8 +52,12 @@ enum GameConnectionConstants
 
 class IDisplayDevice;
 class SFXProfile;
+#ifndef USE_TEMPLATE_MATRIX
 class MatrixF;
-class MatrixF;
+#else
+template<typename DATA_TYPE, U32 rows, U32 cols> class Matrix;
+typedef Matrix<F32, 4, 4> MatrixF;
+#endif
 class Point3F;
 class MoveManager;
 class MoveList;

Engine/source/T3D/physics/physicsCollision.h

@@ -28,7 +28,12 @@
 #endif
 
 class Point3F;
+#ifndef USE_TEMPLATE_MATRIX
 class MatrixF;
+#else
+template<typename DATA_TYPE, U32 rows, U32 cols> class Matrix;
+typedef Matrix<F32, 4, 4> MatrixF;
+#endif
 class PlaneF;
 
 
@@ -84,4 +89,4 @@ public:
                                  const MatrixF &localXfm ) = 0;
 };
 
-#endif // _T3D_PHYSICS_PHYSICSCOLLISION_H_
+#endif // _T3D_PHYSICS_PHYSICSCOLLISION_H_

Engine/source/T3D/physics/physicsObject.h

@@ -34,7 +34,12 @@
 #endif
 
 class PhysicsWorld;
+#ifndef USE_TEMPLATE_MATRIX
 class MatrixF;
+#else
+template<typename DATA_TYPE, U32 rows, U32 cols> class Matrix;
+typedef Matrix<F32, 4, 4> MatrixF;
+#endif
 class Point3F;
 class Box3F;
 
@@ -88,4 +93,4 @@ protected:
    U32 mQueuedEvent;
 };
 
-#endif // _T3D_PHYSICS_PHYSICSOBJECT_H_
+#endif // _T3D_PHYSICS_PHYSICSOBJECT_H_

Engine/source/console/propertyParsing.h

@@ -35,7 +35,12 @@ class RectI;
 class RectF;
 class Box3I;
 class Box3F;
+#ifndef USE_TEMPLATE_MATRIX
 class MatrixF;
+#else
+template<typename DATA_TYPE, U32 rows, U32 cols> class Matrix;
+typedef Matrix<F32, 4, 4> MatrixF;
+#endif
 class AngAxisF;
 class QuatF;
 class String;

Engine/source/core/stream/bitStream.h

@@ -45,7 +45,12 @@
 //
 
 class Point3F;
+#ifndef USE_TEMPLATE_MATRIX
 class MatrixF;
+#else
+template<typename DATA_TYPE, U32 rows, U32 cols> class Matrix;
+typedef Matrix<F32, 4, 4> MatrixF;
+#endif
 class HuffmanProcessor;
 class BitVector;
 class QuatF;

Engine/source/gfx/gfxShader.h

@@ -60,7 +60,12 @@
 class Point2I;
 class Point2F;
 class LinearColorF;
+#ifndef USE_TEMPLATE_MATRIX
 class MatrixF;
+#else
+template<typename DATA_TYPE, U32 rows, U32 cols> class Matrix;
+typedef Matrix<F32, 4, 4> MatrixF;
+#endif
 class GFXShader;
 class GFXVertexFormat;
 

Engine/source/math/mAngAxis.h

@@ -27,7 +27,13 @@
 #include "math/mPoint3.h"
 #endif
 
+#ifndef USE_TEMPLATE_MATRIX
 class MatrixF;
+#else
+template<typename DATA_TYPE, U32 rows, U32 cols> class Matrix;
+typedef Matrix<F32, 4, 4> MatrixF;
+#endif
+
 class QuatF;
 
 //----------------------------------------------------------------------------

Engine/source/math/mBox.h

@@ -36,7 +36,12 @@
 #endif
 
 
+#ifndef USE_TEMPLATE_MATRIX
 class MatrixF;
+#else
+template<typename DATA_TYPE, U32 rows, U32 cols> class Matrix;
+typedef Matrix<F32, 4, 4> MatrixF;
+#endif
 class SphereF;
 
 

Engine/source/math/mMatrix.cpp

@@ -29,6 +29,8 @@
 #include "console/enginePrimitives.h"
 #include "console/engineTypes.h"
 
+#ifndef USE_TEMPLATE_MATRIX
+
 const MatrixF MatrixF::Identity( true );
 
 // idx(i,j) is index to element in column i, row j
@@ -209,3 +211,12 @@ EngineFieldTable::Field MatrixFEngineExport::getMatrixField()
    typedef MatrixF ThisType;
    return _FIELD_AS(F32, m, m, 16, "");
 }
+
+#else // !USE_TEMPLATE_MATRIX
+
+//------------------------------------
+// Templatized matrix class to replace MATRIXF above
+// due to templated class, all functions need to be inline
+//------------------------------------
+
+#endif // !USE_TEMPLATE_MATRIX

Engine/source/math/mMatrix.h

@@ -39,6 +39,19 @@
 #include "console/engineTypeInfo.h"
 #endif
 
+#ifndef _FRAMEALLOCATOR_H_
+#include "core/frameAllocator.h"
+#endif
+
+#ifndef _STRINGFUNCTIONS_H_
+#include "core/strings/stringFunctions.h"
+#endif
+
+#ifndef _CONSOLE_H_
+#include "console/console.h"
+#endif
+
+#ifndef USE_TEMPLATE_MATRIX
 
 /// 4x4 Matrix Class
 ///
@@ -114,6 +127,10 @@ public:
 
    EulerF toEuler() const;
 
+   F32 determinant() const {
+      return m_matF_determinant(*this);
+   }
+
    /// Compute the inverse of the matrix.
    ///
    /// Computes inverse of full 4x4 matrix. Returns false and performs no inverse if
@@ -620,4 +637,1386 @@ inline void mTransformPlane(const MatrixF& mat, const Point3F& scale, const Plan
    m_matF_x_scale_x_planeF(mat, &scale.x, &plane.x, &result->x);
 }
 
+#else // !USE_TEMPLATE_MATRIX
+
+//------------------------------------
+// Templatized matrix class to replace MATRIXF above
+//------------------------------------
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+class Matrix {
+   friend class MatrixTemplateExport;
+private:
+   DATA_TYPE data[rows * cols];
+
+public:
+
+   static_assert(rows >= 2 && cols >= 2, "Matrix must have at least 2 rows and 2 cols.");
+
+   // ------ Setters and initializers ------
+   explicit Matrix(bool identity = false) {
+      std::fill(data, data + (rows * cols), DATA_TYPE(0));
+
+      if (identity) {
+         for (U32 i = 0; i < rows; i++) {
+            for (U32 j = 0; j < cols; j++) {
+               // others already get filled with 0
+               if (j == i)
+                  (*this)(i, j) = static_cast<DATA_TYPE>(1);
+            }
+         }
+      }
+   }
+
+   explicit Matrix(const EulerF& e) {
+      set(e);
+   }
+
+   ~Matrix() = default;
+
+   /// Make this an identity matrix.
+   Matrix<DATA_TYPE, rows, cols>& identity();
+   void reverseProjection();
+   void normalize();
+
+   Matrix<DATA_TYPE, rows, cols>& set(const EulerF& e);
+
+   Matrix(const EulerF& e, const Point3F p);
+   Matrix<DATA_TYPE, rows, cols>& set(const EulerF& e, const Point3F p);
+
+   Matrix<DATA_TYPE, rows, cols>& inverse();
+   Matrix<DATA_TYPE, rows, cols>& transpose();
+   void invert();
+
+   Matrix<DATA_TYPE, rows, cols>& setCrossProduct(const Point3F& p);
+   Matrix<DATA_TYPE, rows, cols>& setTensorProduct(const Point3F& p, const Point3F& q);
+
+   /// M * Matrix(p) -> M
+   Matrix<DATA_TYPE, rows, cols>& scale(const Point3F& s);
+   Matrix<DATA_TYPE, rows, cols>& scale(DATA_TYPE s) { return scale(Point3F(s, s, s)); }
+
+   void setColumn(S32 col, const Point4F& cptr);
+   void setColumn(S32 col, const Point3F& cptr);
+   void setRow(S32 row, const Point4F& cptr);
+   void setRow(S32 row, const Point3F& cptr);
+   void displace(const Point3F& delta);
+   bool fullInverse();
+
+   void setPosition(const Point3F& pos) { setColumn(3, pos); }
+
+   DATA_TYPE determinant() const {
+      AssertFatal(rows == cols, "Determinant is only defined for square matrices.");
+      // For simplicity, only implement for 3x3 matrices
+      AssertFatal(rows >= 3 && cols >= 3, "Determinant only for 3x3 or more"); // Ensure the matrix is 3x3
+      return (*this)(0, 0) * ((*this)(1, 1) * (*this)(2, 2) - (*this)(1, 2) * (*this)(2, 1)) +
+             (*this)(1, 0) * ((*this)(0, 2) * (*this)(2, 1) - (*this)(0, 1) * (*this)(2, 2)) +
+             (*this)(2, 0) * ((*this)(0, 1) * (*this)(1, 2) - (*this)(0, 2) * (*this)(1, 1));
+   }
+
+   ///< M * a -> M
+   Matrix<DATA_TYPE, rows, cols>& mul(const Matrix<DATA_TYPE, rows, cols>& a)
+   { return *this = *this * a; }
+   ///< a * M -> M
+   Matrix<DATA_TYPE, rows, cols>& mulL(const Matrix<DATA_TYPE, rows, cols>& a)
+   { return *this = a * *this; }
+   ///< a * b -> M
+   Matrix<DATA_TYPE, rows, cols>& mul(const Matrix<DATA_TYPE, rows, cols>& a, const Matrix<DATA_TYPE, rows, cols>& b)
+   { return *this = a * b; }
+   ///< M * a -> M
+   Matrix<DATA_TYPE, rows, cols>& mul(const F32 a)
+   { return *this = *this * a; }
+   ///< a * b -> M
+   Matrix<DATA_TYPE, rows, cols>& mul(const Matrix<DATA_TYPE, rows, cols>& a, const F32 b)
+   { return *this = a * b; }
+   Matrix<DATA_TYPE, rows, cols>& add(const Matrix<DATA_TYPE, rows, cols>& a)
+   {
+      return *this = *this += a;
+   }
+
+   ///< M * p -> p (full [4x4] * [1x4])
+   void mul(Point4F& p) const { p = *this * p; }
+   ///< M * p -> p (assume w = 1.0f)
+   void mulP(Point3F& p) const {
+      Point3F result;
+      result.x = (*this)(0, 0) * p.x + (*this)(0, 1) * p.y + (*this)(0, 2) * p.z + (*this)(0, 3);
+      result.y = (*this)(1, 0) * p.x + (*this)(1, 1) * p.y + (*this)(1, 2) * p.z + (*this)(1, 3);
+      result.z = (*this)(2, 0) * p.x + (*this)(2, 1) * p.y + (*this)(2, 2) * p.z + (*this)(2, 3);
+
+      p = result;
+   }
+   ///< M * p -> d (assume w = 1.0f)
+   void mulP(const Point3F& p, Point3F* d) const { *d = *this * p; }
+   ///< M * v -> v (assume w = 0.0f)
+   void mulV(VectorF& v) const
+   {
+      AssertFatal(rows == 4 && cols == 4, "Multiplying VectorF with matrix requires 4x4");
+      VectorF result(
+         (*this)(0, 0) * v.x + (*this)(0, 1) * v.y + (*this)(0, 2) * v.z,
+         (*this)(1, 0) * v.x + (*this)(1, 1) * v.y + (*this)(1, 2) * v.z,
+         (*this)(2, 0) * v.x + (*this)(2, 1) * v.y + (*this)(2, 2) * v.z
+      );
+
+      v = result;
+
+   }
+   ///< M * v -> d (assume w = 0.0f)
+   void mulV(const VectorF& v, Point3F* d) const
+   {
+      AssertFatal(rows == 4 && cols == 4, "Multiplying VectorF with matrix requires 4x4");
+      VectorF result(
+         (*this)(0, 0) * v.x + (*this)(0, 1) * v.y + (*this)(0, 2) * v.z,
+         (*this)(1, 0) * v.x + (*this)(1, 1) * v.y + (*this)(1, 2) * v.z,
+         (*this)(2, 0) * v.x + (*this)(2, 1) * v.y + (*this)(2, 2) * v.z
+      );
+
+      d->x = result.x;
+      d->y = result.y;
+      d->z = result.z;
+   }
+
+   ///< Axial box -> Axial Box (too big a function to be inline)
+   void mul(Box3F& box) const;
+
+   // ------ Getters ------
+   bool isNaN() {
+      for (U32 i = 0; i < rows; i++) {
+         for (U32 j = 0; j < cols; j++) {
+            if (mIsNaN_F((*this)(i, j)))
+               return true;
+         }
+      }
+
+      return false;
+   }
+   // row + col * cols
+   static U32 idx(U32 i, U32 j) { return (i + j * cols); }
+   bool isAffine() const;
+   bool isIdentity() const;
+   /// Take inverse of matrix assuming it is affine (rotation,
+   /// scale, sheer, translation only).
+   Matrix<DATA_TYPE, rows, cols>& affineInverse();
+
+   Point3F getScale() const;
+   
+   EulerF toEuler() const;
+
+   Point3F getPosition() const;
+
+   void getColumn(S32 col, Point4F* cptr) const;
+   Point4F getColumn4F(S32 col) const { Point4F ret; getColumn(col, &ret); return ret; }
+
+   void getColumn(S32 col, Point3F* cptr) const;
+   Point3F getColumn3F(S32 col) const { Point3F ret; getColumn(col, &ret); return ret; }
+
+   void getRow(S32 row, Point4F* cptr) const;
+   Point4F getRow4F(S32 row) const { Point4F ret; getRow(row, &ret); return ret; }
+
+   void getRow(S32 row, Point3F* cptr) const;
+   Point3F getRow3F(S32 row) const { Point3F ret; getRow(row, &ret); return ret; }
+
+   VectorF getRightVector() const;
+   VectorF getForwardVector() const;
+   VectorF getUpVector() const;
+
+   DATA_TYPE* getData() {
+      return data;
+   }
+
+   const DATA_TYPE* getData() const {
+      return data;
+   }
+
+   void transposeTo(Matrix<DATA_TYPE, cols, rows>& matrix) const {
+      for (U32 i = 0; i < rows; ++i) {
+         for (U32 j = 0; j < cols; ++j) {
+            matrix(j, i) = (*this)(i, j);
+         }
+      }
+   }
+
+   void swap(DATA_TYPE& a, DATA_TYPE& b) {
+      DATA_TYPE temp = a;
+      a = b;
+      b = temp;
+   }
+
+   void invertTo(Matrix<DATA_TYPE, cols, rows>* matrix) const;
+
+   void dumpMatrix(const char* caption = NULL) const;
+   // Static identity matrix
+   static const Matrix Identity;
+
+   // ------ Operators ------
+   friend Matrix<DATA_TYPE, rows, cols> operator*(const Matrix<DATA_TYPE, rows, cols>& m1, const Matrix<DATA_TYPE, rows, cols>& m2) {
+      Matrix<DATA_TYPE, rows, cols> result;
+
+      for (U32 i = 0; i < rows; ++i) {
+        for (U32 j = 0; j < cols; ++j)
+        {
+            result(i, j) = static_cast<DATA_TYPE>(0);
+            for (U32 k = 0; k < cols; ++k)
+            {
+                result(i, j) += m1(i, k) * m2(k, j);
+            }
+         }
+      }
+
+      return result;
+   }
+
+   Matrix<DATA_TYPE, rows, cols> operator *= (const Matrix<DATA_TYPE, rows, cols>& other) {
+      *this = *this * other;
+      return *this;
+   }
+
+   Matrix<DATA_TYPE, rows, cols> operator+(const Matrix<DATA_TYPE, rows, cols>& m2) {
+      Matrix<DATA_TYPE, rows, cols> result;
+
+      for (U32 i = 0; i < rows; ++i)
+      {
+         for (U32 j = 0; j < cols; ++j)
+         {
+            result(i, j) = 0; // Initialize result element to 0
+            result(i, j) = (*this)(i, j) + m2(i, j);
+         }
+      }
+
+      return result;
+   }
+
+   Matrix<DATA_TYPE, rows, cols> operator+=(const Matrix<DATA_TYPE, rows, cols>& m2) {
+      for (U32 i = 0; i < rows; ++i)
+      {
+         for (U32 j = 0; j < cols; ++j)
+         {
+            (*this)(i, j) += m2(i, j);
+         }
+      }
+
+      return (*this);
+   }
+
+   Matrix<DATA_TYPE, rows, cols> operator * (const DATA_TYPE scalar) const {
+      Matrix<DATA_TYPE, rows, cols> result;
+      for (U32 i = 0; i < rows; i++)
+      {
+         for (U32 j = 0; j < cols; j++)
+         {
+            result(i, j) = (*this)(i, j) * scalar;
+         }
+      }
+
+      return result;
+   }
+   Matrix<DATA_TYPE, rows, cols>& operator *= (const DATA_TYPE scalar) {
+      for (U32 i = 0; i < rows; i++)
+      {
+         for (U32 j = 0; j < cols; j++)
+         {
+            (*this)(i, j) *= scalar;
+         }
+      }
+
+      return *this;
+   }
+
+   Point3F operator*(const Point3F& point) const {
+      AssertFatal(rows == 4 && cols == 4, "Multiplying point3 with matrix requires 4x4");
+      Point3F result;
+      result.x = (*this)(0, 0) * point.x + (*this)(0, 1) * point.y + (*this)(0, 2) * point.z + (*this)(0, 3);
+      result.y = (*this)(1, 0) * point.x + (*this)(1, 1) * point.y + (*this)(1, 2) * point.z + (*this)(1, 3);
+      result.z = (*this)(2, 0) * point.x + (*this)(2, 1) * point.y + (*this)(2, 2) * point.z + (*this)(2, 3);
+
+      return result;
+   }
+   
+   Point4F operator*(const Point4F& point) const {
+      AssertFatal(rows == 4 && cols == 4, "Multiplying point4 with matrix requires 4x4");
+      return Point4F(
+         (*this)(0, 0) * point.x + (*this)(0, 1) * point.y + (*this)(0, 2) * point.z + (*this)(0, 3) * point.w,
+         (*this)(1, 0) * point.x + (*this)(1, 1) * point.y + (*this)(1, 2) * point.z + (*this)(1, 3) * point.w,
+         (*this)(2, 0) * point.x + (*this)(2, 1) * point.y + (*this)(2, 2) * point.z + (*this)(2, 3) * point.w,
+         (*this)(3, 0) * point.x + (*this)(3, 1) * point.y + (*this)(3, 2) * point.z + (*this)(3, 3) * point.w
+      );
+   }
+
+   Matrix<DATA_TYPE, rows, cols>& operator = (const Matrix<DATA_TYPE, rows, cols>& other) {
+      if (this != &other) {
+         std::copy(other.data, other.data + rows * cols, this->data);
+      }
+
+      return *this;
+   }
+
+   bool operator == (const Matrix<DATA_TYPE, rows, cols>& other) const {
+      for (U32 i = 0; i < rows; i++)
+      {
+         for (U32 j = 0; j < cols; j++)
+         {
+            if ((*this)(i, j) != other(i, j))
+               return false;
+         }
+      }
+      return true;
+   }
+
+   bool operator != (const Matrix<DATA_TYPE, rows, cols>& other) const {
+      return !(*this == other);
+   }
+
+   operator DATA_TYPE* () { return (data); }
+   operator const DATA_TYPE* () const { return (DATA_TYPE*)(data); }
+
+   DATA_TYPE& operator () (U32 row, U32 col) {
+      if (row >= rows || col >= cols)
+         AssertFatal(false, "Matrix indices out of range");
+
+      return data[idx(col,row)];
+   }
+
+   DATA_TYPE operator () (U32 row, U32 col) const {
+      if (row >= rows || col >= cols)
+         AssertFatal(false, "Matrix indices out of range");
+
+      return data[idx(col, row)];
+   }
+};
+
+//--------------------------------------------
+// INLINE FUNCTIONS
+//--------------------------------------------
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::transpose()
+{
+   AssertFatal(rows == cols, "Transpose can only be performed on square matrices.");
+
+   swap((*this)(0, 1), (*this)(1, 0));
+   swap((*this)(0, 2), (*this)(2, 0));
+   swap((*this)(0, 3), (*this)(3, 0));
+   swap((*this)(1, 2), (*this)(2, 1));
+   swap((*this)(1, 3), (*this)(3, 1));
+   swap((*this)(2, 3), (*this)(3, 2));
+
+   return (*this);
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::identity()
+{
+   for (U32 i = 0; i < rows; i++)
+   {
+      for (U32 j = 0; j < cols; j++)
+      {
+         if (j == i)
+            (*this)(i, j) = static_cast<DATA_TYPE>(1);
+         else
+            (*this)(i, j) = static_cast<DATA_TYPE>(0);
+      }
+   }
+
+   return (*this);
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline void Matrix<DATA_TYPE, rows, cols>::normalize()
+{
+   AssertFatal(rows >= 3 && cols >= 3, "Normalize can only be applied 3x3 or more");
+   Point3F col0, col1, col2;
+   getColumn(0, &col0);
+   getColumn(1, &col1);
+
+   mCross(col0, col1, &col2);
+   mCross(col2, col0, &col1);
+
+   col0.normalize();
+   col1.normalize();
+   col2.normalize();
+
+   setColumn(0, col0);
+   setColumn(1, col1);
+   setColumn(2, col2);
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::scale(const Point3F& s)
+{
+   // torques scale applies directly, does not create another matrix to multiply with the translation matrix.
+   AssertFatal(rows >= 4 && cols >= 4, "Scale can only be applied 4x4 or more");
+
+   (*this)(0, 0) *= s.x;   (*this)(0, 1) *= s.y;   (*this)(0, 2) *= s.z;
+   (*this)(1, 0) *= s.x;   (*this)(1, 1) *= s.y;   (*this)(1, 2) *= s.z;
+   (*this)(2, 0) *= s.x;   (*this)(2, 1) *= s.y;   (*this)(2, 2) *= s.z;
+   (*this)(3, 0) *= s.x;   (*this)(3, 1) *= s.y;   (*this)(3, 2) *= s.z;
+
+   return (*this);
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline bool Matrix<DATA_TYPE, rows, cols>::isIdentity() const {
+   for (U32 i = 0; i < rows; i++)
+   {
+      for (U32 j = 0; j < cols; j++)
+      {
+         if (j == i)
+         {
+            if((*this)(i, j) != static_cast<DATA_TYPE>(1))
+            {
+               return false;
+            }
+         }
+         else
+         {
+            if((*this)(i, j) != static_cast<DATA_TYPE>(0))
+            {
+               return false;
+            }
+         }
+      }
+   }
+   
+   return true;
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline Point3F Matrix<DATA_TYPE, rows, cols>::getScale() const
+{
+   // this function assumes the matrix has scale applied through the scale(const Point3F& s) function.
+   // for now assume float since we have point3F.
+   AssertFatal(rows >= 4 && cols >= 4, "Scale can only be applied 4x4 or more");
+
+   Point3F scale;
+
+   scale.x = mSqrt((*this)(0, 0) * (*this)(0, 0) + (*this)(1, 0) * (*this)(1, 0) + (*this)(2, 0) * (*this)(2, 0));
+   scale.y = mSqrt((*this)(0, 1) * (*this)(0, 1) + (*this)(1, 1) * (*this)(1, 1) + (*this)(2, 1) * (*this)(2, 1));
+   scale.z = mSqrt((*this)(0, 2) * (*this)(0, 2) + (*this)(1, 2) * (*this)(1, 2) + (*this)(2, 2) * (*this)(2, 2));
+
+   return scale;
+}
+
+
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline Point3F Matrix<DATA_TYPE, rows, cols>::getPosition() const
+{
+   Point3F pos;
+   getColumn(3, &pos);
+   return pos;
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline void Matrix<DATA_TYPE, rows, cols>::getColumn(S32 col, Point4F* cptr) const
+{
+   if (rows >= 2)
+   {
+      cptr->x = (*this)(0, col);
+      cptr->y = (*this)(1, col);
+   }
+
+   if (rows >= 3)
+      cptr->z = (*this)(2, col);
+   else
+      cptr->z = 0.0f;
+
+   if (rows >= 4)
+      cptr->w = (*this)(3, col);
+   else
+      cptr->w = 0.0f;
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline void Matrix<DATA_TYPE, rows, cols>::getColumn(S32 col, Point3F* cptr) const
+{
+   if (rows >= 2)
+   {
+      cptr->x = (*this)(0, col);
+      cptr->y = (*this)(1, col);
+   }
+
+   if (rows >= 3)
+      cptr->z = (*this)(2, col);
+   else
+      cptr->z = 0.0f;
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline void Matrix<DATA_TYPE, rows, cols>::setColumn(S32 col, const Point4F &cptr) {
+   if(rows >= 2)
+   {
+      (*this)(0, col) = cptr.x;
+      (*this)(1, col) = cptr.y;
+   }
+   
+   if(rows >= 3)
+      (*this)(2, col) = cptr.z;
+   
+   if(rows >= 4)
+      (*this)(3, col) = cptr.w;
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline void Matrix<DATA_TYPE, rows, cols>::setColumn(S32 col, const Point3F &cptr) {
+   if(rows >= 2)
+   {
+      (*this)(0, col) = cptr.x;
+      (*this)(1, col) = cptr.y;
+   }
+   
+   if(rows >= 3)
+      (*this)(2, col) = cptr.z;
+   
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline void Matrix<DATA_TYPE, rows, cols>::getRow(S32 row, Point4F* cptr) const
+{
+   if (cols >= 2)
+   {
+      cptr->x = (*this)(row, 0);
+      cptr->y = (*this)(row, 1);
+   }
+
+   if (cols >= 3)
+      cptr->z = (*this)(row, 2);
+   else
+      cptr->z = 0.0f;
+
+   if (cols >= 4)
+      cptr->w = (*this)(row, 3);
+   else
+      cptr->w = 0.0f;
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline void Matrix<DATA_TYPE, rows, cols>::getRow(S32 row, Point3F* cptr) const
+{
+   if (cols >= 2)
+   {
+      cptr->x = (*this)(row, 0);
+      cptr->y = (*this)(row, 1);
+   }
+
+   if (cols >= 3)
+      cptr->z = (*this)(row, 2);
+   else
+      cptr->z = 0.0f;
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline VectorF Matrix<DATA_TYPE, rows, cols>::getRightVector() const
+{
+   VectorF vec;
+   getColumn(0, &vec);
+   return vec;
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline VectorF Matrix<DATA_TYPE, rows, cols>::getForwardVector() const
+{
+   VectorF vec;
+   getColumn(1, &vec);
+   return vec;
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline VectorF Matrix<DATA_TYPE, rows, cols>::getUpVector() const
+{
+   VectorF vec;
+   getColumn(2, &vec);
+   return vec;
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline void Matrix<DATA_TYPE, rows, cols>::invertTo(Matrix<DATA_TYPE, cols, rows>* matrix) const
+{
+   Matrix<DATA_TYPE, rows, cols> invMatrix;
+   for (U32 i = 0; i < rows; ++i)
+   {
+      for (U32 j = 0; j < cols; ++j)
+      {
+         invMatrix(i, j) = (*this)(i, j);
+      }
+   }
+
+   invMatrix.inverse();
+
+   for (U32 i = 0; i < rows; ++i)
+   {
+      for (U32 j = 0; j < cols; ++j)
+      {
+         (*matrix)(i, j) = invMatrix(i, j);
+      }
+   }
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline void Matrix<DATA_TYPE, rows, cols>::setRow(S32 row, const Point4F& cptr) {
+   if(cols >= 2)
+   {
+      (*this)(row, 0) = cptr.x;
+      (*this)(row, 1) = cptr.y;
+   }
+   
+   if(cols >= 3)
+      (*this)(row, 2) = cptr.z;
+   
+   if(cols >= 4)
+      (*this)(row, 3) = cptr.w;
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline void Matrix<DATA_TYPE, rows, cols>::setRow(S32 row, const Point3F& cptr) {
+   if(cols >= 2)
+   {
+      (*this)(row, 0) = cptr.x;
+      (*this)(row, 1) = cptr.y;
+   }
+   
+   if(cols >= 3)
+      (*this)(row, 2) = cptr.z;
+   
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline void Matrix<DATA_TYPE, rows, cols>::displace(const Point3F& delta)
+{
+   (*this)(0, 3) += delta.x;
+   (*this)(1, 3) += delta.y;
+   (*this)(2, 3) += delta.z;
+}
+
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline void Matrix<DATA_TYPE, rows, cols>::reverseProjection()
+{
+   AssertFatal(rows == 4 && cols == 4, "reverseProjection requires a 4x4 matrix.");
+
+   (*this)(2, 0) = (*this)(3, 0) - (*this)(2, 0);
+   (*this)(2, 1) = (*this)(3, 1) - (*this)(2, 1);
+   (*this)(2, 2) = (*this)(3, 2) - (*this)(2, 2);
+   (*this)(2, 3) = (*this)(3, 3) - (*this)(2, 3);
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+const Matrix<DATA_TYPE, rows, cols> Matrix<DATA_TYPE, rows, cols>::Identity = []() {
+   Matrix<DATA_TYPE, rows, cols> identity(true);
+   return identity;
+}();
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::set(const EulerF& e)
+{
+   // when the template refactor is done, euler will be able to be setup in different ways
+   AssertFatal(rows >= 3 && cols >= 3, "EulerF can only initialize 3x3 or more");
+   static_assert(std::is_same<DATA_TYPE, float>::value, "Can only initialize eulers with floats for now");
+
+   F32 cosPitch, sinPitch;
+   mSinCos(e.x, sinPitch, cosPitch);
+
+   F32 cosYaw, sinYaw;
+   mSinCos(e.y, sinYaw, cosYaw);
+
+   F32 cosRoll, sinRoll;
+   mSinCos(e.z, sinRoll, cosRoll);
+
+   enum {
+      AXIS_X = (1 << 0),
+      AXIS_Y = (1 << 1),
+      AXIS_Z = (1 << 2)
+   };
+
+   U32 axis = 0;
+   if (e.x != 0.0f) axis |= AXIS_X;
+   if (e.y != 0.0f) axis |= AXIS_Y;
+   if (e.z != 0.0f) axis |= AXIS_Z;
+
+   switch (axis) {
+   case 0:
+      (*this) = Matrix<DATA_TYPE, rows, cols>(true);
+      break;
+   case AXIS_X:
+      (*this)(0, 0) = 1.0f; (*this)(0, 1) = 0.0f;      (*this)(0, 2) = 0.0f;
+      (*this)(1, 0) = 0.0f; (*this)(1, 1) = cosPitch;  (*this)(1, 2) = sinPitch; 
+      (*this)(2, 0) = 0.0f; (*this)(2, 1) = -sinPitch; (*this)(2, 2) = cosPitch;
+      break;
+   case AXIS_Y:
+      (*this)(0, 0) = cosYaw;    (*this)(1, 0) = 0.0f;   (*this)(2, 0) = sinYaw;
+      (*this)(0, 1) = 0.0f;      (*this)(1, 1) = 1.0f;   (*this)(2, 1) = 0.0f;
+      (*this)(0, 2) = -sinYaw;   (*this)(1, 2) = 0.0f;   (*this)(2, 2) = cosYaw;
+      break;
+   case AXIS_Z:
+      (*this)(0, 0) = cosRoll;  (*this)(0, 1) = sinRoll; (*this)(0, 2) = 0.0f;   
+      (*this)(1, 0) = -sinRoll; (*this)(1, 1) = cosRoll; (*this)(1, 2) = 0.0f; 
+      (*this)(2, 0) = 0.0f;     (*this)(2, 1) = 0.0f;    (*this)(2, 2) = 1.0f;
+      break;
+   default:
+      F32 r1 = cosYaw * cosRoll;
+      F32 r2 = cosYaw * sinRoll;
+      F32 r3 = sinYaw * cosRoll;
+      F32 r4 = sinYaw * sinRoll;
+
+      // the matrix looks like this:
+      //  r1 - (r4 * sin(x))     r2 + (r3 * sin(x))   -cos(x) * sin(y)
+      //  -cos(x) * sin(z)       cos(x) * cos(z)      sin(x)
+      //  r3 + (r2 * sin(x))     r4 - (r1 * sin(x))   cos(x) * cos(y)
+      //
+      // where:
+      //  r1 = cos(y) * cos(z)
+      //  r2 = cos(y) * sin(z)
+      //  r3 = sin(y) * cos(z)
+      //  r4 = sin(y) * sin(z)
+
+      // init the euler 3x3 rotation matrix.
+      (*this)(0, 0) = r1 - (r4 * sinPitch); (*this)(0, 1) = r2 + (r3 * sinPitch); (*this)(0, 2) = -cosPitch * sinYaw;  
+      (*this)(1, 0) = -cosPitch * sinRoll;  (*this)(1, 1) = cosPitch * cosRoll;   (*this)(1, 2) = sinPitch;    
+      (*this)(2, 0) = r3 + (r2 * sinPitch); (*this)(2, 1) = r4 - (r1 * sinPitch); (*this)(2, 2) = cosPitch * cosYaw;
+      break;
+   }
+
+   if (rows == 4)
+   {
+      (*this)(3, 0) = 0.0f;
+      (*this)(3, 1) = 0.0f;
+      (*this)(3, 2) = 0.0f;
+   }
+
+   if (cols == 4)
+   {
+      (*this)(0, 3) = 0.0f;
+      (*this)(1, 3) = 0.0f;
+      (*this)(2, 3) = 0.0f;
+   }
+
+   if (rows == 4 && cols == 4)
+   {
+      (*this)(3, 3) = 1.0f;
+   }
+
+   return(*this);
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+Matrix<DATA_TYPE, rows, cols>::Matrix(const EulerF& e, const Point3F p)
+{
+   set(e, p);
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::set(const EulerF& e, const Point3F p)
+{
+   AssertFatal(rows >= 3 && cols >= 4, "Euler and Point can only initialize 3x4 or more");
+   // call set euler, this already sets the last row if it exists.
+   set(e);
+
+   // does this need to multiply with the result of the euler? or are we just setting position.
+   (*this)(0, 3) = p.x;
+   (*this)(1, 3) = p.y;
+   (*this)(2, 3) = p.z;
+
+   return (*this);
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::inverse()
+{
+#if 1
+   // NOTE: Gauss-Jordan elimination is yielding unpredictable results due to precission handling and
+   // numbers near 0.0
+   // 
+   AssertFatal(rows == cols, "Can only perform inverse on square matrices.");
+   const U32 size = rows - 1;
+   const DATA_TYPE pivot_eps = static_cast<DATA_TYPE>(1e-20);  // Smaller epsilon to handle numerical precision
+
+   // Create augmented matrix [this | I]
+   Matrix<DATA_TYPE, size, rows + size> augmentedMatrix;
+
+   for (U32 i = 0; i < size; i++)
+   {
+      for (U32 j = 0; j < size; j++)
+      {
+         augmentedMatrix(i, j) = (*this)(i, j);
+         augmentedMatrix(i, j + size) = (i == j) ? static_cast<DATA_TYPE>(1) : static_cast<DATA_TYPE>(0);
+      }
+   }
+
+   // Apply gauss-joran elimination
+   for (U32 i = 0; i < size; i++)
+   {
+      U32 pivotRow = i;
+
+      DATA_TYPE pivotValue = std::abs(augmentedMatrix(i, i));
+
+      for (U32 k = i + 1; k < size; k++)
+      {
+         DATA_TYPE curValue = std::abs(augmentedMatrix(k, i));
+         if (curValue > pivotValue) {
+            pivotRow = k;
+            pivotValue = curValue;
+         }
+      }
+
+      // Swap if needed.
+      if (i != pivotRow)
+      {
+         for (U32 j = 0; j < 2 * size; j++)
+         {
+            DATA_TYPE temp = augmentedMatrix(i, j);
+            augmentedMatrix(i, j) = augmentedMatrix(pivotRow, j);
+            augmentedMatrix(pivotRow, j) = temp;
+         }
+      }
+
+      // Early out if pivot is 0, return identity matrix.
+      if (std::abs(augmentedMatrix(i, i)) < pivot_eps)
+      {
+         return *this;
+      }
+
+      DATA_TYPE pivotVal = static_cast<DATA_TYPE>(1.0) / augmentedMatrix(i, i);
+
+      // scale the pivot
+      for (U32 j = 0; j < 2 * size; j++)
+      {
+         augmentedMatrix(i, j) *= pivotVal;
+      }
+
+      // Eliminate the current column in all other rows
+      for (U32 k = 0; k < size; k++)
+      {
+         if (k != i)
+         {
+            DATA_TYPE factor = augmentedMatrix(k, i);
+            for (U32 j = 0; j < 2 * size; j++)
+            {
+               augmentedMatrix(k, j) -= factor * augmentedMatrix(i, j);
+            }
+         }
+      }
+   }
+
+   for (U32 i = 0; i < size; i++)
+   {
+      for (U32 j = 0; j < size; j++)
+      {
+         (*this)(i, j) = augmentedMatrix(i, j + size);
+      }
+   }
+#else
+   AssertFatal(rows == cols, "Can only perform inverse on square matrices.");
+   AssertFatal(rows >= 3 && cols >= 3, "Must be at least a 3x3 matrix");
+   DATA_TYPE det = determinant();
+
+   // Check if the determinant is non-zero
+   if (std::abs(det) < static_cast<DATA_TYPE>(1e-10)) {
+      this->identity(); // Return the identity matrix if the determinant is zero
+      return *this;
+   }
+
+   DATA_TYPE invDet = DATA_TYPE(1) / det;
+
+   Matrix<DATA_TYPE, rows, cols> temp;
+
+   // Calculate the inverse of the 3x3 upper-left submatrix using Cramer's rule
+   temp(0, 0) = ((*this)(1, 1) * (*this)(2, 2) - (*this)(1, 2) * (*this)(2, 1)) * invDet;
+   temp(0, 1) = ((*this)(2, 1) * (*this)(0, 2) - (*this)(2, 2) * (*this)(0, 1)) * invDet;
+   temp(0, 2) = ((*this)(0, 1) * (*this)(1, 2) - (*this)(0, 2) * (*this)(1, 1)) * invDet;
+
+   temp(1, 0) = ((*this)(1, 2) * (*this)(2, 0) - (*this)(1, 0) * (*this)(2, 2)) * invDet;
+   temp(1, 1) = ((*this)(2, 2) * (*this)(0, 0) - (*this)(2, 0) * (*this)(0, 2)) * invDet;
+   temp(1, 2) = ((*this)(0, 2) * (*this)(1, 0) - (*this)(0, 0) * (*this)(1, 2)) * invDet;
+
+   temp(2, 0) = ((*this)(1, 0) * (*this)(2, 1) - (*this)(1, 1) * (*this)(2, 0)) * invDet;
+   temp(2, 1) = ((*this)(2, 0) * (*this)(0, 1) - (*this)(2, 1) * (*this)(0, 0)) * invDet;
+   temp(2, 2) = ((*this)(0, 0) * (*this)(1, 1) - (*this)(0, 1) * (*this)(1, 0)) * invDet;
+
+   // Copy the 3x3 inverse back into this matrix
+   for (U32 i = 0; i < 3; ++i)
+   {
+      for (U32 j = 0; j < 3; ++j)
+      {
+         (*this)(i, j) = temp(i, j);
+      }
+   }
+
+#endif
+
+   Point3F pos = -this->getPosition();
+   mulV(pos);
+   this->setPosition(pos);
+
+   return (*this);
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline bool Matrix<DATA_TYPE, rows, cols>::fullInverse()
+{
+#if 1
+   // NOTE: Gauss-Jordan elimination is yielding unpredictable results due to precission handling and
+   // numbers near 0.0
+   AssertFatal(rows == cols, "Can only perform inverse on square matrices.");
+   const U32 size = rows;
+   const DATA_TYPE pivot_eps = static_cast<DATA_TYPE>(1e-20);  // Smaller epsilon to handle numerical precision
+
+   // Create augmented matrix [this | I]
+   Matrix<DATA_TYPE, size, rows + size> augmentedMatrix;
+
+   for (U32 i = 0; i < size; i++)
+   {
+      for (U32 j = 0; j < size; j++)
+      {
+         augmentedMatrix(i, j) = (*this)(i, j);
+         augmentedMatrix(i, j + size) = (i == j) ? static_cast<DATA_TYPE>(1) : static_cast<DATA_TYPE>(0);
+      }
+   }
+
+   // Apply gauss-joran elimination
+   for (U32 i = 0; i < size; i++)
+   {
+      U32 pivotRow = i;
+
+      DATA_TYPE pivotValue = std::abs(augmentedMatrix(i, i));
+
+      for (U32 k = i + 1; k < size; k++)
+      {
+         DATA_TYPE curValue = std::abs(augmentedMatrix(k, i));
+         if (curValue > pivotValue) {
+            pivotRow = k;
+            pivotValue = curValue;
+         }
+      }
+
+      // Swap if needed.
+      if (i != pivotRow)
+      {
+         for (U32 j = 0; j < 2 * size; j++)
+         {
+            DATA_TYPE temp = augmentedMatrix(i, j);
+            augmentedMatrix(i, j) = augmentedMatrix(pivotRow, j);
+            augmentedMatrix(pivotRow, j) = temp;
+         }
+      }
+
+      // Early out if pivot is 0, return identity matrix.
+      if (std::abs(augmentedMatrix(i, i)) < pivot_eps)
+      {
+         return false;
+      }
+
+      DATA_TYPE pivotVal = static_cast<DATA_TYPE>(1.0) / augmentedMatrix(i, i);
+
+      // scale the pivot
+      for (U32 j = 0; j < 2 * size; j++)
+      {
+         augmentedMatrix(i, j) *= pivotVal;
+      }
+
+      // Eliminate the current column in all other rows
+      for (U32 k = 0; k < size; k++)
+      {
+         if (k != i)
+         {
+            DATA_TYPE factor = augmentedMatrix(k, i);
+            for (U32 j = 0; j < 2 * size; j++)
+            {
+               augmentedMatrix(k, j) -= factor * augmentedMatrix(i, j);
+            }
+         }
+      }
+   }
+
+   for (U32 i = 0; i < size; i++)
+   {
+      for (U32 j = 0; j < size; j++)
+      {
+         (*this)(i, j) = augmentedMatrix(i, j + size);
+      }
+   }
+#else
+   AssertFatal(rows == cols, "Can only perform inverse on square matrices.");
+   AssertFatal(rows >= 4 && cols >= 4, "Can only perform fullInverse on minimum 4x4 matrix");
+
+   Point4F a, b, c, d;
+   getRow(0, &a);
+   getRow(1, &b);
+   getRow(2, &c);
+   getRow(3, &d);
+
+   F32 det = a.x * b.y * c.z * d.w - a.x * b.y * c.w * d.z - a.x * c.y * b.z * d.w + a.x * c.y * b.w * d.z + a.x * d.y * b.z * c.w - a.x * d.y * b.w * c.z
+      - b.x * a.y * c.z * d.w + b.x * a.y * c.w * d.z + b.x * c.y * a.z * d.w - b.x * c.y * a.w * d.z - b.x * d.y * a.z * c.w + b.x * d.y * a.w * c.z
+      + c.x * a.y * b.z * d.w - c.x * a.y * b.w * d.z - c.x * b.y * a.z * d.w + c.x * b.y * a.w * d.z + c.x * d.y * a.z * b.w - c.x * d.y * a.w * b.z
+      - d.x * a.y * b.z * c.w + d.x * a.y * b.w * c.z + d.x * b.y * a.z * c.w - d.x * b.y * a.w * c.z - d.x * c.y * a.z * b.w + d.x * c.y * a.w * b.z;
+
+   if (mFabs(det) < 0.00001f)
+      return false;
+
+   Point4F aa, bb, cc, dd;
+   aa.x = b.y * c.z * d.w - b.y * c.w * d.z - c.y * b.z * d.w + c.y * b.w * d.z + d.y * b.z * c.w - d.y * b.w * c.z;
+   aa.y = -a.y * c.z * d.w + a.y * c.w * d.z + c.y * a.z * d.w - c.y * a.w * d.z - d.y * a.z * c.w + d.y * a.w * c.z;
+   aa.z = a.y * b.z * d.w - a.y * b.w * d.z - b.y * a.z * d.w + b.y * a.w * d.z + d.y * a.z * b.w - d.y * a.w * b.z;
+   aa.w = -a.y * b.z * c.w + a.y * b.w * c.z + b.y * a.z * c.w - b.y * a.w * c.z - c.y * a.z * b.w + c.y * a.w * b.z;
+
+   bb.x = -b.x * c.z * d.w + b.x * c.w * d.z + c.x * b.z * d.w - c.x * b.w * d.z - d.x * b.z * c.w + d.x * b.w * c.z;
+   bb.y = a.x * c.z * d.w - a.x * c.w * d.z - c.x * a.z * d.w + c.x * a.w * d.z + d.x * a.z * c.w - d.x * a.w * c.z;
+   bb.z = -a.x * b.z * d.w + a.x * b.w * d.z + b.x * a.z * d.w - b.x * a.w * d.z - d.x * a.z * b.w + d.x * a.w * b.z;
+   bb.w = a.x * b.z * c.w - a.x * b.w * c.z - b.x * a.z * c.w + b.x * a.w * c.z + c.x * a.z * b.w - c.x * a.w * b.z;
+
+   cc.x = b.x * c.y * d.w - b.x * c.w * d.y - c.x * b.y * d.w + c.x * b.w * d.y + d.x * b.y * c.w - d.x * b.w * c.y;
+   cc.y = -a.x * c.y * d.w + a.x * c.w * d.y + c.x * a.y * d.w - c.x * a.w * d.y - d.x * a.y * c.w + d.x * a.w * c.y;
+   cc.z = a.x * b.y * d.w - a.x * b.w * d.y - b.x * a.y * d.w + b.x * a.w * d.y + d.x * a.y * b.w - d.x * a.w * b.y;
+   cc.w = -a.x * b.y * c.w + a.x * b.w * c.y + b.x * a.y * c.w - b.x * a.w * c.y - c.x * a.y * b.w + c.x * a.w * b.y;
+
+   dd.x = -b.x * c.y * d.z + b.x * c.z * d.y + c.x * b.y * d.z - c.x * b.z * d.y - d.x * b.y * c.z + d.x * b.z * c.y;
+   dd.y = a.x * c.y * d.z - a.x * c.z * d.y - c.x * a.y * d.z + c.x * a.z * d.y + d.x * a.y * c.z - d.x * a.z * c.y;
+   dd.z = -a.x * b.y * d.z + a.x * b.z * d.y + b.x * a.y * d.z - b.x * a.z * d.y - d.x * a.y * b.z + d.x * a.z * b.y;
+   dd.w = a.x * b.y * c.z - a.x * b.z * c.y - b.x * a.y * c.z + b.x * a.z * c.y + c.x * a.y * b.z - c.x * a.z * b.y;
+
+   setRow(0, aa);
+   setRow(1, bb);
+   setRow(2, cc);
+   setRow(3, dd);
+
+   mul(1.0f / det);
+#endif
+
+   return true;
+
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline void Matrix<DATA_TYPE, rows, cols>::invert()
+{
+   (*this) = inverse();
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::setCrossProduct(const Point3F& p)
+{
+   AssertFatal(rows == 4 && cols == 4, "Cross product only supported on 4x4 for now");
+
+   (*this)(0, 0) = 0;
+   (*this)(0, 1) = -p.z;
+   (*this)(0, 2) = p.y;
+   (*this)(0, 3) = 0;
+
+   (*this)(1, 0) = p.z;
+   (*this)(1, 1) = 0;
+   (*this)(1, 2) = -p.x;
+   (*this)(1, 3) = 0;
+
+   (*this)(2, 0) = -p.y;
+   (*this)(2, 1) = p.x;
+   (*this)(2, 2) = 0;
+   (*this)(2, 3) = 0;
+
+   (*this)(3, 0) = 0;
+   (*this)(3, 1) = 0;
+   (*this)(3, 2) = 0;
+   (*this)(3, 3) = 1;
+
+   return (*this);
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::setTensorProduct(const Point3F& p, const Point3F& q)
+{
+   AssertFatal(rows == 4 && cols == 4, "Tensor product only supported on 4x4 for now");
+
+   (*this)(0, 0) = p.x * q.x;
+   (*this)(0, 1) = p.x * q.y;
+   (*this)(0, 2) = p.x * q.z;
+   (*this)(0, 3) = 0;
+
+   (*this)(1, 0) = p.y * q.x;
+   (*this)(1, 1) = p.y * q.y;
+   (*this)(1, 2) = p.y * q.z;
+   (*this)(1, 3) = 0;
+
+   (*this)(2, 0) = p.z * q.x;
+   (*this)(2, 1) = p.z * q.y;
+   (*this)(2, 2) = p.z * q.z;
+   (*this)(2, 3) = 0;
+
+   (*this)(3, 0) = 0;
+   (*this)(3, 1) = 0;
+   (*this)(3, 2) = 0;
+   (*this)(3, 3) = 1;
+
+   return (*this);
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline void Matrix<DATA_TYPE, rows, cols>::mul(Box3F& box) const
+{
+   AssertFatal(rows == 4 && cols == 4, "Multiplying Box3F with matrix requires 4x4");
+
+   // Extract the min and max extents
+   const Point3F& originalMin = box.minExtents;
+   const Point3F& originalMax = box.maxExtents;
+
+   // Array to store transformed corners
+   Point3F transformedCorners[8];
+
+   // Compute all 8 corners of the box
+   Point3F corners[8] = {
+       {originalMin.x, originalMin.y, originalMin.z},
+       {originalMax.x, originalMin.y, originalMin.z},
+       {originalMin.x, originalMax.y, originalMin.z},
+       {originalMax.x, originalMax.y, originalMin.z},
+       {originalMin.x, originalMin.y, originalMax.z},
+       {originalMax.x, originalMin.y, originalMax.z},
+       {originalMin.x, originalMax.y, originalMax.z},
+       {originalMax.x, originalMax.y, originalMax.z}
+   };
+
+   // Transform each corner
+   for (U32 i = 0; i < 8; ++i)
+   {
+      const Point3F& corner = corners[i];
+      transformedCorners[i].x = (*this)(0, 0) * corner.x + (*this)(0, 1) * corner.y + (*this)(0, 2) * corner.z + (*this)(0, 3);
+      transformedCorners[i].y = (*this)(1, 0) * corner.x + (*this)(1, 1) * corner.y + (*this)(1, 2) * corner.z + (*this)(1, 3);
+      transformedCorners[i].z = (*this)(2, 0) * corner.x + (*this)(2, 1) * corner.y + (*this)(2, 2) * corner.z + (*this)(2, 3);
+   }
+
+   // Initialize min and max extents to the transformed values
+   Point3F newMin = transformedCorners[0];
+   Point3F newMax = transformedCorners[0];
+
+   // Compute the new min and max extents from the transformed corners
+   for (U32 i = 1; i < 8; ++i)
+   {
+      const Point3F& corner = transformedCorners[i];
+      if (corner.x < newMin.x) newMin.x = corner.x;
+      if (corner.y < newMin.y) newMin.y = corner.y;
+      if (corner.z < newMin.z) newMin.z = corner.z;
+
+      if (corner.x > newMax.x) newMax.x = corner.x;
+      if (corner.y > newMax.y) newMax.y = corner.y;
+      if (corner.z > newMax.z) newMax.z = corner.z;
+   }
+
+   // Update the box with the new min and max extents
+   box.minExtents = newMin;
+   box.maxExtents = newMax;
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline bool Matrix<DATA_TYPE, rows, cols>::isAffine() const
+{
+   if ((*this)(3, 3) != 1.0f)
+   {
+      return false;
+   }
+
+   for (U32 col = 0; col < cols - 1; ++col)
+   {
+      if ((*this)(3, col) != 0.0f)
+      {
+         return false;
+      }
+   }
+
+   Point3F one, two, three;
+   getColumn(0, &one);
+   getColumn(1, &two);
+   getColumn(2, &three);
+
+   // check columns
+   {
+      if (mDot(one, two) > 0.0001f ||
+         mDot(one, three) > 0.0001f ||
+         mDot(two, three) > 0.0001f)
+         return false;
+
+      if (mFabs(1.0f - one.lenSquared()) > 0.0001f ||
+         mFabs(1.0f - two.lenSquared()) > 0.0001f ||
+         mFabs(1.0f - three.lenSquared()) > 0.0001f)
+         return false;
+   }
+
+   getRow(0, &one);
+   getRow(1, &two);
+   getRow(2, &three);
+   // check rows
+   {
+      if (mDot(one, two) > 0.0001f ||
+         mDot(one, three) > 0.0001f ||
+         mDot(two, three) > 0.0001f)
+         return false;
+
+      if (mFabs(1.0f - one.lenSquared()) > 0.0001f ||
+         mFabs(1.0f - two.lenSquared()) > 0.0001f ||
+         mFabs(1.0f - three.lenSquared()) > 0.0001f)
+         return false;
+   }
+
+   return true;
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::affineInverse()
+{
+   AssertFatal(rows >= 4 && cols >= 4, "affineInverse requires at least 4x4");
+   Matrix<DATA_TYPE, rows, cols> temp = *this;
+
+   // Transpose rotation part
+   (*this)(0, 1) = temp(1, 0);
+   (*this)(0, 2) = temp(2, 0);
+   (*this)(1, 0) = temp(0, 1);
+   (*this)(1, 2) = temp(2, 1);
+   (*this)(2, 0) = temp(0, 2);
+   (*this)(2, 1) = temp(1, 2);
+
+   // Adjust translation part
+   (*this)(0, 3) = -(temp(0, 0) * temp(0, 3) + temp(1, 0) * temp(1, 3) + temp(2, 0) * temp(2, 3));
+   (*this)(1, 3) = -(temp(0, 1) * temp(0, 3) + temp(1, 1) * temp(1, 3) + temp(2, 1) * temp(2, 3));
+   (*this)(2, 3) = -(temp(0, 2) * temp(0, 3) + temp(1, 2) * temp(1, 3) + temp(2, 2) * temp(2, 3));
+
+   return *this;
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline EulerF Matrix<DATA_TYPE, rows, cols>::toEuler() const
+{
+   AssertFatal(rows >= 3 && cols >= 3, "Euler rotations require at least a 3x3 matrix.");
+
+   // like all others assume float for now.
+   EulerF r;
+
+   r.x = mAsin(mClampF((*this)(1,2), -1.0, 1.0));
+   if (mCos(r.x) != 0.0f)
+   {
+      r.y = mAtan2(-(*this)(0, 2), (*this)(2, 2)); // yaw
+      r.z = mAtan2(-(*this)(1, 0), (*this)(1, 1)); // roll
+   }
+   else
+   {
+      r.y = 0.0f;
+      r.z = mAtan2((*this)(0, 1), (*this)(0, 0)); // this rolls when pitch is +90 degrees
+   }
+
+   return r;
+}
+
+template<typename DATA_TYPE, U32 rows, U32 cols>
+inline void Matrix<DATA_TYPE, rows, cols>::dumpMatrix(const char* caption) const
+{
+   U32 size = (caption == NULL) ? 0 : dStrlen(caption);
+   FrameTemp<char> spacer(size + 1);
+   char* spacerRef = spacer;
+
+   // is_floating_point should return true for floats and doubles.
+   const char* formatSpec = std::is_floating_point_v<DATA_TYPE> ? " %-8.4f" : " %d";
+
+   dMemset(spacerRef, ' ', size);
+   // null terminate.
+   spacerRef[size] = '\0';
+
+   /*Con::printf("%s = | %-8.4f %-8.4f %-8.4f %-8.4f |", caption, m[idx(0, 0)], m[idx(0, 1)], m[idx(0, 2)], m[idx(0, 3)]);
+   Con::printf("%s   | %-8.4f %-8.4f %-8.4f %-8.4f |", spacerRef, m[idx(1, 0)], m[idx(1, 1)], m[idx(1, 2)], m[idx(1, 3)]);
+   Con::printf("%s   | %-8.4f %-8.4f %-8.4f %-8.4f |", spacerRef, m[idx(2, 0)], m[idx(2, 1)], m[idx(2, 2)], m[idx(2, 3)]);
+   Con::printf("%s   | %-8.4f %-8.4f %-8.4f %-8.4f |", spacerRef, m[idx(3, 0)], m[idx(3, 1)], m[idx(3, 2)], m[idx(3, 3)]);*/
+
+   StringBuilder str;
+   str.format("%s = |", caption);
+   for (U32 i = 0; i < rows; i++)
+   {
+      if (i > 0)
+      {
+         str.append(spacerRef);
+      }
+
+      for (U32 j = 0; j < cols; j++)
+      {
+         str.format(formatSpec, (*this)(i, j));
+      }
+      str.append(" |\n");
+   }
+
+   Con::printf("%s", str.end().c_str());
+}
+
+//------------------------------------
+// Non-member methods
+//------------------------------------
+
+inline void mTransformPlane(
+   const MatrixF& mat,
+   const Point3F& scale,
+   const PlaneF& plane,
+   PlaneF* result
+) {
+   // Create the inverse scale matrix
+   MatrixF invScale(true);
+   invScale(0, 0) = 1.0f / scale.x;
+   invScale(1, 1) = 1.0f / scale.y;
+   invScale(2, 2) = 1.0f / scale.z;
+
+   const Point3F shear(mat(0, 3), mat(1, 3), mat(2, 3));
+
+   const Point3F row0 = mat.getRow3F(0);
+   const Point3F row1 = mat.getRow3F(1);
+   const Point3F row2 = mat.getRow3F(2);
+
+   const F32 A = -mDot(row0, shear);
+   const F32 B = -mDot(row1, shear);
+   const F32 C = -mDot(row2, shear);
+
+   // Compute the inverse transpose of the matrix
+   MatrixF invTrMatrix(true);
+   invTrMatrix(0, 0) = mat(0, 0);
+   invTrMatrix(0, 1) = mat(0, 1);
+   invTrMatrix(0, 2) = mat(0, 2);
+   invTrMatrix(1, 0) = mat(1, 0);
+   invTrMatrix(1, 1) = mat(1, 1);
+   invTrMatrix(1, 2) = mat(1, 2);
+   invTrMatrix(2, 0) = mat(2, 0);
+   invTrMatrix(2, 1) = mat(2, 1);
+   invTrMatrix(2, 2) = mat(2, 2);
+   invTrMatrix(3, 0) = A;
+   invTrMatrix(3, 1) = B;
+   invTrMatrix(3, 2) = C;
+   invTrMatrix.mul(invScale);
+
+   // Transform the plane normal
+   Point3F norm(plane.x, plane.y, plane.z);
+   invTrMatrix.mulP(norm);
+   norm.normalize();
+
+   // Transform the plane point
+   Point3F point = norm * -plane.d;
+   MatrixF temp = mat;
+   point.x *= scale.x;
+   point.y *= scale.y;
+   point.z *= scale.z;
+   temp.mulP(point);
+
+   // Recompute the plane distance
+   PlaneF resultPlane(point, norm);
+   result->x = resultPlane.x;
+   result->y = resultPlane.y;
+   result->z = resultPlane.z;
+   result->d = resultPlane.d;
+}
+
+//--------------------------------------------
+// INLINE FUNCTIONS END
+//--------------------------------------------
+
+typedef Matrix<F32, 4, 4> MatrixF;
+
+class MatrixTemplateExport
+{
+public:
+   template <typename T, U32 rows, U32 cols>
+   static EngineFieldTable::Field getMatrixField();
+};
+
+template<typename T, U32 rows, U32 cols>
+inline EngineFieldTable::Field MatrixTemplateExport::getMatrixField()
+{
+   typedef Matrix<T, rows, cols> ThisType;
+   return _FIELD_AS(T, data, data, rows * cols, "");
+}
+
+#endif // !USE_TEMPLATE_MATRIX
+
 #endif //_MMATRIX_H_

Engine/source/math/mOrientedBox.h

@@ -32,7 +32,12 @@
 #endif
 
 
+#ifndef USE_TEMPLATE_MATRIX
 class MatrixF;
+#else
+template<typename DATA_TYPE, U32 rows, U32 cols> class Matrix;
+typedef Matrix<F32, 4, 4> MatrixF;
+#endif
 class Box3F;
 
 

Engine/source/math/mQuat.h

@@ -27,7 +27,12 @@
 #include "math/mPoint3.h"
 #endif
 
+#ifndef USE_TEMPLATE_MATRIX
 class MatrixF;
+#else
+template<typename DATA_TYPE, U32 rows, U32 cols> class Matrix;
+typedef Matrix<F32, 4, 4> MatrixF;
+#endif
 class AngAxisF;
 
 //----------------------------------------------------------------------------

Engine/source/math/mathTypes.cpp

@@ -101,6 +101,7 @@ IMPLEMENT_STRUCT( RectF,
    FIELD( extent, extent, 1, "The width and height of the Rect.")
 
 END_IMPLEMENT_STRUCT;
+#ifndef USE_TEMPLATE_MATRIX
 IMPLEMENT_STRUCT( MatrixF,
    MatrixF, MathTypes,
    "" )
@@ -108,6 +109,15 @@ IMPLEMENT_STRUCT( MatrixF,
    MatrixFEngineExport::getMatrixField(),
 
 END_IMPLEMENT_STRUCT;
+#else
+IMPLEMENT_STRUCT(MatrixF,
+MatrixF, MathTypes,
+"")
+
+MatrixTemplateExport::getMatrixField<F32, 4, 4>(),
+
+END_IMPLEMENT_STRUCT;
+#endif
 IMPLEMENT_STRUCT( AngAxisF,
    AngAxisF, MathTypes,
    "" )

Engine/source/math/mathTypes.h

@@ -38,7 +38,12 @@ class Point3F;
 class Point4F;
 class RectI;
 class RectF;
+#ifndef USE_TEMPLATE_MATRIX
 class MatrixF;
+#else
+template<typename DATA_TYPE, U32 rows, U32 cols> class Matrix;
+typedef Matrix<F32, 4, 4> MatrixF;
+#endif
 class Box3F;
 class EaseF;
 class AngAxisF;

Engine/source/math/test/mMatrixTest.cpp

@@ -1,93 +0,0 @@
-//-----------------------------------------------------------------------------
-// Copyright (c) 2014 GarageGames, LLC
-//
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to
-// deal in the Software without restriction, including without limitation the
-// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
-// sell copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-//
-// The above copyright notice and this permission notice shall be included in
-// all copies or substantial portions of the Software.
-//
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
-// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
-// IN THE SOFTWARE.
-//-----------------------------------------------------------------------------
-
-#ifdef TORQUE_TESTS_ENABLED
-#include "testing/unitTesting.h"
-#include "platform/platform.h"
-#include "math/mMatrix.h"
-#include "math/mRandom.h"
-
-extern void default_matF_x_matF_C(const F32 *a, const F32 *b, F32 *mresult);
-extern void mInstallLibrary_ASM();
-
-// If we're x86 and not Mac, then include these. There's probably a better way to do this.
-#if defined(WIN32) && defined(TORQUE_CPU_X86)
-void Athlon_MatrixF_x_MatrixF(const F32 *matA, const F32 *matB, F32 *result);
-void SSE_MatrixF_x_MatrixF(const F32 *matA, const F32 *matB, F32 *result);
-#endif
-
-#if defined( __VEC__ )
-extern void vec_MatrixF_x_MatrixF(const F32 *matA, const F32 *matB, F32 *result);
-#endif
-
-TEST(MatrixF, MultiplyImplmentations)
-{
-   F32 m1[16], m2[16], mrC[16];
-
-   // I am not positive that the best way to do this is to use random numbers
-   // but I think that using some kind of standard matrix may not always catch
-   // all problems.
-   for (S32 i = 0; i < 16; i++)
-   {
-      m1[i] = gRandGen.randF();
-      m2[i] = gRandGen.randF();
-   }
-
-   // C will be the baseline
-   default_matF_x_matF_C(m1, m2, mrC);
-
-#if defined(WIN32) && defined(TORQUE_CPU_X86)
-   // Check the CPU info
-   U32 cpuProperties = Platform::SystemInfo.processor.properties;
-   bool same;
-
-   // Test SSE if it is available
-   F32 mrSSE[16];
-   if (cpuProperties & CPU_PROP_SSE)
-   {
-      SSE_MatrixF_x_MatrixF(m1, m2, mrSSE);
-
-      same = true;
-      for (S32 i = 0; i < 16; i++)
-         same &= mIsEqual(mrC[i], mrSSE[i]);
-
-      EXPECT_TRUE(same) << "Matrix multiplication verification failed. (C vs. SSE)";
-   }
-
-   same = true;
-#endif
-
-   // If Altivec exists, test it!
-#if defined(__VEC__)
-   bool same = false;
-   F32 mrVEC[16];
-
-   vec_MatrixF_x_MatrixF(m1, m2, mrVEC);
-
-   for (S32 i = 0; i < 16; i++)
-      same &= isEqual(mrC[i], mrVEC[i]);
-
-   EXPECT_TRUE(same) << "Matrix multiplication verification failed. (C vs. Altivec)";
-#endif
-}
-
-#endif

Engine/source/math/util/matrixSetDelegateMethods.h

@@ -22,7 +22,7 @@
 #ifndef _MATRIXSETDELEGATES_H_
 #define _MATRIXSETDELEGATES_H_
 
-   // Access to the direct value
+#ifndef USE_TEMPLATE_MATRIX
 #define MATRIX_SET_GET_VALUE_FN(xfm) _transform_##xfm
 #define MATRIX_SET_GET_VALUE(xfm) inline const MatrixF &MATRIX_SET_GET_VALUE_FN(xfm)() { return mTransform[xfm]; }
 #define MATRIX_SET_BIND_VALUE(xfm) mEvalDelegate[xfm].bind(this, &MatrixSet::MATRIX_SET_GET_VALUE_FN(xfm))
@@ -44,5 +44,28 @@
       return mTransform[matC]; \
    }
 
+#else
+   // Access to the direct value
+#define MATRIX_SET_GET_VALUE_FN(xfm) _transform_##xfm
+#define MATRIX_SET_GET_VALUE(xfm) inline const MatrixF &MATRIX_SET_GET_VALUE_FN(xfm)() { return mTransform[xfm]; }
+#define MATRIX_SET_BIND_VALUE(xfm) mEvalDelegate[xfm].bind(this, &MatrixSet::MATRIX_SET_GET_VALUE_FN(xfm))
+
+#define MATRIX_SET_IS_INVERSE_OF_FN(inv_xfm, src_xfm) _##inv_xfm##_is_inverse_of_##src_xfm
+#define MATRIX_SET_IS_INVERSE_OF(inv_xfm, src_xfm) inline const MatrixF &MATRIX_SET_IS_INVERSE_OF_FN(inv_xfm, src_xfm)() \
+   { \
+      mEvalDelegate[src_xfm]().invertTo(&mTransform[inv_xfm]); \
+      MATRIX_SET_BIND_VALUE(inv_xfm); \
+      return mTransform[inv_xfm]; \
+   }
+
+
+#define MATRIX_SET_MULT_ASSIGN_FN(matA, matB, matC) _##matC##_is_##matA##_x_##matB
+#define MATRIX_SET_MULT_ASSIGN(matA, matB, matC) inline const MatrixF &MATRIX_SET_MULT_ASSIGN_FN(matA, matB, matC)() \
+   { \
+      mTransform[matC].mul(mEvalDelegate[matA](),mEvalDelegate[matB]()); \
+      MATRIX_SET_BIND_VALUE(matC); \
+      return mTransform[matC]; \
+   }
 
+#endif
 #endif // _MATRIXSETDELEGATES_H_

Engine/source/scene/sgUtil.h

@@ -32,7 +32,12 @@
 
 class Frustum;
 class RectI;
+#ifndef USE_TEMPLATE_MATRIX
 class MatrixF;
+#else
+template<typename DATA_TYPE, U32 rows, U32 cols> class Matrix;
+typedef Matrix<F32, 4, 4> MatrixF;
+#endif
 class PlaneF;
 
 struct SGWinding

Engine/source/testing/mathMatrixTest.cpp

@@ -0,0 +1,1014 @@
+#include "testing/unitTesting.h"
+
+#include "platform/platform.h"
+#include "console/simBase.h"
+#include "console/consoleTypes.h"
+#include "console/scriptObjects.h"
+#include "console/simBase.h"
+#include "console/engineAPI.h"
+#include "math/mMath.h"
+#include "math/util/frustum.h"
+#include "math/mathUtils.h"
+
+TEST(MatrixTest, TestIdentityInit)
+{
+   MatrixF test(true);
+
+   EXPECT_NEAR(test(0, 0), 1.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 1), 1.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 2), 1.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestIdentitySet)
+{
+   MatrixF test(false);
+   test.identity();
+
+   EXPECT_NEAR(test(0, 0), 1.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 1), 1.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 2), 1.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestIsIdentity)
+{
+   MatrixF test(true);
+   EXPECT_TRUE(test.isIdentity());
+}
+
+TEST(MatrixTest, TestEulerInit)
+{
+   MatrixF test(EulerF(1.0f, 0.0f, 1.0f));
+
+   EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON);   EXPECT_NEAR(test(0, 1), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(0, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), -0.4546f, POINT_EPSILON);  EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON);  EXPECT_NEAR(test(1, 2), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 0.7081f, POINT_EPSILON);   EXPECT_NEAR(test(2, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);      EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON);
+}
+
+
+TEST(MatrixTest, TestEulerSet)
+{
+   MatrixF test(true);
+
+   test.set(EulerF(1.0f, 0.0f, 1.0f));
+
+   EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON);   EXPECT_NEAR(test(0, 1), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(0, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), -0.4546f, POINT_EPSILON);  EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON);  EXPECT_NEAR(test(1, 2), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 0.7081f, POINT_EPSILON);   EXPECT_NEAR(test(2, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);      EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestToEuler)
+{
+   MatrixF test(true);
+
+   test.set(EulerF(1.0f, 0.0f, 1.0f));
+
+   EulerF euler = test.toEuler();
+
+   EXPECT_NEAR(euler.x, 1.0f, POINT_EPSILON); EXPECT_NEAR(euler.y, 0.0f, POINT_EPSILON); EXPECT_NEAR(euler.z, 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestEulerPointInit)
+{
+   MatrixF test(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f));
+
+   EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON);   EXPECT_NEAR(test(0, 1), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(0, 3), 5.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), -0.4546f, POINT_EPSILON);  EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON);  EXPECT_NEAR(test(1, 2), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 0.7081f, POINT_EPSILON);   EXPECT_NEAR(test(2, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 1.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);      EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON);
+}
+
+
+TEST(MatrixTest, TestEulerPointSet)
+{
+   MatrixF test(true);
+
+   test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f));
+
+   EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON);   EXPECT_NEAR(test(0, 1), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(0, 3), 5.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), -0.4546f, POINT_EPSILON);  EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON);  EXPECT_NEAR(test(1, 2), 0.8415f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 0.7081f, POINT_EPSILON);   EXPECT_NEAR(test(2, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 1.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);      EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestGetColumn4)
+{
+   MatrixF test(true);
+
+   test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f));
+
+   Point4F column;
+
+   test.getColumn(0, &column);
+
+   EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(3, 0), POINT_EPSILON);
+
+   test.getColumn(1, &column);
+
+   EXPECT_NEAR(column.x, test(0, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(3, 1), POINT_EPSILON);
+
+   test.getColumn(2, &column);
+
+   EXPECT_NEAR(column.x, test(0, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(3, 2), POINT_EPSILON);
+
+   test.getColumn(3, &column);
+
+   EXPECT_NEAR(column.x, test(0, 3), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 3), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 3), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(3, 3), POINT_EPSILON);
+
+}
+
+TEST(MatrixTest, TestGetRow4)
+{
+   MatrixF test(true);
+
+   test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f));
+
+   Point4F column;
+
+   test.getRow(0, &column);
+
+   EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(0, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(0, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(0, 3), POINT_EPSILON);
+
+   test.getRow(1, &column);
+
+   EXPECT_NEAR(column.x, test(1, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(1, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(1, 3), POINT_EPSILON);
+
+   test.getRow(2, &column);
+
+   EXPECT_NEAR(column.x, test(2, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(2, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(2, 3), POINT_EPSILON);
+
+   test.getRow(3, &column);
+
+   EXPECT_NEAR(column.x, test(3, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(3, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(3, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(3, 3), POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestGetColumn3)
+{
+   MatrixF test(true);
+
+   test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f));
+
+   Point3F column;
+
+   test.getColumn(0, &column);
+
+   EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 0), POINT_EPSILON);
+
+   test.getColumn(1, &column);
+
+   EXPECT_NEAR(column.x, test(0, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 1), POINT_EPSILON);
+
+   test.getColumn(2, &column);
+
+   EXPECT_NEAR(column.x, test(0, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON);
+
+   test.getColumn(3, &column);
+
+   EXPECT_NEAR(column.x, test(0, 3), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 3), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 3), POINT_EPSILON);
+
+}
+
+TEST(MatrixTest, TestGetRow3)
+{
+   MatrixF test(true);
+
+   test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f));
+
+   Point3F column;
+
+   test.getRow(0, &column);
+
+   EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(0, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(0, 2), POINT_EPSILON);
+
+   test.getRow(1, &column);
+
+   EXPECT_NEAR(column.x, test(1, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(1, 2), POINT_EPSILON);
+
+   test.getRow(2, &column);
+
+   EXPECT_NEAR(column.x, test(2, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(2, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON);
+
+   test.getRow(3, &column);
+
+   EXPECT_NEAR(column.x, test(3, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(3, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(3, 2), POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestGetColumn4F)
+{
+   MatrixF test(true);
+
+   test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f));
+
+   Point4F column;
+
+   column = test.getColumn4F(0);
+
+   EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(3, 0), POINT_EPSILON);
+
+   column = test.getColumn4F(1);
+
+   EXPECT_NEAR(column.x, test(0, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(3, 1), POINT_EPSILON);
+
+   column = test.getColumn4F(2);
+
+   EXPECT_NEAR(column.x, test(0, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(3, 2), POINT_EPSILON);
+
+   column = test.getColumn4F(3);
+
+   EXPECT_NEAR(column.x, test(0, 3), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 3), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 3), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(3, 3), POINT_EPSILON);
+
+}
+
+TEST(MatrixTest, TestGetRow4F)
+{
+   MatrixF test(true);
+
+   test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f));
+
+   Point4F column;
+
+   column = test.getRow4F(0);
+
+   EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(0, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(0, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(0, 3), POINT_EPSILON);
+
+   column = test.getRow4F(1);
+
+   EXPECT_NEAR(column.x, test(1, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(1, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(1, 3), POINT_EPSILON);
+
+   column = test.getRow4F(2);
+
+   EXPECT_NEAR(column.x, test(2, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(2, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(2, 3), POINT_EPSILON);
+
+   column = test.getRow4F(3);
+
+   EXPECT_NEAR(column.x, test(3, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(3, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(3, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(3, 3), POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestGetColumn3F)
+{
+   MatrixF test(true);
+
+   test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f));
+
+   Point3F column;
+
+   column = test.getColumn3F(0);
+
+   EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 0), POINT_EPSILON);
+
+   column = test.getColumn3F(1);
+
+   EXPECT_NEAR(column.x, test(0, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 1), POINT_EPSILON);
+
+   column = test.getColumn3F(2);
+
+   EXPECT_NEAR(column.x, test(0, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON);
+
+   column = test.getColumn3F(3);
+
+   EXPECT_NEAR(column.x, test(0, 3), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 3), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 3), POINT_EPSILON);
+
+}
+
+TEST(MatrixTest, TestGetRow3F)
+{
+   MatrixF test(true);
+
+   test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f));
+
+   Point3F column;
+
+   column = test.getRow3F(0);
+
+   EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(0, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(0, 2), POINT_EPSILON);
+
+   column = test.getRow3F(1);
+
+   EXPECT_NEAR(column.x, test(1, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(1, 2), POINT_EPSILON);
+
+   column = test.getRow3F(2);
+
+   EXPECT_NEAR(column.x, test(2, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(2, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON);
+
+   column = test.getRow3F(3);
+
+   EXPECT_NEAR(column.x, test(3, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(3, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(3, 2), POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestSetColumn4)
+{
+   MatrixF test(true);
+
+   test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f));
+
+   Point4F column(1.0f, 1.0f, 1.0f, 1.0f);
+
+   test.setColumn(0, column);
+
+   EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(3, 0), POINT_EPSILON);
+
+   test.setColumn(1, column);
+
+   EXPECT_NEAR(column.x, test(0, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(3, 1), POINT_EPSILON);
+
+   test.setColumn(2, column);
+
+   EXPECT_NEAR(column.x, test(0, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(3, 2), POINT_EPSILON);
+
+   test.setColumn(3, column);
+
+   EXPECT_NEAR(column.x, test(0, 3), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 3), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 3), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(3, 3), POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestSetRow4)
+{
+   MatrixF test(true);
+
+   test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f));
+
+   Point4F column(1.0f, 1.0f, 1.0f, 1.0f);
+
+   test.setRow(0, column);
+
+   EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(0, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(0, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(0, 3), POINT_EPSILON);
+
+   test.setRow(1, column);
+
+   EXPECT_NEAR(column.x, test(1, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(1, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(1, 3), POINT_EPSILON);
+
+   test.setRow(2, column);
+
+   EXPECT_NEAR(column.x, test(2, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(2, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(2, 3), POINT_EPSILON);
+
+   test.setRow(3, column);
+
+   EXPECT_NEAR(column.x, test(3, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(3, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(3, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.w, test(3, 3), POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestSetColumn3)
+{
+   MatrixF test(true);
+
+   test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f));
+
+   Point3F column(1.0f, 1.0f, 1.0f);
+
+   test.setColumn(0, column);
+
+   EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 0), POINT_EPSILON);
+
+   test.setColumn(1, column);
+
+   EXPECT_NEAR(column.x, test(0, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 1), POINT_EPSILON);
+
+   test.setColumn(2, column);
+
+   EXPECT_NEAR(column.x, test(0, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 2), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON);
+
+   test.setColumn(3, column);
+
+   EXPECT_NEAR(column.x, test(0, 3), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 3), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 3), POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestSetRow3)
+{
+   MatrixF test(true);
+
+   test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f));
+
+   Point3F column(1.0f, 1.0f, 1.0f);
+
+   test.setRow(0, column);
+
+   EXPECT_NEAR(column.x, test(0, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(0, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(0, 2), POINT_EPSILON);
+
+   test.setRow(1, column);
+
+   EXPECT_NEAR(column.x, test(1, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(1, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(1, 2), POINT_EPSILON);
+
+   test.setRow(2, column);
+
+   EXPECT_NEAR(column.x, test(2, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(2, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(2, 2), POINT_EPSILON);
+
+   test.setRow(3, column);
+
+   EXPECT_NEAR(column.x, test(3, 0), POINT_EPSILON);
+   EXPECT_NEAR(column.y, test(3, 1), POINT_EPSILON);
+   EXPECT_NEAR(column.z, test(3, 2), POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestDisplace)
+{
+   MatrixF test(true);
+
+   test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f));
+
+   Point3F column(1.0f, 1.0f, 1.0f);
+
+   test.displace(column);
+
+   EXPECT_NEAR(test(0, 3), 6.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 3), 1.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 3), 2.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestGetVectorFunctions)
+{
+   MatrixF test(true);
+
+   test.set(EulerF(1.0f, 0.0f, 1.0f), Point3F(5.0f, 0.0f, 1.0f));
+
+   VectorF vector;
+
+   vector = test.getRightVector();
+
+   EXPECT_NEAR(vector.x, test(0, 0), POINT_EPSILON);
+   EXPECT_NEAR(vector.y, test(1, 0), POINT_EPSILON);
+   EXPECT_NEAR(vector.z, test(2, 0), POINT_EPSILON);
+
+   vector = test.getForwardVector();
+
+   EXPECT_NEAR(vector.x, test(0, 1), POINT_EPSILON);
+   EXPECT_NEAR(vector.y, test(1, 1), POINT_EPSILON);
+   EXPECT_NEAR(vector.z, test(2, 1), POINT_EPSILON);
+
+   vector = test.getUpVector();
+
+   EXPECT_NEAR(vector.x, test(0, 2), POINT_EPSILON);
+   EXPECT_NEAR(vector.y, test(1, 2), POINT_EPSILON);
+   EXPECT_NEAR(vector.z, test(2, 2), POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestSetCrossProduct)
+{
+   MatrixF test(true);
+
+   test.setCrossProduct(Point3F(5.0f, 0.0f, 1.0f));
+
+   EXPECT_NEAR(test(0, 0), 0.0f, POINT_EPSILON);  EXPECT_NEAR(test(0, 1), -1.0f, POINT_EPSILON);  EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON);  EXPECT_NEAR(test(0, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), 1.0f, POINT_EPSILON);  EXPECT_NEAR(test(1, 1), 0.0, POINT_EPSILON);    EXPECT_NEAR(test(1, 2), -5.0f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 0.0f, POINT_EPSILON);  EXPECT_NEAR(test(2, 1), 5.0f, POINT_EPSILON);   EXPECT_NEAR(test(2, 2), 0.0f, POINT_EPSILON);  EXPECT_NEAR(test(2, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);  EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON);   EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);  EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestSetTensorProduct)
+{
+   MatrixF test(true);
+
+   test.setTensorProduct(Point3F(5.0f, 2.0f, 1.0f), Point3F(5.0f, 2.0f, 1.0f));
+
+   EXPECT_NEAR(test(0, 0), 25.0f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), 10.0f, POINT_EPSILON);  EXPECT_NEAR(test(0, 2), 5.0f, POINT_EPSILON);  EXPECT_NEAR(test(0, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), 10.0f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 4.0, POINT_EPSILON);    EXPECT_NEAR(test(1, 2), 2.0f, POINT_EPSILON);  EXPECT_NEAR(test(1, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 5.0f, POINT_EPSILON);  EXPECT_NEAR(test(2, 1), 2.0f, POINT_EPSILON);   EXPECT_NEAR(test(2, 2), 1.0f, POINT_EPSILON);  EXPECT_NEAR(test(2, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);  EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON);   EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);  EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestMulFunction)
+{
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test.mul(test2);
+
+   EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test(0, 1), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(0, 3), 5.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON);  EXPECT_NEAR(test(1, 2), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(1, 3), 2.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 0.7081f, POINT_EPSILON);  EXPECT_NEAR(test(2, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test(2, 3), 1.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestMulOperator)
+{
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test = test * test2;
+
+   EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test(0, 1), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(0, 3), 5.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON);  EXPECT_NEAR(test(1, 2), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(1, 3), 2.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 0.7081f, POINT_EPSILON);  EXPECT_NEAR(test(2, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test(2, 3), 1.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestMulLFunction)
+{
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test.mulL(test2);
+
+   EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test(0, 1), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(0, 3), 4.3845f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON);  EXPECT_NEAR(test(1, 2), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(1, 3), -0.8479f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 0.7081f, POINT_EPSILON);  EXPECT_NEAR(test(2, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test(2, 3), 3.1714, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestMulArgMatrixFunction)
+{
+   MatrixF testResult(true);
+
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   testResult.mul(test2, test);
+
+   EXPECT_NEAR(testResult(0, 0), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(testResult(0, 1), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(testResult(0, 2), 0.0f, POINT_EPSILON);     EXPECT_NEAR(testResult(0, 3), 4.3845f, POINT_EPSILON);
+   EXPECT_NEAR(testResult(1, 0), -0.4546f, POINT_EPSILON); EXPECT_NEAR(testResult(1, 1), 0.2919f, POINT_EPSILON);  EXPECT_NEAR(testResult(1, 2), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(testResult(1, 3), -0.8479f, POINT_EPSILON);
+   EXPECT_NEAR(testResult(2, 0), 0.7081f, POINT_EPSILON);  EXPECT_NEAR(testResult(2, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(testResult(2, 2), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(testResult(2, 3), 3.1714, POINT_EPSILON);
+   EXPECT_NEAR(testResult(3, 0), 0.0f, POINT_EPSILON);     EXPECT_NEAR(testResult(3, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(testResult(3, 2), 0.0f, POINT_EPSILON);     EXPECT_NEAR(testResult(3, 3), 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestMulArgMultipleRotationMatrix)
+{
+   MatrixF testResult(true);
+
+   MatrixF xRot(EulerF(-1.54f, 0.0f, 0.0f));
+   MatrixF zRot(EulerF(0.0f, 0.0f, -1.57f));
+
+   testResult.mul(zRot, xRot);
+
+   EXPECT_NEAR(testResult(0, 0), 0.0008f, POINT_EPSILON); EXPECT_NEAR(testResult(0, 1), -0.0308f, POINT_EPSILON);EXPECT_NEAR(testResult(0, 2), 0.9995f, POINT_EPSILON);  EXPECT_NEAR(testResult(0, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(testResult(1, 0), 1.0f, POINT_EPSILON);    EXPECT_NEAR(testResult(1, 1), 0.0f, POINT_EPSILON);    EXPECT_NEAR(testResult(1, 2), -0.0008f, POINT_EPSILON); EXPECT_NEAR(testResult(1, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(testResult(2, 0), 0.0f, POINT_EPSILON);    EXPECT_NEAR(testResult(2, 1), 0.9995f, POINT_EPSILON); EXPECT_NEAR(testResult(2, 2), 0.0308f, POINT_EPSILON);  EXPECT_NEAR(testResult(2, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(testResult(3, 0), 0.0f, POINT_EPSILON);    EXPECT_NEAR(testResult(3, 1), 0.0f, POINT_EPSILON);    EXPECT_NEAR(testResult(3, 2), 0.0f, POINT_EPSILON);     EXPECT_NEAR(testResult(3, 3), 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestMulScalarFunction)
+{
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test.mulL(test2);
+
+   test.mul(2.0f);
+
+   EXPECT_NEAR(test(0, 0), 1.0806f, POINT_EPSILON);  EXPECT_NEAR(test(0, 1), 1.6829f, POINT_EPSILON);  EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(0, 3), 8.7689f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), -0.9093f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.5839f, POINT_EPSILON);  EXPECT_NEAR(test(1, 2), 1.6829f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), -1.6958f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 1.4161f, POINT_EPSILON);  EXPECT_NEAR(test(2, 1), -0.9093f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 1.0806f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 6.3427f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(3, 3), 2.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestMulMatScalarFunction)
+{
+   MatrixF testTran(true);
+   testTran.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   testTran.mulL(test2);
+
+   MatrixF test(true);
+   test.mul(testTran, 2.0f);
+
+   EXPECT_NEAR(test(0, 0), 1.0806f, POINT_EPSILON);  EXPECT_NEAR(test(0, 1), 1.6829f, POINT_EPSILON);  EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(0, 3), 8.7689f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), -0.9093f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.5839f, POINT_EPSILON);  EXPECT_NEAR(test(1, 2), 1.6829f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), -1.6958f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 1.4161f, POINT_EPSILON);  EXPECT_NEAR(test(2, 1), -0.9093f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 1.0806f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 6.3427f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(3, 3), 2.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestMulPoint4)
+{
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test.mulL(test2);
+
+   Point4F testPoint(0.5f, 1.0f, 2.0f, 1.0f);
+   test.mul(testPoint);
+
+   EXPECT_NEAR(testPoint.x, 5.4960f, POINT_EPSILON);  EXPECT_NEAR(testPoint.y, 0.8996f, POINT_EPSILON);  EXPECT_NEAR(testPoint.z, 4.1513f, POINT_EPSILON); EXPECT_NEAR(testPoint.w, 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestMulPoint3)
+{
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test.mulL(test2);
+
+   Point3F testPoint(0.5f, 1.0f, 2.0f);
+   test.mulP(testPoint);
+
+   EXPECT_NEAR(testPoint.x, 5.4960f, POINT_EPSILON);  EXPECT_NEAR(testPoint.y, 0.8996f, POINT_EPSILON);  EXPECT_NEAR(testPoint.z, 4.1513f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestMulPoint3ToPoint3)
+{
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test.mulL(test2);
+
+   Point3F point(0.5f, 1.0f, 2.0f);
+   Point3F testPoint;
+   test.mulP(point, &testPoint);
+
+   EXPECT_NEAR(testPoint.x, 5.4960f, POINT_EPSILON);  EXPECT_NEAR(testPoint.y, 0.8996f, POINT_EPSILON);  EXPECT_NEAR(testPoint.z, 4.1513f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestMulVector)
+{
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test.mulL(test2);
+
+   VectorF testPoint(0.5f, 1.0f, 2.0f);
+   test.mulV(testPoint);
+
+   EXPECT_NEAR(testPoint.x, 1.1116f, POINT_EPSILON);  EXPECT_NEAR(testPoint.y, 1.7475f, POINT_EPSILON);  EXPECT_NEAR(testPoint.z, 0.9799f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestMulVectorToPoint3)
+{
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test.mulL(test2);
+
+   VectorF vec(0.5f, 1.0f, 2.0f);
+   Point3F testPoint;
+   test.mulV(vec, &testPoint);
+
+   EXPECT_NEAR(testPoint.x, 1.1116f, POINT_EPSILON);  EXPECT_NEAR(testPoint.y, 1.7475f, POINT_EPSILON);  EXPECT_NEAR(testPoint.z, 0.9799f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestMulBox)
+{
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   Box3F testBox(1.0f);
+
+   test.mul(testBox);
+
+   EXPECT_NEAR(testBox.minExtents.x, 4.5f, POINT_EPSILON); EXPECT_NEAR(testBox.minExtents.y, 1.5f, POINT_EPSILON); EXPECT_NEAR(testBox.minExtents.z, 0.5f, POINT_EPSILON);
+   EXPECT_NEAR(testBox.maxExtents.x, 5.5f, POINT_EPSILON); EXPECT_NEAR(testBox.maxExtents.y, 2.5f, POINT_EPSILON); EXPECT_NEAR(testBox.maxExtents.z, 1.5f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestMatrixAdd)
+{
+   MatrixF test(true);
+   MatrixF test2(true);
+   for (U32 i = 0; i < 4; i++)
+   {
+      for (U32 j = 0; j < 4; j++)
+      {
+         test(i, j) = 1.0f;
+         test2(i, j) = 1.0f;
+      }
+   }
+
+   test.add(test2);
+
+   for (U32 i = 0; i < 4; i++)
+   {
+      for (U32 j = 0; j < 4; j++)
+      {
+         EXPECT_NEAR(test(i,j), 2.0f, POINT_EPSILON);
+      }
+   }
+
+}
+
+TEST(MatrixTest, TestFrustumProjectionMatrix)
+{
+   MatrixF test(true);
+   Frustum testFrustum(false, -0.119894862f, 0.119894862f, 0.0767327100f, -0.0767327100f, 0.1f, 1000.0f);
+
+   testFrustum.getProjectionMatrix(&test);
+
+   EXPECT_NEAR(test(0, 0), 0.8341f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(0, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(1, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(1, 2), 1.3032f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(2, 1), -0.0001f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(2, 3), 0.1f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(3, 1), 1.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(3, 3), 0.0f, POINT_EPSILON);
+
+   test.reverseProjection();
+
+   EXPECT_NEAR(test(0, 0), 0.8341f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(0, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(1, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(1, 2), 1.3032f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(2, 1), 1.0001f, POINT_EPSILON);  EXPECT_NEAR(test(2, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(2, 3), -0.1f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(3, 1), 1.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(3, 3), 0.0f, POINT_EPSILON);
+
+   test.inverse();
+
+   EXPECT_NEAR(test(0, 0), 1.1989f, POINT_EPSILON); EXPECT_NEAR(test(0, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(0, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(1, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(1, 2), 0.9999f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.1f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(2, 1), 0.7673f, POINT_EPSILON);  EXPECT_NEAR(test(2, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(2, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(3, 1), 1.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(3, 3), 0.0f, POINT_EPSILON);
+
+}
+
+TEST(MatrixTest, TestUnProjectStack)
+{
+   MatrixF saveModel(true);
+   MatrixF saveProjection(true);
+   RectI renderRect(0 ,0, 1600, 1024);
+
+   Frustum testFrustum(false, -0.119894862f, 0.119894862f, 0.0767327100f, -0.0767327100f, 0.1f, 1000.0f);
+
+   testFrustum.getProjectionMatrix(&saveProjection);
+
+   Point3F dest;
+   MathUtils::mProjectScreenToWorld(Point3F(626.0f, 600.0f, 1.0f), &dest, renderRect, saveModel, saveProjection, 0.1f, 10.0f);
+   EXPECT_NEAR(dest.x, -0.2868f, POINT_EPSILON);  EXPECT_NEAR(dest.y, 1.1998f, POINT_EPSILON);   EXPECT_NEAR(dest.z, -0.1450f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestInverse)
+{
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test.mulL(test2);
+
+   test.inverse();
+
+   EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test(0, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.7081f, POINT_EPSILON);  EXPECT_NEAR(test(0, 3), -5.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON);  EXPECT_NEAR(test(1, 2), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), -2.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 0.0, POINT_EPSILON);      EXPECT_NEAR(test(2, 1), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test(2, 3), -1.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestInvertTo)
+{
+   MatrixF test1(true);
+   test1.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test1.mulL(test2);
+
+   MatrixF test(true);
+
+   test1.invertTo(&test);
+
+   EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test(0, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.7081f, POINT_EPSILON);  EXPECT_NEAR(test(0, 3), -5.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON);  EXPECT_NEAR(test(1, 2), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), -2.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 0.0, POINT_EPSILON);      EXPECT_NEAR(test(2, 1), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test(2, 3), -1.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON);
+
+   // make sure our original matrix is unchanged
+   EXPECT_NEAR(test1(0, 0), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test1(0, 1), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test1(0, 2), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test1(0, 3), 4.3845f, POINT_EPSILON);
+   EXPECT_NEAR(test1(1, 0), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test1(1, 1), 0.2919f, POINT_EPSILON);  EXPECT_NEAR(test1(1, 2), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test1(1, 3), -0.8479f, POINT_EPSILON);
+   EXPECT_NEAR(test1(2, 0), 0.7081f, POINT_EPSILON);  EXPECT_NEAR(test1(2, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test1(2, 2), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test1(2, 3), 3.1714, POINT_EPSILON);
+   EXPECT_NEAR(test1(3, 0), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test1(3, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test1(3, 2), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test1(3, 3), 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestFullInverse)
+{
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test.mulL(test2);
+
+   EXPECT_TRUE(test.fullInverse());
+
+   EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test(0, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.7081f, POINT_EPSILON);  EXPECT_NEAR(test(0, 3), -5.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON);  EXPECT_NEAR(test(1, 2), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), -2.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 0.0, POINT_EPSILON);      EXPECT_NEAR(test(2, 1), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test(2, 3), -1.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestIsAffine)
+{
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test.mulL(test2);
+
+   EXPECT_TRUE(test.isAffine());
+}
+
+TEST(MatrixTest, TestScale)
+{
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test.mulL(test2);
+
+   test.scale(2.0f);
+
+   EXPECT_NEAR(test(0, 0), 1.0806f, POINT_EPSILON);  EXPECT_NEAR(test(0, 1), 1.6829f, POINT_EPSILON);  EXPECT_NEAR(test(0, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(0, 3), 4.3845f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), -0.9093f, POINT_EPSILON); EXPECT_NEAR(test(1, 1), 0.5839f, POINT_EPSILON);  EXPECT_NEAR(test(1, 2), 1.6829f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), -0.8479f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 1.4161f, POINT_EPSILON);  EXPECT_NEAR(test(2, 1), -0.9093f, POINT_EPSILON); EXPECT_NEAR(test(2, 2), 1.0806f, POINT_EPSILON); EXPECT_NEAR(test(2, 3), 3.1714f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);    EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestGetScale)
+{
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test.mulL(test2);
+
+   test.scale(2.0f);
+
+   Point3F scale;
+   scale = test.getScale();
+
+   EXPECT_NEAR(scale.x, 2.0f, POINT_EPSILON);  EXPECT_NEAR(scale.y, 2.0f, POINT_EPSILON);  EXPECT_NEAR(scale.z, 2.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestAffineInverse)
+{
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test.mulL(test2);
+
+   test.affineInverse();
+
+   EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test(0, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.7081f, POINT_EPSILON);  EXPECT_NEAR(test(0, 3), -5.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON);  EXPECT_NEAR(test(1, 2), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), -2.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 0.0, POINT_EPSILON);      EXPECT_NEAR(test(2, 1), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test(2, 3), -1.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 1), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 2), 0.0f, POINT_EPSILON);     EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestTranspose)
+{
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test.mulL(test2);
+
+   test.transpose();
+
+   EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test(0, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.7081f, POINT_EPSILON);  EXPECT_NEAR(test(0, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON);  EXPECT_NEAR(test(1, 2), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 0.0, POINT_EPSILON);      EXPECT_NEAR(test(2, 1), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test(2, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 4.3845f, POINT_EPSILON);  EXPECT_NEAR(test(3, 1), -0.8479f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 3.1714, POINT_EPSILON);   EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestTransposeTo)
+{
+   MatrixF test1(true);
+   test1.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test1.mulL(test2);
+
+   MatrixF test(true);
+
+   test1.transposeTo(test);
+
+   EXPECT_NEAR(test(0, 0), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test(0, 1), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(0, 2), 0.7081f, POINT_EPSILON);  EXPECT_NEAR(test(0, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(1, 0), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(1, 1), 0.2919f, POINT_EPSILON);  EXPECT_NEAR(test(1, 2), -0.4546f, POINT_EPSILON); EXPECT_NEAR(test(1, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(2, 0), 0.0, POINT_EPSILON);      EXPECT_NEAR(test(2, 1), 0.8415f, POINT_EPSILON);  EXPECT_NEAR(test(2, 2), 0.5403f, POINT_EPSILON);  EXPECT_NEAR(test(2, 3), 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(test(3, 0), 4.3845f, POINT_EPSILON);  EXPECT_NEAR(test(3, 1), -0.8479f, POINT_EPSILON); EXPECT_NEAR(test(3, 2), 3.1714, POINT_EPSILON);   EXPECT_NEAR(test(3, 3), 1.0f, POINT_EPSILON);
+}
+
+TEST(MatrixTest, TestTransformPlane)
+{
+   MatrixF test(true);
+   test.setPosition(Point3F(5.0f, 2.0f, 1.0f));
+   MatrixF test2(EulerF(1.0f, 0.0f, 1.0f));
+
+   test.mulL(test2);
+
+   PlaneF plane(Point3F(0.0f, 0.0f, 0.0f), Point3F(0.0f, 0.0f, 1.0f));
+
+   PlaneF res;
+   mTransformPlane(test, Point3F(1.0f, 1.0f, 1.0f), plane, &res);
+
+   EXPECT_NEAR(res.x, 0.0f, POINT_EPSILON);
+   EXPECT_NEAR(res.y, 0.8414f, POINT_EPSILON);
+   EXPECT_NEAR(res.z, 0.5403f, POINT_EPSILON);
+   EXPECT_NEAR(res.d, -1.0f, POINT_EPSILON);
+
+}

Tools/CMake/torqueConfig.h.in

@@ -32,7 +32,7 @@
 
 /// What's the name of your application? Used in a variety of places.
 #define TORQUE_APP_NAME            "@TORQUE_APP_NAME@"
-
+#cmakedefine USE_TEMPLATE_MATRIX
 /// What version of the application specific source code is this?
 ///
 /// Version number is major * 1000 + minor * 100 + revision * 10.

Tools/CMake/torque_configs.cmake

@@ -74,7 +74,7 @@ advanced_option(TORQUE_TOOLS "Enable or disable the tools" ON)
 advanced_option(TORQUE_TOOLS_EXT_COMMANDS "Enable or disable some extended functionality like shell commands or free write access" OFF)
 advanced_option(TORQUE_ENABLE_PROFILER "Enable or disable the profiler" OFF)
 advanced_option(TORQUE_SHOW_LEGACY_FILE_FIELDS "If on, shows legacy direct file path fields in the inspector." OFF)
-
+advanced_option(USE_TEMPLATE_MATRIX "Set to true to use the new templated matrix class(still in beta)." OFF)
 #testing
 advanced_option(TORQUE_TESTING "Unit test build" OFF)