Implement generic matrix Inverse, LUDecompose, CholeskyDecompose

include/J3ML/LinearAlgebra/Matrix4x4.h

@@ -11,6 +11,115 @@
 
 namespace J3ML::LinearAlgebra {
 
+    template <typename Matrix>
+    bool InverseMatrix(Matrix &mat, float epsilon)
+    {
+        Matrix inversed = Matrix::Identity;
+
+        const int nc = std::min<int>(Matrix::Rows, Matrix::Cols);
+
+        for (int column = 0; column < nc; ++column)
+        {
+            // find the row i with i >= j such that M has the largest absolute value.
+            int greatest = column;
+            float greatestVal = std::abs(mat[greatest][column]);
+            for (int i = column+1; i < Matrix::Rows; i++)
+            {
+                float val = std::abs(mat[i][column]);
+                if (val > greatestVal) {
+                    greatest = i;
+                    greatestVal = val;
+                }
+            }
+
+            if (greatestVal < epsilon) {
+                mat = inversed;
+                return false;
+            }
+
+            // exchange rows
+            if (greatest != column) {
+                inversed.SwapRows(greatest, column);
+                mat.SwapRows(greatest, column);
+            }
+
+            // multiply rows
+            assert(!Math::EqualAbs(mat[column][column], 0.f, epsilon));
+            float scale = 1.f / mat[column][column];
+            inversed.ScaleRow(column, scale);
+            mat.ScaleRow(column, scale);
+
+            // add rows
+            for (int i = 0; i < column; i++) {
+                inversed.SetRow(i, inversed.Row(i) - inversed.Row(column) * mat[i][column]);
+                mat.SetRow(i, mat.Row(i) - mat.Row(column) * mat[i][column]);
+            }
+
+            for (int i = column + 1; i < Matrix::Rows; i++) {
+                inversed.SetRow(i, inversed.Row(i) - inversed.Row(column) * mat[i][column]);
+                mat.SetRow(i, mat.Row(i) - mat.Row(column) * mat[i][column]);
+            }
+        }
+        mat = inversed;
+        return true;
+    }
+
+
+    /// Computes the LU-decomposition on the given square matrix.
+    /// @return True if the composition was successful, false otherwise. If the return value is false, the contents of the output matrix are unspecified.
+    template <typename Matrix>
+    bool LUDecomposeMatrix(const Matrix &mat, Matrix &lower, Matrix &upper)
+    {
+        lower = Matrix::Identity;
+        upper = Matrix::Zero;
+
+        for (int i = 0; i < Matrix::Rows; ++i)
+        {
+            for (int col = i; col < Matrix::Cols; ++col)
+            {
+                upper[i][col] = mat[i][col];
+                for (int k = 0; k < i; ++k)
+                    upper[i][col] -= lower[i][k] * upper[k][col];
+            }
+            for (int row = i+1; row < Matrix::Rows; ++row)
+            {
+                lower[row][i] = mat[row][i];
+                for (int k = 0; k < i; ++k)
+                    lower[row][i] -= lower[row][k] * upper[k][i];
+                if (Math::EqualAbs(upper[i][i], 0.f))
+                    return false;
+                lower[row][i] /= upper[i][i];
+            }
+        }
+        return true;
+    }
+
+    /// Computes the Cholesky decomposition on the given square matrix *on the real domain*.
+    /// @return True if successful, false otherwise. If the return value is false, the contents of the output matrix are uspecified.
+    template <typename Matrix>
+    bool CholeskyDecomposeMatrix(const Matrix &mat, Matrix& lower)
+    {
+        lower = Matrix::Zero;
+        for (int i = 0; i < Matrix::Rows; ++i)
+        {
+            for (int j = 0; j < i; ++i)
+            {
+                lower[i][j] = mat[i][j];
+                for (int k = 0; k < j; ++k)
+                    lower[i][j] -= lower[i][j] * lower[j][k];
+                if (Math::EqualAbs(lower[j][j], 0.f))
+                    return false;
+                lower[i][j] /= lower[j][j];
+            }
+            lower[i][i] = mat[i][i];
+            if (lower[i][i])
+                return false;
+            for (int k = 0; k < i; ++k)
+                lower[i][i] -= lower[i][k] * lower[i][k];
+            lower[i][i] = std::sqrt(lower[i][i]);
+        }
+    }
+
     /// @brief A 4-by-4 matrix for affine transformations and perspective projections of 3D geometry.
     /// This matrix can represent the most generic form of transformations for 3D objects,
     /// including perspective projections, which a 4-by-3 cannot store, and translations, which a 3-by-3 cannot represent.
@@ -175,9 +284,29 @@ namespace J3ML::LinearAlgebra {
         Vector3 GetTranslatePart() const;
         /// Returns the top-left 3x3 part of this matrix. This stores the rotation part of this matrix (if this matrix represents a rotation).
         Matrix3x3 GetRotatePart() const;
+
+        /// Sets the translation part of this matrix.
+        /** This function sets the translation part of this matrix. These are the first three elements of the fourth column. All other entries are left untouched. */
         void SetTranslatePart(float translateX, float translateY, float translateZ);
         void SetTranslatePart(const Vector3& offset);
+        /// Sets the 3-by-3 part of this matrix to perform rotation about the given axis and angle (in radians). Leaves all other
+        /// entries of this matrix untouched.
         void SetRotatePart(const Quaternion& q);
+        void SetRotatePart(const Vector3& axisDirection, float angleRadians);
+
+        /// Sets the 3-by-3 part of this matrix.
+        /// @note This is a convenience function which calls Set3x3Part.
+        /// @note This function erases the previous top-left 3x3 part of this matrix (any previous rotation, scaling and shearing, etc.) Translation is unaffecte.d
+        void SetRotatePart(const Matrix3x3& rotation) { Set3x3Part(rotation); }
+        /// Sets the 3-by-3 part of this matrix to perform rotation about the positive X axis which passes through the origin.
+        /// Leaves all other entries of this matrix untouched.
+        void SetRotatePartX(float angleRadians);
+        /// Sets the 3-by-3 part of this matrix to perform the rotation about the positive Y axis.
+        /// Leaves all other entries of the matrix untouched.
+        void SetRotatePartY(float angleRadians);
+        /// Sets the 3-by-3 part of this matrix to perform the rotation about the positive Z axis.
+        /// Leaves all other entries of the matrix untouched.
+        void SetRotatePartZ(float angleRadians);
         void Set3x3Part(const Matrix3x3& r);
 
         void SetRow(int row, const Vector3& rowVector, float m_r3);
@@ -347,8 +476,101 @@ namespace J3ML::LinearAlgebra {
         /// i.e. whether the last row of this matrix differs from [0 0 0 1]
         bool ContainsProjection(float epsilon = 1e-3f) const;
 
+
+        /// Sets all values of this matrix.
+        void Set(float _00, float _01, float _02, float _03,
+                 float _10, float _11, float _12, float _13,
+                 float _20, float _21, float _22, float _23,
+                 float _30, float _31, float _32, float _34);
+
+        /// Sets this to be a copy of the matrix rhs.
+        void Set(const Matrix4x4 &rhs);
+
+        /// Sets all values of this matrix.
+        /** @param values The values in this array will be copied over to this matrix. The source must contain 16 floats in row-major order
+            (the same order as the Set() ufnction above has its input parameters in. */
+        void Set(const float *values);
+
+        /// Sets this matrix to equal the identity.
+        void SetIdentity();
+        /// Returns the adjugate of this matrix.
+        Matrix4x4 Adjugate() const;
+
+        /// Computes the Cholesky decomposition of this matrix.
+        /// The returned matrix L satisfies L * transpose(L) = this;
+        /// Returns true on success.
+        bool ColeskyDecompose(Matrix4x4 &outL) const;
+
+        /// Computes the LU decomposition of this matrix.
+        /// This decomposition has the form 'this = L * U'
+        /// Returns true on success.
+        bool LUDecompose(Matrix4x4& outLower, Matrix4x4& outUpper) const;
+
+        /// Inverts this matrix using the generic Gauss's method.
+        /// @return Returns true on success, false otherwise.
+        bool Inverse(float epsilon = 1e-6f)
+        {
+            return InverseMatrix(*this, epsilon);
+        }
+
+        /// Returns an inverted copy of this matrix.
+        /// If this matrix does not have an inverse, returns the matrix that was the result of running
+        /// Gauss's method on the matrix.
+        Matrix4x4 Inverted() const;
+
+        /// Inverts a column-orthogonal matrix.
+        /// If a matrix is of form M=T*R*S, where T is an affine translation matrix
+        /// R is a rotation matrix and S is a diagonal matrix with non-zero but pote ntially non-uniform scaling
+        /// factors (possibly mirroring), then the matrix M is column-orthogonal and this function can be used to compute the inverse.
+        /// Calling this function is faster than the calling the generic matrix Inverse() function.
+        /// Returns true on success. On failure, the matrix is not modified. This function fails if any of the
+        /// elements of this vector are not finite, or if the matrix contains a zero scaling factor on X, Y, or Z.
+        /// This function may not be called if this matrix contains any projection (last row differs from (0 0 0 1)).
+        /// @note The returned matrix will be row-orthogonal, but not column-orthogonal in general.
+        /// The returned matrix will be column-orthogonal if the original matrix M was row-orthogonal as well.
+        /// (in which case S had uniform scale, InverseOrthogonalUniformScale() could have been used instead).
+        bool InverseColOrthogonal();
+
+
+        /// Inverts a matrix that is a concatenation of only translate, rotate, and uniform scale operations.
+        /// If a matrix is of form M = T*R*S, where T is an affine translation matrix,
+        /// R is a rotation matrix and S is a diagonal matrix with non-zero and uniform scaling factors (possibly mirroring),
+        /// then the matrix M is both column- and row-orthogonal and this function can be used to compute this inverse.
+        /// This function is faster than calling InverseColOrthogonal() or the generic Inverse().
+        /// Returns true on success. On failure, the matrix is not modified. This function fails if any of the
+        /// elements of this vector are not finite, or if the matrix contains a zero scaling factor on X, Y, or Z.
+        /// This function may not be called if this matrix contains any shearing or nonuniform scaling.
+        /// This function may not be called if this matrix contains any projection (last row differs from (0 0 0 1)).
+        bool InverseOrthogonalUniformScale();
+
+        /// Inverts a matrix that is a concatenation of only translate and rotate operations.
+        /// If a matrix is of form M = T*R*S, where T is an affine translation matrix, R is a rotation
+        /// matrix and S is either identity or a mirroring matrix, then the matrix M is orthonormal and this function can be used to compute the inverse.
+        /// This function is faster than calling InverseOrthogonalUniformScale(), InverseColOrthogonal(), or the generic Inverse().
+        /// This function may not be called if this matrix contains any scaling or shearing, but it may contain mirroring.
+        /// This function may not be called if this matrix contains any projection (last row differs from (0 0 0 1)).
         void InverseOrthonormal();
 
+        /// Transposes this matrix.
+        /// This operation swaps all elements with respect to the diagonal.
+        void Transpose();
+        /// Returns a transposed copy of this matrix.
+        Matrix4x4 Transposed() const;
+        /// Computes the inverse transpose of this matrix in-place.
+        /// Use the inverse transpose to transform covariant vectors (normal vectors).
+        bool InverseTranspose();
+        /// Returns the inverse transpose of this matrix.
+        /// Use that matrix to transform covariant vectors (normal vectors).
+        Matrix4x4 InverseTransposed() const
+        {
+            Matrix4x4 copy = *this;
+            copy.Transpose();
+            copy.Inverse();
+            return copy;
+        }
+        /// Returns the sum of the diagonal elements of this matrix.
+        float Trace() const;
+
     protected:
         float elems[4][4];
 

src/J3ML/LinearAlgebra/Matrix4x4.cpp

@@ -706,4 +706,61 @@ namespace J3ML::LinearAlgebra {
         SetCol(column, columnVector.x, columnVector.y, columnVector.z, columnVector.w);
     }
 
+    void Matrix4x4::Transpose() {
+        Swap(elems[0][1], elems[1][0]);
+        Swap(elems[0][2], elems[2][0]);
+        Swap(elems[0][3], elems[3][0]);
+
+        Swap(elems[1][2], elems[2][1]);
+        Swap(elems[1][3], elems[3][1]);
+        Swap(elems[2][3], elems[3][2]);
+    }
+
+    Matrix4x4 Matrix4x4::Transposed() const {
+        Matrix4x4 copy;
+        copy.elems[0][0] = elems[0][0]; copy.elems[0][1] = elems[1][0]; copy.elems[0][2] = elems[2][0]; copy.elems[0][3] = elems[3][0];
+        copy.elems[1][0] = elems[0][1]; copy.elems[1][1] = elems[1][1]; copy.elems[1][2] = elems[2][1]; copy.elems[1][3] = elems[3][1];
+        copy.elems[2][0] = elems[0][2]; copy.elems[2][1] = elems[1][2]; copy.elems[2][2] = elems[2][2]; copy.elems[2][3] = elems[3][2];
+        copy.elems[3][0] = elems[0][3]; copy.elems[3][1] = elems[1][3]; copy.elems[3][2] = elems[2][3]; copy.elems[3][3] = elems[3][3];
+        return copy;
+    }
+
+    bool Matrix4x4::InverseTranspose() {
+        bool success = Inverse();
+        Transpose();
+        return success;
+    }
+
+    float Matrix4x4::Trace() const {
+        assert(IsFinite());
+        return elems[0][0] + elems[1][1] + elems[2][2] + elems[3][3];
+    }
+
+    bool Matrix4x4::InverseOrthogonalUniformScale() {
+        assert(!ContainsProjection());
+        assert(IsColOrthogonal(1e-3f));
+        assert(HasUniformScale());
+
+        Swap(At(0, 1), At(1, 0));
+        Swap(At(0, 2), At(2, 0));
+        Swap(At(1, 2), At(2, 1));
+        float scale = Vector3(At(0,0), At(1, 0), At(2, 0)).LengthSquared();
+        if (scale == 0.f)
+            return false;
+        scale = 1.f / scale;
+
+        At(0, 0) *= scale; At(0, 1) *= scale; At(0, 2) *= scale;
+        At(1, 0) *= scale; At(1, 1) *= scale; At(1, 2) *= scale;
+        At(2, 0) *= scale; At(2, 1) *= scale; At(2, 2) *= scale;
+
+        SetTranslatePart(TransformDir(-At(0, 3), -At(1, 3), -At(2, 3)));
+        return true;
+    }
+
+    Matrix4x4 Matrix4x4::Inverted() const {
+        Matrix4x4 copy = *this;
+        copy.Inverse();
+        return copy;
+    }
+
 }