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;
}
