double R3MatrixDet4 (double a, double b, double c, double d, double e, double f, double g, double h, double i, double j, double k, double l, double m, double n, double o, double p) { // Return determinant of 4x4 matrix double det = 0.0; det += a * R3MatrixDet3(f,g,h,j,k,l,n,o,p); det -= b * R3MatrixDet3(e,g,h,i,k,l,m,o,p); det += c * R3MatrixDet3(e,f,h,i,j,l,m,n,p); det -= d * R3MatrixDet3(e,f,g,i,j,k,m,n,o); return (det); }
const RNScalar R3Matrix:: Determinant(void) const { // Return matrix determinant return R3MatrixDet3( m[0][0], m[0][1], m[0][2], m[1][0], m[1][1], m[1][2], m[2][0], m[2][1], m[2][2]); }
void R3Matrix:: Invert(void) { // Copy matrix into local variables double Ma, Mb, Mc, Md, Me, Mf, Mg, Mh, Mi, Mj, Mk, Ml, Mm, Mn, Mo, Mp; Ma = m[0][0]; Mb = m[0][1]; Mc = m[0][2]; Md = m[0][3]; Me = m[1][0]; Mf = m[1][1]; Mg = m[1][2]; Mh = m[1][3]; Mi = m[2][0]; Mj = m[2][1]; Mk = m[2][2]; Ml = m[2][3]; Mm = m[3][0]; Mn = m[3][1]; Mo = m[3][2]; Mp = m[3][3]; // Compute sub-determinants and determinant double a1 = R3MatrixDet3(Mf, Mg, Mh, Mj, Mk, Ml, Mn, Mo, Mp); double a2 = R3MatrixDet3(Me, Mg, Mh, Mi, Mk, Ml, Mm, Mo, Mp); double a3 = R3MatrixDet3(Me, Mf, Mh, Mi, Mj, Ml, Mm, Mn, Mp); double a4 = R3MatrixDet3(Me, Mf, Mg, Mi, Mj, Mk, Mm, Mn, Mo); double det = Ma*a1 - Mb*a2 + Mc*a3 - Md*a4; if (det == 0) { fprintf(stderr, "Unable to invert matrix with zero determinant"); return; } // Compute inverse matrix m[0][0] = (a1)/det; m[1][0] = -(a2)/det; m[2][0] = (a3)/det; m[3][0] = -(a4)/det; m[0][1] = -(R3MatrixDet3(Mb, Mc, Md, Mj, Mk, Ml, Mn, Mo, Mp))/det; m[1][1] = (R3MatrixDet3(Ma, Mc, Md, Mi, Mk, Ml, Mm, Mo, Mp))/det; m[2][1] = -(R3MatrixDet3(Ma, Mb, Md, Mi, Mj, Ml, Mm, Mn, Mp))/det; m[3][1] = (R3MatrixDet3(Ma, Mb, Mc, Mi, Mj, Mk, Mm, Mn, Mo))/det; m[0][2] = (R3MatrixDet3(Mb, Mc, Md, Mf, Mg, Mh, Mn, Mo, Mp))/det; m[1][2] = -(R3MatrixDet3(Ma, Mc, Md, Me, Mg, Mh, Mm, Mo, Mp))/det; m[2][2] = (R3MatrixDet3(Ma, Mb, Md, Me, Mf, Mh, Mm, Mn, Mp))/det; m[3][2] = -(R3MatrixDet3(Ma, Mb, Mc, Me, Mf, Mg, Mm, Mn, Mo))/det; m[0][3] = -(R3MatrixDet3(Mb, Mc, Md, Mf, Mg, Mh, Mj, Mk, Ml))/det; m[1][3] = (R3MatrixDet3(Ma, Mc, Md, Me, Mg, Mh, Mi, Mk, Ml))/det; m[2][3] = -(R3MatrixDet3(Ma, Mb, Md, Me, Mf, Mh, Mi, Mj, Ml))/det; m[3][3] = (R3MatrixDet3(Ma, Mb, Mc, Me, Mf, Mg, Mi, Mj, Mk))/det; }