static double determinant4x4(const TransformationMatrix::Matrix4& m) { // Assign to individual variable names to aid selecting // correct elements double a1 = m[0][0]; double b1 = m[0][1]; double c1 = m[0][2]; double d1 = m[0][3]; double a2 = m[1][0]; double b2 = m[1][1]; double c2 = m[1][2]; double d2 = m[1][3]; double a3 = m[2][0]; double b3 = m[2][1]; double c3 = m[2][2]; double d3 = m[2][3]; double a4 = m[3][0]; double b4 = m[3][1]; double c4 = m[3][2]; double d4 = m[3][3]; return a1 * determinant3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4) - b1 * determinant3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4) + c1 * determinant3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4) - d1 * determinant3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4); }
static inline double determinant(const Matrix& matrix) { const Matrix::MatrixData & m = matrix.m_matrix; const double a1 = m[0][0]; const double b1 = m[0][1]; const double c1 = m[0][2]; const double d1 = m[0][3]; const double a2 = m[1][0]; const double b2 = m[1][1]; const double c2 = m[1][2]; const double d2 = m[1][3]; const double a3 = m[2][0]; const double b3 = m[2][1]; const double c3 = m[2][2]; const double d3 = m[2][3]; const double a4 = m[3][0]; const double b4 = m[3][1]; const double c4 = m[3][2]; const double d4 = m[3][3]; return a1 * determinant3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4) - b1 * determinant3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4) + c1 * determinant3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4) - d1 * determinant3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4); }
/** 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; }
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 void adjoint(const TransformationMatrix::Matrix4& matrix, TransformationMatrix::Matrix4& result) { // Assign to individual variable names to aid // selecting correct values double a1 = matrix[0][0]; double b1 = matrix[0][1]; double c1 = matrix[0][2]; double d1 = matrix[0][3]; double a2 = matrix[1][0]; double b2 = matrix[1][1]; double c2 = matrix[1][2]; double d2 = matrix[1][3]; double a3 = matrix[2][0]; double b3 = matrix[2][1]; double c3 = matrix[2][2]; double d3 = matrix[2][3]; double a4 = matrix[3][0]; double b4 = matrix[3][1]; double c4 = matrix[3][2]; double d4 = matrix[3][3]; // Row column labeling reversed since we transpose rows & columns result[0][0] = determinant3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4); result[1][0] = - determinant3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4); result[2][0] = determinant3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4); result[3][0] = - determinant3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4); result[0][1] = - determinant3x3(b1, b3, b4, c1, c3, c4, d1, d3, d4); result[1][1] = determinant3x3(a1, a3, a4, c1, c3, c4, d1, d3, d4); result[2][1] = - determinant3x3(a1, a3, a4, b1, b3, b4, d1, d3, d4); result[3][1] = determinant3x3(a1, a3, a4, b1, b3, b4, c1, c3, c4); result[0][2] = determinant3x3(b1, b2, b4, c1, c2, c4, d1, d2, d4); result[1][2] = - determinant3x3(a1, a2, a4, c1, c2, c4, d1, d2, d4); result[2][2] = determinant3x3(a1, a2, a4, b1, b2, b4, d1, d2, d4); result[3][2] = - determinant3x3(a1, a2, a4, b1, b2, b4, c1, c2, c4); result[0][3] = - determinant3x3(b1, b2, b3, c1, c2, c3, d1, d2, d3); result[1][3] = determinant3x3(a1, a2, a3, c1, c2, c3, d1, d2, d3); result[2][3] = - determinant3x3(a1, a2, a3, b1, b2, b3, d1, d2, d3); result[3][3] = determinant3x3(a1, a2, a3, b1, b2, b3, c1, c2, c3); }