normalize and affineInverse

added functions for normalize and affineInverse

Engine/source/math/mMatrix.cpp

@@ -372,9 +372,9 @@ Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::inverse()
          }
       }
 
-      // Early out if pivot is 0, return a new empty matrix.
+      // Early out if pivot is 0, return identity matrix.
       if (augmentedMatrix(i, i) == static_cast<DATA_TYPE>(0)) {
-         return Matrix<DATA_TYPE, rows, cols>();
+         return Matrix<DATA_TYPE, rows, cols>(true);
       }
 
       DATA_TYPE pivotVal = augmentedMatrix(i, i);
@@ -550,6 +550,27 @@ bool Matrix<DATA_TYPE, rows, cols>::isAffine() const
    return true;
 }
 
+template<typename DATA_TYPE, U32 rows, U32 cols>
+Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::affineInverse()
+{
+   AssertFatal(rows >= 4 && cols >= 4, "affineInverse requires at least 4x4");
+   Matrix<DATA_TYPE, 3, 3> subMatrix;
+
+   for (U32 i = 0; i < 3; i++) {
+      for (U32 j = 0; j < 3; j++) {
+         subMatrix(i, j) = (*this)(i, j);
+      }
+   }
+
+   subMatrix.transpose();
+
+   Point3F pos = getPosition();
+   (*this)(0, 3) = mDot(subMatrix.getColumn3F(0), pos);
+   (*this)(1, 3) = mDot(subMatrix.getColumn3F(1), pos);
+   (*this)(2, 3) = mDot(subMatrix.getColumn3F(2), pos);
+
+   return *this;
+}
 
 template<typename DATA_TYPE, U32 rows, U32 cols>
 EulerF Matrix<DATA_TYPE, rows, cols>::toEuler() const

Engine/source/math/mMatrix.h

@@ -653,6 +653,8 @@ public:
    /// Make this an identity matrix.
    Matrix<DATA_TYPE, rows, cols>& identity();
 
+   void normalize();
+
    Matrix<DATA_TYPE, rows, cols>& set(const EulerF& e);
 
    Matrix(const EulerF& e, const Point3F p);
@@ -730,8 +732,14 @@ public:
    void mul(Box3F& box) const;
 
    // ------ Getters ------
+   // col * rows + row
+   // static U32 idx(U32 i, U32 j) { return (i * rows + j); }
    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;
@@ -906,6 +914,26 @@ inline Matrix<DATA_TYPE, rows, cols>& Matrix<DATA_TYPE, rows, cols>::identity()
    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)
 {