Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <stdio.h>
- #include <math.h>
- void printMatrix (float matrix[16]) {
- printf("%5.1f %5.1f %5.1f %5.1f\n",matrix[0],matrix[4],matrix[8], matrix[12]);
- printf("%5.1f %5.1f %5.1f %5.1f\n",matrix[1],matrix[5],matrix[9], matrix[13]);
- printf("%5.1f %5.1f %5.1f %5.1f\n",matrix[2],matrix[6],matrix[10],matrix[14]);
- printf("%5.1f %5.1f %5.1f %5.1f\n",matrix[3],matrix[7],matrix[11],matrix[15]);
- }
- float lengthVector (float vector[]) {
- float xSquared = vector[0] * vector[0];
- float ySquared = vector[1] * vector[1];
- float zSquared = vector[2] * vector[2];
- return sqrt(xSquared + ySquared + zSquared);
- }
- void createMatrix (float matrix[16], float pos[3], float rot[3], float scale[3]) {
- matrix[12] = pos[0];
- matrix[13] = pos[1];
- matrix[14] = pos[2];
- matrix[15] = 1.0;
- float A = cos(rot[1]), B = sin(rot[1]);
- float C = cos(rot[0]), D = sin(rot[0]);
- float E = cos(rot[2]), F = sin(rot[2]);
- matrix[0] = ( A*E + B*D*F) * scale[0];
- matrix[1] = ( C*F ) * scale[0];
- matrix[2] = (-B*E + A*D*F) * scale[0];
- matrix[3] = 0.0;
- matrix[4] = ( B*D*E - A*F) * scale[1];
- matrix[5] = ( C*E ) * scale[1];
- matrix[6] = ( B*F + A*D*E) * scale[1];
- matrix[7] = 0.0;
- matrix[8] = ( B*C ) * scale[2];
- matrix[9] = (-D ) * scale[2];
- matrix[10]= ( A*C ) * scale[2];
- matrix[11]= 0.0;
- }
- void invertMatrix (float A[], float B[]) {
- float sclX = lengthVector(&B[0]);
- float sclY = lengthVector(&B[4]);
- float sclZ = lengthVector(&B[8]);
- sclX *= sclX;
- sclY *= sclY;
- sclZ *= sclZ;
- if (sclX == 0) sclX = 0.00000001;
- if (sclY == 0) sclY = 0.00000001;
- if (sclZ == 0) sclZ = 0.00000001;
- A[0] = B[0]/sclX; A[4] = B[1]/sclX; A[8] = B[2] /sclX;
- A[1] = B[4]/sclY; A[5] = B[5]/sclY; A[9] = B[6] /sclY;
- A[2] = B[8]/sclZ; A[6] = B[9]/sclZ; A[10] = B[10]/sclZ;
- A[3] = 0.0; A[7] = 0.0; A[11] = 0.0;
- float posX = -B[12],
- posY = -B[13],
- posZ = -B[14];
- A[12] = A[0]*posX + A[4]*posY + A[8] *posZ;
- A[13] = A[1]*posX + A[5]*posY + A[9] *posZ;
- A[14] = A[2]*posX + A[6]*posY + A[10]*posZ;
- A[15] = 1.0;
- }
- void multiplyMatrix (float A[], float B[], float C[]) {
- A[0] = B[0]*C[0] + B[4]*C[1] + B[8] *C[2] + B[12]*C[3];
- A[1] = B[1]*C[0] + B[5]*C[1] + B[9] *C[2] + B[13]*C[3];
- A[2] = B[2]*C[0] + B[6]*C[1] + B[10]*C[2] + B[14]*C[3];
- A[3] = B[3]*C[0] + B[7]*C[1] + B[11]*C[2] + B[15]*C[3];
- A[4] = B[0]*C[4] + B[4]*C[5] + B[8] *C[6] + B[12]*C[7];
- A[5] = B[1]*C[4] + B[5]*C[5] + B[9] *C[6] + B[13]*C[7];
- A[6] = B[2]*C[4] + B[6]*C[5] + B[10]*C[6] + B[14]*C[7];
- A[7] = B[3]*C[4] + B[7]*C[5] + B[11]*C[6] + B[15]*C[7];
- A[8] = B[0]*C[8] + B[4]*C[9] + B[8] *C[10] + B[12]*C[11];
- A[9] = B[1]*C[8] + B[5]*C[9] + B[9] *C[10] + B[13]*C[11];
- A[10] = B[2]*C[8] + B[6]*C[9] + B[10]*C[10] + B[14]*C[11];
- A[11] = B[3]*C[8] + B[7]*C[9] + B[11]*C[10] + B[15]*C[11];
- A[12] = B[0]*C[12] + B[4]*C[13] + B[8] *C[14] + B[12]*C[15];
- A[13] = B[1]*C[12] + B[5]*C[13] + B[9] *C[14] + B[13]*C[15];
- A[14] = B[2]*C[12] + B[6]*C[13] + B[10]*C[14] + B[14]*C[15];
- A[15] = B[3]*C[12] + B[7]*C[13] + B[11]*C[14] + B[15]*C[15];
- }
- int main () {
- float matrix[16];
- float inverse[16];
- float identity[16];
- float position[3] = {0, 0, 0}; // X, Y, Z
- float rotation[3] = {0, 0, 0}; // X, Y, Z
- float scale[3] = {1, 1, 3}; // X, Y, Z
- createMatrix(matrix,position,rotation,scale);
- invertMatrix(inverse,matrix);
- multiplyMatrix(identity,inverse,matrix);
- printf("Matrix\n");
- printMatrix(matrix);
- printf("Inverse\n");
- printMatrix(inverse);
- printf("Inverse * Matrix\n");
- printMatrix(identity);
- return 0;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement