From 23e5d817469c2b3e84739b3b5a9ced44cca29d6f Mon Sep 17 00:00:00 2001 From: Magnus Norddahl Date: Sun, 5 Nov 2017 15:32:42 +0100 Subject: [PATCH] - Implement VSMatrix::inverseMatrix --- src/gl/data/gl_matrix.cpp | 210 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 210 insertions(+) diff --git a/src/gl/data/gl_matrix.cpp b/src/gl/data/gl_matrix.cpp index 115882857..9063fc59b 100644 --- a/src/gl/data/gl_matrix.cpp +++ b/src/gl/data/gl_matrix.cpp @@ -484,3 +484,213 @@ VSMatrix::multMatrix(FLOATTYPE *resMat, const FLOATTYPE *aMatrix) } memcpy(resMat, res, 16 * sizeof(FLOATTYPE)); } + +static double mat3Determinant(const FLOATTYPE *mMat3x3) +{ + return mMat3x3[0] * (mMat3x3[4] * mMat3x3[8] - mMat3x3[5] * mMat3x3[7]) + + mMat3x3[1] * (mMat3x3[5] * mMat3x3[6] - mMat3x3[8] * mMat3x3[3]) + + mMat3x3[2] * (mMat3x3[3] * mMat3x3[7] - mMat3x3[4] * mMat3x3[6]); +} + +static double mat4Determinant(const FLOATTYPE *matrix) +{ + FLOATTYPE mMat3x3_a[9] = + { + matrix[1 * 4 + 1], matrix[2 * 4 + 1], matrix[3 * 4 + 1], + matrix[1 * 4 + 2], matrix[2 * 4 + 2], matrix[3 * 4 + 2], + matrix[1 * 4 + 3], matrix[2 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_b[9] = + { + matrix[1 * 4 + 0], matrix[2 * 4 + 0], matrix[3 * 4 + 0], + matrix[1 * 4 + 2], matrix[2 * 4 + 2], matrix[3 * 4 + 2], + matrix[1 * 4 + 3], matrix[2 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_c[9] = + { + matrix[1 * 4 + 0], matrix[2 * 4 + 0], matrix[3 * 4 + 0], + matrix[1 * 4 + 1], matrix[2 * 4 + 1], matrix[3 * 4 + 1], + matrix[1 * 4 + 3], matrix[2 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_d[9] = + { + matrix[1 * 4 + 0], matrix[2 * 4 + 0], matrix[3 * 4 + 0], + matrix[1 * 4 + 1], matrix[2 * 4 + 1], matrix[3 * 4 + 1], + matrix[1 * 4 + 2], matrix[2 * 4 + 2], matrix[3 * 4 + 2] + }; + + FLOATTYPE a, b, c, d; + FLOATTYPE value; + + a = mat3Determinant(mMat3x3_a); + b = mat3Determinant(mMat3x3_b); + c = mat3Determinant(mMat3x3_c); + d = mat3Determinant(mMat3x3_d); + + value = matrix[0 * 4 + 0] * a; + value -= matrix[0 * 4 + 1] * b; + value += matrix[0 * 4 + 2] * c; + value -= matrix[0 * 4 + 3] * d; + + return value; +} + +static void mat4Adjoint(const FLOATTYPE *matrix, FLOATTYPE *result) +{ + FLOATTYPE mMat3x3_a[9] = + { + matrix[1 * 4 + 1], matrix[2 * 4 + 1], matrix[3 * 4 + 1], + matrix[1 * 4 + 2], matrix[2 * 4 + 2], matrix[3 * 4 + 2], + matrix[1 * 4 + 3], matrix[2 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_b[9] = + { + matrix[1 * 4 + 0], matrix[2 * 4 + 0], matrix[3 * 4 + 0], + matrix[1 * 4 + 2], matrix[2 * 4 + 2], matrix[3 * 4 + 2], + matrix[1 * 4 + 3], matrix[2 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_c[9] = + { + matrix[1 * 4 + 0], matrix[2 * 4 + 0], matrix[3 * 4 + 0], + matrix[1 * 4 + 1], matrix[2 * 4 + 1], matrix[3 * 4 + 1], + matrix[1 * 4 + 3], matrix[2 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_d[9] = + { + matrix[1 * 4 + 0], matrix[2 * 4 + 0], matrix[3 * 4 + 0], + matrix[1 * 4 + 1], matrix[2 * 4 + 1], matrix[3 * 4 + 1], + matrix[1 * 4 + 2], matrix[2 * 4 + 2], matrix[3 * 4 + 2] + }; + + FLOATTYPE mMat3x3_e[9] = + { + matrix[0 * 4 + 1], matrix[2 * 4 + 1], matrix[3 * 4 + 1], + matrix[0 * 4 + 2], matrix[2 * 4 + 2], matrix[3 * 4 + 2], + matrix[0 * 4 + 3], matrix[2 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_f[9] = + { + matrix[0 * 4 + 0], matrix[2 * 4 + 0], matrix[3 * 4 + 0], + matrix[0 * 4 + 2], matrix[2 * 4 + 2], matrix[3 * 4 + 2], + matrix[0 * 4 + 3], matrix[2 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_g[9] = + { + matrix[0 * 4 + 0], matrix[2 * 4 + 0], matrix[3 * 4 + 0], + matrix[0 * 4 + 1], matrix[2 * 4 + 1], matrix[3 * 4 + 1], + matrix[0 * 4 + 3], matrix[2 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_h[9] = + { + matrix[0 * 4 + 0], matrix[2 * 4 + 0], matrix[3 * 4 + 0], + matrix[0 * 4 + 1], matrix[2 * 4 + 1], matrix[3 * 4 + 1], + matrix[0 * 4 + 2], matrix[2 * 4 + 2], matrix[3 * 4 + 2] + }; + + FLOATTYPE mMat3x3_i[9] = + { + matrix[0 * 4 + 1], matrix[1 * 4 + 1], matrix[3 * 4 + 1], + matrix[0 * 4 + 2], matrix[1 * 4 + 2], matrix[3 * 4 + 2], + matrix[0 * 4 + 3], matrix[1 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_j[9] = + { + matrix[0 * 4 + 0], matrix[1 * 4 + 0], matrix[3 * 4 + 0], + matrix[0 * 4 + 2], matrix[1 * 4 + 2], matrix[3 * 4 + 2], + matrix[0 * 4 + 3], matrix[1 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_k[9] = + { + matrix[0 * 4 + 0], matrix[1 * 4 + 0], matrix[3 * 4 + 0], + matrix[0 * 4 + 1], matrix[1 * 4 + 1], matrix[3 * 4 + 1], + matrix[0 * 4 + 3], matrix[1 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_l[9] = + { + matrix[0 * 4 + 0], matrix[1 * 4 + 0], matrix[3 * 4 + 0], + matrix[0 * 4 + 1], matrix[1 * 4 + 1], matrix[3 * 4 + 1], + matrix[0 * 4 + 2], matrix[1 * 4 + 2], matrix[3 * 4 + 2] + }; + + FLOATTYPE mMat3x3_m[9] = + { + matrix[0 * 4 + 1], matrix[1 * 4 + 1], matrix[2 * 4 + 1], + matrix[0 * 4 + 2], matrix[1 * 4 + 2], matrix[2 * 4 + 2], + matrix[0 * 4 + 3], matrix[1 * 4 + 3], matrix[2 * 4 + 3] + }; + + FLOATTYPE mMat3x3_n[9] = + { + matrix[0 * 4 + 0], matrix[1 * 4 + 0], matrix[2 * 4 + 0], + matrix[0 * 4 + 2], matrix[1 * 4 + 2], matrix[2 * 4 + 2], + matrix[0 * 4 + 3], matrix[1 * 4 + 3], matrix[2 * 4 + 3] + }; + + FLOATTYPE mMat3x3_o[9] = + { + matrix[0 * 4 + 0], matrix[1 * 4 + 0], matrix[2 * 4 + 0], + matrix[0 * 4 + 1], matrix[1 * 4 + 1], matrix[2 * 4 + 1], + matrix[0 * 4 + 3], matrix[1 * 4 + 3], matrix[2 * 4 + 3] + }; + + FLOATTYPE mMat3x3_p[9] = + { + matrix[0 * 4 + 0], matrix[1 * 4 + 0], matrix[2 * 4 + 0], + matrix[0 * 4 + 1], matrix[1 * 4 + 1], matrix[2 * 4 + 1], + matrix[0 * 4 + 2], matrix[1 * 4 + 2], matrix[2 * 4 + 2] + }; + + result[0 * 4 + 0] = mat3Determinant(mMat3x3_a); + result[1 * 4 + 0] = -mat3Determinant(mMat3x3_b); + result[2 * 4 + 0] = mat3Determinant(mMat3x3_c); + result[3 * 4 + 0] = -mat3Determinant(mMat3x3_d); + result[0 * 4 + 1] = -mat3Determinant(mMat3x3_e); + result[1 * 4 + 1] = mat3Determinant(mMat3x3_f); + result[2 * 4 + 1] = -mat3Determinant(mMat3x3_g); + result[3 * 4 + 1] = mat3Determinant(mMat3x3_h); + result[0 * 4 + 2] = mat3Determinant(mMat3x3_i); + result[1 * 4 + 2] = -mat3Determinant(mMat3x3_j); + result[2 * 4 + 2] = mat3Determinant(mMat3x3_k); + result[3 * 4 + 2] = -mat3Determinant(mMat3x3_l); + result[0 * 4 + 3] = -mat3Determinant(mMat3x3_m); + result[1 * 4 + 3] = mat3Determinant(mMat3x3_n); + result[2 * 4 + 3] = -mat3Determinant(mMat3x3_o); + result[3 * 4 + 3] = mat3Determinant(mMat3x3_p); +} + +bool VSMatrix::inverseMatrix(VSMatrix &result) +{ + // Calculate mat4 determinant + FLOATTYPE det = mat4Determinant(mMatrix); + + // Inverse unknown when determinant is close to zero + if (fabs(det) < 1e-15) + { + for (int i = 0; i < 16; i++) + result.mMatrix[i] = FLOATTYPE(0.0); + return false; + } + else + { + mat4Adjoint(mMatrix, result.mMatrix); + + FLOATTYPE invDet = FLOATTYPE(1.0) / det; + for (int i = 0; i < 16; i++) + { + result.mMatrix[i] = result.mMatrix[i] * invDet; + } + } + return true; +}