diff --git a/include/J3ML/LinearAlgebra/Matrix4x4.h b/include/J3ML/LinearAlgebra/Matrix4x4.h index 513b46f..5cf6132 100644 --- a/include/J3ML/LinearAlgebra/Matrix4x4.h +++ b/include/J3ML/LinearAlgebra/Matrix4x4.h @@ -65,14 +65,7 @@ namespace LinearAlgebra { Matrix4x4(const Quaternion& orientation, const Vector3 &translation); Vector3 GetTranslatePart() const; - Matrix3x3 GetRotatePart() const - { - return Matrix3x3 { - At(0, 0), At(0, 1), At(0, 2), - At(1, 0), At(1, 1), At(1, 2), - At(2, 0), At(2, 1), At(2, 2) - }; - } + Matrix3x3 GetRotatePart() const; void SetTranslatePart(float translateX, float translateY, float translateZ); void SetTranslatePart(const Vector3& offset); void SetRotatePart(const Quaternion& q); @@ -103,10 +96,15 @@ namespace LinearAlgebra { Vector4 WorldY() const; Vector4 WorldZ() const; + float Determinant3x3() const; /// Computes the determinant of this matrix. // If the determinant is nonzero, this matrix is invertible. float Determinant() const; + #define SKIPNUM(val, skip) (val >= skip ? (val+1) : val) + + float Minor(int i, int j) const; + Matrix4x4 Inverse() const; Matrix4x4 Transpose() const; diff --git a/src/J3ML/LinearAlgebra/Matrix4x4.cpp b/src/J3ML/LinearAlgebra/Matrix4x4.cpp index d79a5d9..9311767 100644 --- a/src/J3ML/LinearAlgebra/Matrix4x4.cpp +++ b/src/J3ML/LinearAlgebra/Matrix4x4.cpp @@ -309,4 +309,52 @@ namespace LinearAlgebra { }; return i; } + + float Matrix4x4::Minor(int i, int j) const { + int r0 = SKIPNUM(0, i); + int r1 = SKIPNUM(1, i); + int r2 = SKIPNUM(2, i); + int c0 = SKIPNUM(0, j); + int c1 = SKIPNUM(1, j); + int c2 = SKIPNUM(2, j); + + float a = At(r0, c0); + float b = At(r0, c1); + float c = At(r0, c2); + float d = At(r1, c0); + float e = At(r1, c1); + float f = At(r1, c2); + float g = At(r2, c0); + float h = At(r2, c1); + float k = At(r2, c2); + + return a*e*k + b*f*g + c*d*h - a*f*h - b*d*k - c*e*g; + } + + float Matrix4x4::Determinant() const { + return At(0, 0) * Minor(0,0) - At(0, 1) * Minor(0,1) + At(0, 2) * Minor(0,2) - At(0, 3) * Minor(0,3); + } + + float Matrix4x4::Determinant3x3() const { + + const float a = elems[0][0]; + const float b = elems[0][1]; + const float c = elems[0][2]; + const float d = elems[1][0]; + const float e = elems[1][1]; + const float f = elems[1][2]; + const float g = elems[2][0]; + const float h = elems[2][1]; + const float i = elems[2][2]; + + return a*e*i + b*f*g + c*d*h - a*f*h - b*d*i - c*e*g; + } + + Matrix3x3 Matrix4x4::GetRotatePart() const { + return Matrix3x3 { + At(0, 0), At(0, 1), At(0, 2), + At(1, 0), At(1, 1), At(1, 2), + At(2, 0), At(2, 1), At(2, 2) + }; + } } \ No newline at end of file