I have managed to implement a Gauss-Jordan elimination function and tested it in inverting a 6x6 matrix successfully. It's a lot faster than cofactors (which I wasted time on) and hopefully it'll help anyone who needs it and save you a lot of pain when dealing with 5 and 6 D.O.F constraint block matrices.
Also, it computes the determinant of the matrix early on in the process.
Here is some sample code, let me know if I can clarify anything, the process is straightforward once you get the hang of it.
Code: Select all
// 00 01 02 03 ...
// 10 11 12 13 ...
// 20 21 22 23 ...
// 30 31 32 33 ...
// . . . . ...
// . . . . ...
// . . . . ...
//
// [ A I ]
// AA^-1 = I
// Inverse is the property of a square matrix
if(rows!=cols)
return;
// Matrix must be 3x3 minimum
if(rows<3)
return;
MatrixRxC A = m;
MatrixRxC I(rows, cols);
I.identity();
Scalar sDeterminant = 1.0f;
// Forward pass, triangularize lower matrix
for(int i=0; i<cols-1; ++i) {
Scalar pivot = A.e[i][i];
for(int j=i+1; j<rows; ++j) {
Scalar factor;
if(A.e[j][i]<0 && A.e[i][i]<0)
factor = -fabsf(A.e[j][i]/pivot);
else if(A.e[j][i]<0 && A.e[i][i]>0)
factor = fabsf(A.e[j][i]/pivot);
else if(A.e[j][i]>0 && A.e[i][i]<0)
factor = fabsf(A.e[j][i]/pivot);
else if(A.e[j][i]>0 && A.e[i][i]>0)
factor = -fabsf(A.e[j][i]/pivot);
// Complete operation on entire row
for(int k=0; k<cols; ++k) {
A.e[j][k] = A.e[j][k] + factor*A.e[i][k];
I.e[j][k] = I.e[j][k] + factor*I.e[i][k];
}
}
}
// Check matrix determinant by reduction to triangular form
for(int i=0; i<rows; ++i) {
// Add the elements on the main diagonal to obtain the determinant
sDeterminant *= A.e[i][i];
}
if(sDeterminant==0.0f)
return; // Matrix has no inverse
// Identity pass, triangularize identity diagonal
for(int i=0; i<cols; ++i) {
Scalar divide = 1/A.e[i][i];
// Complete operation on entire row
for(int j=0; j<cols; ++j) {
A.e[i][j] = A.e[i][j] * divide;
I.e[i][j] = I.e[i][j] * divide;
}
}
// Backward pass, triangularize upper matrix
for(int i=cols-1; i>=1; --i) {
Scalar pivot = A.e[i][i];
for(int j=i-1; j>=0; --j) {
Scalar factor;
if(A.e[j][i]<0 && A.e[i][i]<0)
factor = -fabsf(A.e[j][i]/pivot);
else if(A.e[j][i]<0 && A.e[i][i]>0)
factor = fabsf(A.e[j][i]/pivot);
else if(A.e[j][i]>0 && A.e[i][i]<0)
factor = fabsf(A.e[j][i]/pivot);
else if(A.e[j][i]>0 && A.e[i][i]>0)
factor = -fabsf(A.e[j][i]/pivot);
// Complete operation on entire row
for(int k=0; k<cols; ++k) {
A.e[j][k] = A.e[j][k] + factor*A.e[i][k];
I.e[j][k] = I.e[j][k] + factor*I.e[i][k];
}
}
}
*this = I;
}