Advertisement
Guest User

Matrix Inverse

a guest
Apr 22nd, 2012
274
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 3.63 KB | None | 0 0
  1. #include <stdio.h>
  2. #include <math.h>
  3.  
  4.  
  5. void printMatrix (float matrix[16]) {
  6.     printf("%5.1f %5.1f %5.1f %5.1f\n",matrix[0],matrix[4],matrix[8], matrix[12]);
  7.     printf("%5.1f %5.1f %5.1f %5.1f\n",matrix[1],matrix[5],matrix[9], matrix[13]);
  8.     printf("%5.1f %5.1f %5.1f %5.1f\n",matrix[2],matrix[6],matrix[10],matrix[14]);
  9.     printf("%5.1f %5.1f %5.1f %5.1f\n",matrix[3],matrix[7],matrix[11],matrix[15]);
  10. }
  11.  
  12. float lengthVector (float vector[]) {
  13.     float xSquared = vector[0] * vector[0];
  14.     float ySquared = vector[1] * vector[1];
  15.     float zSquared = vector[2] * vector[2];
  16.     return sqrt(xSquared + ySquared + zSquared);
  17. }
  18.  
  19. void createMatrix (float matrix[16], float pos[3], float rot[3], float scale[3]) {
  20.     matrix[12] = pos[0];
  21.     matrix[13] = pos[1];
  22.     matrix[14] = pos[2];
  23.     matrix[15] = 1.0;
  24.    
  25.     float A = cos(rot[1]), B = sin(rot[1]);
  26.     float C = cos(rot[0]), D = sin(rot[0]);
  27.     float E = cos(rot[2]), F = sin(rot[2]);
  28.    
  29.     matrix[0] = ( A*E + B*D*F) * scale[0];
  30.     matrix[1] = ( C*F        ) * scale[0];
  31.     matrix[2] = (-B*E + A*D*F) * scale[0];
  32.     matrix[3] = 0.0;
  33.    
  34.     matrix[4] = ( B*D*E - A*F) * scale[1];
  35.     matrix[5] = ( C*E        ) * scale[1];
  36.     matrix[6] = ( B*F + A*D*E) * scale[1];
  37.     matrix[7] = 0.0;
  38.    
  39.     matrix[8] = ( B*C        ) * scale[2];
  40.     matrix[9] = (-D          ) * scale[2];
  41.     matrix[10]= ( A*C        ) * scale[2];
  42.     matrix[11]= 0.0;
  43. }
  44.  
  45. void invertMatrix (float A[], float B[]) {
  46.     float sclX = lengthVector(&B[0]);
  47.     float sclY = lengthVector(&B[4]);
  48.     float sclZ = lengthVector(&B[8]);
  49.    
  50.     sclX *= sclX;
  51.     sclY *= sclY;
  52.     sclZ *= sclZ;
  53.    
  54.     if (sclX == 0) sclX = 0.00000001;
  55.     if (sclY == 0) sclY = 0.00000001;
  56.     if (sclZ == 0) sclZ = 0.00000001;
  57.    
  58.     A[0] = B[0]/sclX; A[4] = B[1]/sclX; A[8]  = B[2] /sclX;
  59.     A[1] = B[4]/sclY; A[5] = B[5]/sclY; A[9]  = B[6] /sclY;
  60.     A[2] = B[8]/sclZ; A[6] = B[9]/sclZ; A[10] = B[10]/sclZ;
  61.     A[3] = 0.0;       A[7] = 0.0;       A[11] = 0.0;
  62.    
  63.     float posX = -B[12],
  64.     posY = -B[13],
  65.     posZ = -B[14];
  66.    
  67.     A[12] = A[0]*posX + A[4]*posY + A[8] *posZ;
  68.     A[13] = A[1]*posX + A[5]*posY + A[9] *posZ;
  69.     A[14] = A[2]*posX + A[6]*posY + A[10]*posZ;
  70.     A[15] = 1.0;
  71. }
  72.  
  73. void multiplyMatrix (float A[], float B[], float C[]) {
  74.     A[0]  = B[0]*C[0]  + B[4]*C[1]  + B[8] *C[2]  + B[12]*C[3];
  75.     A[1]  = B[1]*C[0]  + B[5]*C[1]  + B[9] *C[2]  + B[13]*C[3];
  76.     A[2]  = B[2]*C[0]  + B[6]*C[1]  + B[10]*C[2]  + B[14]*C[3];
  77.     A[3]  = B[3]*C[0]  + B[7]*C[1]  + B[11]*C[2]  + B[15]*C[3];
  78.    
  79.     A[4]  = B[0]*C[4]  + B[4]*C[5]  + B[8] *C[6]  + B[12]*C[7];
  80.     A[5]  = B[1]*C[4]  + B[5]*C[5]  + B[9] *C[6]  + B[13]*C[7];
  81.     A[6]  = B[2]*C[4]  + B[6]*C[5]  + B[10]*C[6]  + B[14]*C[7];
  82.     A[7]  = B[3]*C[4]  + B[7]*C[5]  + B[11]*C[6]  + B[15]*C[7];
  83.    
  84.     A[8]  = B[0]*C[8]  + B[4]*C[9]  + B[8] *C[10] + B[12]*C[11];
  85.     A[9]  = B[1]*C[8]  + B[5]*C[9]  + B[9] *C[10] + B[13]*C[11];
  86.     A[10] = B[2]*C[8]  + B[6]*C[9]  + B[10]*C[10] + B[14]*C[11];
  87.     A[11] = B[3]*C[8]  + B[7]*C[9]  + B[11]*C[10] + B[15]*C[11];
  88.    
  89.     A[12] = B[0]*C[12] + B[4]*C[13] + B[8] *C[14] + B[12]*C[15];
  90.     A[13] = B[1]*C[12] + B[5]*C[13] + B[9] *C[14] + B[13]*C[15];
  91.     A[14] = B[2]*C[12] + B[6]*C[13] + B[10]*C[14] + B[14]*C[15];
  92.     A[15] = B[3]*C[12] + B[7]*C[13] + B[11]*C[14] + B[15]*C[15];
  93. }
  94.  
  95. int main () {
  96.     float matrix[16];
  97.     float inverse[16];
  98.     float identity[16];
  99.    
  100.     float position[3] = {0, 0, 0}; // X, Y, Z
  101.     float rotation[3] = {0, 0, 0}; // X, Y, Z
  102.     float scale[3]    = {1, 1, 3}; // X, Y, Z
  103.    
  104.     createMatrix(matrix,position,rotation,scale);
  105.     invertMatrix(inverse,matrix);
  106.     multiplyMatrix(identity,inverse,matrix);
  107.    
  108.     printf("Matrix\n");
  109.     printMatrix(matrix);
  110.    
  111.     printf("Inverse\n");
  112.     printMatrix(inverse);
  113.    
  114.     printf("Inverse * Matrix\n");
  115.     printMatrix(identity);
  116.    
  117.     return 0;
  118. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement