diff --git a/irr/include/matrix4.h b/irr/include/matrix4.h index 7d9a11ca5..0a9f6fd26 100644 --- a/irr/include/matrix4.h +++ b/irr/include/matrix4.h @@ -276,6 +276,8 @@ public: /** \param out: where result matrix is written to. */ bool getInversePrimitive(CMatrix4 &out) const; + T determinant() const; + //! Gets the inverse matrix of this one /** \param out: where result matrix is written to. \return Returns false if there is no inverse matrix. */ @@ -1067,6 +1069,19 @@ inline void CMatrix4::translateVect(vector3df &vect) const vect.Z = vect.Z + M[14]; } +template +inline T CMatrix4::determinant() const +{ + // Calculates the determinant using the rule of Sarrus. + const CMatrix4 &m = *this; + return (m[0] * m[5] - m[1] * m[4]) * (m[10] * m[15] - m[11] * m[14]) - + (m[0] * m[6] - m[2] * m[4]) * (m[9] * m[15] - m[11] * m[13]) + + (m[0] * m[7] - m[3] * m[4]) * (m[9] * m[14] - m[10] * m[13]) + + (m[1] * m[6] - m[2] * m[5]) * (m[8] * m[15] - m[11] * m[12]) - + (m[1] * m[7] - m[3] * m[5]) * (m[8] * m[14] - m[10] * m[12]) + + (m[2] * m[7] - m[3] * m[6]) * (m[8] * m[13] - m[9] * m[12]); +} + template inline bool CMatrix4::getInverse(CMatrix4 &out) const { @@ -1076,12 +1091,7 @@ inline bool CMatrix4::getInverse(CMatrix4 &out) const const CMatrix4 &m = *this; - f32 d = (m[0] * m[5] - m[1] * m[4]) * (m[10] * m[15] - m[11] * m[14]) - - (m[0] * m[6] - m[2] * m[4]) * (m[9] * m[15] - m[11] * m[13]) + - (m[0] * m[7] - m[3] * m[4]) * (m[9] * m[14] - m[10] * m[13]) + - (m[1] * m[6] - m[2] * m[5]) * (m[8] * m[15] - m[11] * m[12]) - - (m[1] * m[7] - m[3] * m[5]) * (m[8] * m[14] - m[10] * m[12]) + - (m[2] * m[7] - m[3] * m[6]) * (m[8] * m[13] - m[9] * m[12]); + f32 d = determinant(); if (iszero(d, FLT_MIN)) return false;