4x4 matrix inversion

Question

I am looking for an example implementation of code on how to invert a 4x4 matrix. I know that there is gaussian eleminiation, LU decomposition, etc., but instead of looking at them in detail, I'm really looking for code for this.

the language is perfect C ++, data is available in an array of 16 floats in the main order.

Thank you!

Answer 1

here:

bool gluInvertMatrix(const double m[16], double invOut[16]) { double inv[16], det; int i; inv[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] + m[9] * m[7] * m[14] + m[13] * m[6] * m[11] - m[13] * m[7] * m[10]; inv[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] - m[8] * m[7] * m[14] - m[12] * m[6] * m[11] + m[12] * m[7] * m[10]; inv[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] + m[8] * m[7] * m[13] + m[12] * m[5] * m[11] - m[12] * m[7] * m[9]; inv[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] - m[8] * m[6] * m[13] - m[12] * m[5] * m[10] + m[12] * m[6] * m[9]; inv[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] - m[9] * m[3] * m[14] - m[13] * m[2] * m[11] + m[13] * m[3] * m[10]; inv[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] + m[8] * m[3] * m[14] + m[12] * m[2] * m[11] - m[12] * m[3] * m[10]; inv[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] - m[8] * m[3] * m[13] - m[12] * m[1] * m[11] + m[12] * m[3] * m[9]; inv[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] + m[8] * m[2] * m[13] + m[12] * m[1] * m[10] - m[12] * m[2] * m[9]; inv[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] + m[5] * m[3] * m[14] + m[13] * m[2] * m[7] - m[13] * m[3] * m[6]; inv[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] - m[4] * m[3] * m[14] - m[12] * m[2] * m[7] + m[12] * m[3] * m[6]; inv[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] + m[4] * m[3] * m[13] + m[12] * m[1] * m[7] - m[12] * m[3] * m[5]; inv[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] - m[4] * m[2] * m[13] - m[12] * m[1] * m[6] + m[12] * m[2] * m[5]; inv[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] - m[5] * m[3] * m[10] - m[9] * m[2] * m[7] + m[9] * m[3] * m[6]; inv[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] + m[4] * m[3] * m[10] + m[8] * m[2] * m[7] - m[8] * m[3] * m[6]; inv[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] - m[4] * m[3] * m[9] - m[8] * m[1] * m[7] + m[8] * m[3] * m[5]; inv[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] + m[4] * m[2] * m[9] + m[8] * m[1] * m[6] - m[8] * m[2] * m[5]; det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12]; if (det == 0) return false; det = 1.0 / det; for (i = 0; i < 16; i++) invOut[i] = inv[i] * det; return true; } 

This was removed from the MESA GLU library implementation.

Answer 2

If you are looking for a “just works” implementation that is also very optimized without having to understand the code, I highly recommend using the Intel optimized SSE described here . There is also a reference to immersion for both Gaussian elimination and the Cramer rule in C.

I warn you that SSE code is not very good if you do not understand the built-in MMX / SSE functions.

Answer 3

If you need a C ++ matrix library with many features, look at the Eigen library - http://eigen.tuxfamily.org

Answer 4

I rolled up the MESA implementation (also wrote some unit tests to make sure this really works).

