diff --git a/include/J3ML/LinearAlgebra/Matrix4x4.h b/include/J3ML/LinearAlgebra/Matrix4x4.h index 984d5c8..a8efaf8 100644 --- a/include/J3ML/LinearAlgebra/Matrix4x4.h +++ b/include/J3ML/LinearAlgebra/Matrix4x4.h @@ -11,6 +11,115 @@ namespace J3ML::LinearAlgebra { + template + bool InverseMatrix(Matrix &mat, float epsilon) + { + Matrix inversed = Matrix::Identity; + + const int nc = std::min(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 + 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 + 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]; diff --git a/src/J3ML/LinearAlgebra/Matrix4x4.cpp b/src/J3ML/LinearAlgebra/Matrix4x4.cpp index b27bdaa..d4c0900 100644 --- a/src/J3ML/LinearAlgebra/Matrix4x4.cpp +++ b/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; + } + } \ No newline at end of file