01 #ifndef SHITSUCKER_HH
02 #define SHITSUCKER_HH 21
03 //https://github.com/eelstork/Matrix-Inversion/blob/master/4x4-matrix-inversion.c
04 F64 Ac(F64 *m,I64 i,I64 j,I64 a,I64 b) {
05   return m[ ((j+b)%4)*4 + ((i+a)%4) ];
06 } 
07 F64 invf(I64 i,I64 j,F64 * m){
08     I64 o = 2+(j-i);
09     i += 4+o;
10     j += 4-o;
11     F64 inv =
12       + Ac(m,i,j,+1,-1) * Ac(m,i,j,+0,+0) * Ac(m,i,j,-1,+1)
13       + Ac(m,i,j,+1,+1) * Ac(m,i,j,+0,-1) * Ac(m,i,j,-1,+0)
14       + Ac(m,i,j,-1,-1) * Ac(m,i,j,+1,+0) * Ac(m,i,j,+0,+1)
15       - Ac(m,i,j,-1,-1) * Ac(m,i,j,+0,+0) * Ac(m,i,j,+1,+1)
16       - Ac(m,i,j,-1,+1) * Ac(m,i,j,+0,-1) * Ac(m,i,j,+1,+0)
17       - Ac(m,i,j,+1,-1) * Ac(m,i,j,-1,+0) * Ac(m,i,j,+0,+1);
18     if(o%2)
19       return inv;
20     return -inv;
21 }
22 
23 Bool inverseMatrix4x4(F64 *m,F64 *out){
24     F64 inv[16];
25     I64 i,j;
26     for(i=0;i<4;i++) for(j=0;j<4;j++) inv[j*4+i] = invf(i,j,m);
27     F64 D = 0;
28     I64 k;
29     for(k=0;k<4;k++) D += m[k] * inv[k*4];
30     if (D == 0.) return FALSE;
31     D = 1.0 / D;
32     for (i = 0; i < 16; i++)
33         out[i] = inv[i] * D;
34     return TRUE;
35 }
36 Bool Mat4x4Inv(I64 *mat) {
37   I64 i;
38   F64 dumb[16],dumb2[16];
39   for(i=0;i!=16;++i)
40     dumb[i]=mat[i]/ToF64(GR_SCALE);
41   if(!inverseMatrix4x4(dumb,dumb2)) {
42     return FALSE;
43   }
44   for(i=0;i!=16;++i)
45     mat[i]=dumb2[i]*GR_SCALE;
46   return TRUE;
47 }
48 #endif