Here:

 float invf(int i,int j,const float* m){ int o = 2+(ji); i += 4+o; j += 4-o; #define e(a,b) m[ ((j+b)%4)*4 + ((i+a)%4) ] float inv = + e(+1,-1)*e(+0,+0)*e(-1,+1) + e(+1,+1)*e(+0,-1)*e(-1,+0) + e(-1,-1)*e(+1,+0)*e(+0,+1) - e(-1,-1)*e(+0,+0)*e(+1,+1) - e(-1,+1)*e(+0,-1)*e(+1,+0) - e(+1,-1)*e(-1,+0)*e(+0,+1); return (o%2)?inv : -inv; #undef e } bool inverseMatrix4x4(const float *m, float *out) { float inv[16]; for(int i=0;i<4;i++) for(int j=0;j<4;j++) inv[j*4+i] = invf(i,j,m); double D = 0; for(int k=0;k<4;k++) D += m[k] * inv[k*4]; if (D == 0) return false; D = 1.0 / D; for (int i = 0; i < 16; i++) out[i] = inv[i] * D; return true; } 

I wrote a little about it and will show a template of positive / negative factors in my blog .

As suggested by @LiraNuna, hardware accelerated versions of such routines are available on many platforms, so I'm glad to have a “backup version” that is readable and concise.

Note : this may work 3.5 times slower or worse than the MESA implementation. You can change the pattern of factors to remove some additions, etc., but it would lose readability and still not be very fast.

Answer 5

You can use the GNU Science Library or see the code in it.

Edit: You seem to need Linear Algebra .

Answer 6

Here is a small (only one heading) C ++ vector math (focused on 3D programming). If you use it, keep in mind that the layout of its matrices in memory is inverted compared to what OpenGL expects, I had fun when it turned out ...

Answer 7

You can do it faster according to this.

 #define SUBP(i,j) input[i][j] #define SUBQ(i,j) input[i][2+j] #define SUBR(i,j) input[2+i][j] #define SUBS(i,j) input[2+i][2+j] #define OUTP(i,j) output[i][j] #define OUTQ(i,j) output[i][2+j] #define OUTR(i,j) output[2+i][j] #define OUTS(i,j) output[2+i][2+j] #define INVP(i,j) invP[i][j] #define INVPQ(i,j) invPQ[i][j] #define RINVP(i,j) RinvP[i][j] #define INVPQ(i,j) invPQ[i][j] #define RINVPQ(i,j) RinvPQ[i][j] #define INVPQR(i,j) invPQR[i][j] #define INVS(i,j) invS[i][j] #define MULTI(MAT1, MAT2, MAT3) \ MAT3(0,0)=MAT1(0,0)*MAT2(0,0) + MAT1(0,1)*MAT2(1,0); \ MAT3(0,1)=MAT1(0,0)*MAT2(0,1) + MAT1(0,1)*MAT2(1,1); \ MAT3(1,0)=MAT1(1,0)*MAT2(0,0) + MAT1(1,1)*MAT2(1,0); \ MAT3(1,1)=MAT1(1,0)*MAT2(0,1) + MAT1(1,1)*MAT2(1,1); #define INV(MAT1, MAT2) \ _det = 1.0 / (MAT1(0,0) * MAT1(1,1) - MAT1(0,1) * MAT1(1,0)); \ MAT2(0,0) = MAT1(1,1) * _det; \ MAT2(1,1) = MAT1(0,0) * _det; \ MAT2(0,1) = -MAT1(0,1) * _det; \ MAT2(1,0) = -MAT1(1,0) * _det; \ #define SUBTRACT(MAT1, MAT2, MAT3) \ MAT3(0,0)=MAT1(0,0) - MAT2(0,0); \ MAT3(0,1)=MAT1(0,1) - MAT2(0,1); \ MAT3(1,0)=MAT1(1,0) - MAT2(1,0); \ MAT3(1,1)=MAT1(1,1) - MAT2(1,1); #define NEGATIVE(MAT) \ MAT(0,0)=-MAT(0,0); \ MAT(0,1)=-MAT(0,1); \ MAT(1,0)=-MAT(1,0); \ MAT(1,1)=-MAT(1,1); void getInvertMatrix(complex<double> input[4][4], complex<double> output[4][4]) { complex<double> _det; complex<double> invP[2][2]; complex<double> invPQ[2][2]; complex<double> RinvP[2][2]; complex<double> RinvPQ[2][2]; complex<double> invPQR[2][2]; complex<double> invS[2][2]; INV(SUBP, INVP); MULTI(SUBR, INVP, RINVP); MULTI(INVP, SUBQ, INVPQ); MULTI(RINVP, SUBQ, RINVPQ); SUBTRACT(SUBS, RINVPQ, INVS); INV(INVS, OUTS); NEGATIVE(OUTS); MULTI(OUTS, RINVP, OUTR); MULTI(INVPQ, OUTS, OUTQ); MULTI(INVPQ, OUTR, INVPQR); SUBTRACT(INVP, INVPQR, OUTP); } 

This is not a complete implementation, because P cannot be reversible, but you can combine this code with the MESA implementation to get better performance.

Answer 8

If someone is looking for a more costumed code and “easier to read”, then I got this

 var A2323 = m.m22 * m.m33 - m.m23 * m.m32 ; var A1323 = m.m21 * m.m33 - m.m23 * m.m31 ; var A1223 = m.m21 * m.m32 - m.m22 * m.m31 ; var A0323 = m.m20 * m.m33 - m.m23 * m.m30 ; var A0223 = m.m20 * m.m32 - m.m22 * m.m30 ; var A0123 = m.m20 * m.m31 - m.m21 * m.m30 ; var A2313 = m.m12 * m.m33 - m.m13 * m.m32 ; var A1313 = m.m11 * m.m33 - m.m13 * m.m31 ; var A1213 = m.m11 * m.m32 - m.m12 * m.m31 ; var A2312 = m.m12 * m.m23 - m.m13 * m.m22 ; var A1312 = m.m11 * m.m23 - m.m13 * m.m21 ; var A1212 = m.m11 * m.m22 - m.m12 * m.m21 ; var A0313 = m.m10 * m.m33 - m.m13 * m.m30 ; var A0213 = m.m10 * m.m32 - m.m12 * m.m30 ; var A0312 = m.m10 * m.m23 - m.m13 * m.m20 ; var A0212 = m.m10 * m.m22 - m.m12 * m.m20 ; var A0113 = m.m10 * m.m31 - m.m11 * m.m30 ; var A0112 = m.m10 * m.m21 - m.m11 * m.m20 ; var det = m.m00 * ( m.m11 * A2323 - m.m12 * A1323 + m.m13 * A1223 ) - m.m01 * ( m.m10 * A2323 - m.m12 * A0323 + m.m13 * A0223 ) + m.m02 * ( m.m10 * A1323 - m.m11 * A0323 + m.m13 * A0123 ) - m.m03 * ( m.m10 * A1223 - m.m11 * A0223 + m.m12 * A0123 ) ; det = 1 / det; return new Matrix4x4() { m00 = det * ( m.m11 * A2323 - m.m12 * A1323 + m.m13 * A1223 ), m01 = det * - ( m.m01 * A2323 - m.m02 * A1323 + m.m03 * A1223 ), m02 = det * ( m.m01 * A2313 - m.m02 * A1313 + m.m03 * A1213 ), m03 = det * - ( m.m01 * A2312 - m.m02 * A1312 + m.m03 * A1212 ), m10 = det * - ( m.m10 * A2323 - m.m12 * A0323 + m.m13 * A0223 ), m11 = det * ( m.m00 * A2323 - m.m02 * A0323 + m.m03 * A0223 ), m12 = det * - ( m.m00 * A2313 - m.m02 * A0313 + m.m03 * A0213 ), m13 = det * ( m.m00 * A2312 - m.m02 * A0312 + m.m03 * A0212 ), m20 = det * ( m.m10 * A1323 - m.m11 * A0323 + m.m13 * A0123 ), m21 = det * - ( m.m00 * A1323 - m.m01 * A0323 + m.m03 * A0123 ), m22 = det * ( m.m00 * A1313 - m.m01 * A0313 + m.m03 * A0113 ), m23 = det * - ( m.m00 * A1312 - m.m01 * A0312 + m.m03 * A0112 ), m30 = det * - ( m.m10 * A1223 - m.m11 * A0223 + m.m12 * A0123 ), m31 = det * ( m.m00 * A1223 - m.m01 * A0223 + m.m02 * A0123 ), m32 = det * - ( m.m00 * A1213 - m.m01 * A0213 + m.m02 * A0113 ), m33 = det * ( m.m00 * A1212 - m.m01 * A0212 + m.m02 * A0112 ), }; 

I do not write code, but my program. I made a small program to create a program that calculates the determinant and inverts any N-matrix.

I do this because in the past I needed code that inverts a 5x5 matrix, but no one on earth did this, so I did it.

Check out the program here .

Answer 9

FOR 3x3 MATRIX

CHANGE OF CODE ACCORDING TO YOUR REQUIREMENT

http://www.dreamincode.net/code/snippet1156.htm

Update:

Yes ... Moving from 3x3 to 4x4 seems like a big difference ... this answer is incorrect for that.