/** Matrix inverse assuming the array follows this index pattern. * [0 1 2] * [3 4 5] * [6 7 8] * @param x matrix to invert * @param y stores the result in y * @return true if the matrix was inverted. false if no solution. */ bool invert3x3matrix(const double *x, double *y) { double det = determinant3x3(x); if(fabs(det) < MY_EPSILON) return false; y[0] = determinant2x2(x[4], x[5], x[7], x[8]) / det; y[1] = determinant2x2(x[2], x[1], x[8], x[7]) / det; y[2] = determinant2x2(x[1], x[2], x[4], x[5]) / det; y[3] = determinant2x2(x[5], x[3], x[8], x[6]) / det; y[4] = determinant2x2(x[0], x[2], x[6], x[8]) / det; y[5] = determinant2x2(x[2], x[0], x[5], x[3]) / det; y[6] = determinant2x2(x[3], x[4], x[6], x[7]) / det; y[7] = determinant2x2(x[1], x[0], x[7], x[6]) / det; y[8] = determinant2x2(x[0], x[1], x[3], x[4]) / det; return true; }
/** Matrix inverse assuming the array follows this index pattern. * [0 1] * [2 3] * @return true if the matrix was inverted. false if no solution. */ bool invert2x2matrix(const double *input, double *output) { double det = determinant2x2(input[0], input[1], input[2], input[3]); if(fabs(det) < MY_EPSILON) return false; output[0] = input[3] / det; output[1] = -input[1] / det; output[2] = -input[2] / det; output[3] = input[0] / det; return true; }
double determinant(const Matrix& mat) { if(mat.getm()==1) return determinant1x1(mat); else if(mat.getm()==2) return determinant2x2(mat); else if(mat.getm()==3) return determinant3x3(mat); double det = 0; Matrix * subMat[mat.getm()]; for(int i=0; i<mat.getm(); i++) { subMat[i] = new Matrix(mat.getm()-1, mat.getm()-1); } for(int i=0; i<mat.getm(); i++) { for(int j=0; j<mat.getm()-1; j++) { for(int k=0, l=0; k<mat.getm()-1; k++,l++) { if(i==l) l++; (*(subMat[i]))[j][k]=mat[j+1][l]; } } } for(int i=0, sign=1; i<mat.getm(); i++) { det+=sign*mat[0][i]*determinant(*subMat[i]); if(sign==-1) sign=1; else sign=-1; } for(int i=0; i<mat.getm(); i++) { delete subMat[i]; } return det; }
static double determinant3x3(double a1, double a2, double a3, double b1, double b2, double b3, double c1, double c2, double c3) { return a1 * determinant2x2(b2, b3, c2, c3) - b1 * determinant2x2(a2, a3, c2, c3) + c1 * determinant2x2(a2, a3, b2, b3); }