Check out the Pastebin Gadgets Shop. We have thousands of fun, geeky & affordable gadgets on sale :-)Want more features on Pastebin? Sign Up, it's FREE!

# Matrix Inverse

By: a guest on Apr 22nd, 2012  |  syntax: C  |  size: 3.63 KB  |  views: 60  |  expires: Never
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. }
clone this paste RAW Paste Data
Top