Inverting a matrix of any size

Please don't post Bullet support questions here, use the above forums instead.
Post Reply
c0der
Posts: 74
Joined: Sun Jul 08, 2012 11:32 am

Inverting a matrix of any size

Post by c0der »

Hi,

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;
}
Dirk Gregorius
Posts: 861
Joined: Sun Jul 03, 2005 4:06 pm
Location: Kirkland, WA

Re: Inverting a matrix of any size

Post by Dirk Gregorius »

I assume you a trying to build the effective mass block matrices. Note that these are symmetric and positive definite. So another option would be Cholesky decomposition. For small matrices I would still use cofactor expansion and since the matrix is symmetric it can be optimized a bit more. Look at the Doom 3 SDK. It has some good examples.
Post Reply