Don't like ads? PRO users don't see any ads ;-)
Public Pastes
Guest

Matrix Inverse

By: a guest on Apr 22nd, 2012  |  syntax: C  |  size: 3.63 KB  |  hits: 43  |  expires: Never
download  |  raw  |  embed  |  report abuse  |  print
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
  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. }
create a new version of this paste RAW Paste Data