conworlds
view src/vr/mathutil.c @ 17:c814f77d177e
moved to SDL2
| author | John Tsiombikas <nuclear@member.fsf.org> |
|---|---|
| date | Mon, 25 Aug 2014 22:02:08 +0300 |
| parents | 778ed91cb7fd |
| children |
line source
1 #include <stdio.h>
2 #include <string.h>
3 #include "mathutil.h"
5 #define M(i, j) ((i) * 4 + (j))
7 static const float idmat[] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
9 void rotation_matrix(const float *quat, float *matrix)
10 {
11 matrix[0] = 1.0 - 2.0 * quat[1] * quat[1] - 2.0 * quat[2] * quat[2];
12 matrix[1] = 2.0 * quat[0] * quat[1] + 2.0 * quat[3] * quat[2];
13 matrix[2] = 2.0 * quat[2] * quat[0] - 2.0 * quat[3] * quat[1];
14 matrix[3] = 0.0f;
16 matrix[4] = 2.0 * quat[0] * quat[1] - 2.0 * quat[3] * quat[2];
17 matrix[5] = 1.0 - 2.0 * quat[0]*quat[0] - 2.0 * quat[2]*quat[2];
18 matrix[6] = 2.0 * quat[1] * quat[2] + 2.0 * quat[3] * quat[0];
19 matrix[7] = 0.0f;
21 matrix[8] = 2.0 * quat[2] * quat[0] + 2.0 * quat[3] * quat[1];
22 matrix[9] = 2.0 * quat[1] * quat[2] - 2.0 * quat[3] * quat[0];
23 matrix[10] = 1.0 - 2.0 * quat[0]*quat[0] - 2.0 * quat[1]*quat[1];
24 matrix[11] = 0.0f;
26 matrix[12] = matrix[13] = matrix[14] = 0.0f;
27 matrix[15] = 1.0f;
29 transpose_matrix(matrix);
30 }
32 void translation_matrix(const float *vec, float *matrix)
33 {
34 memcpy(matrix, idmat, sizeof idmat);
35 matrix[12] = vec[0];
36 matrix[13] = vec[1];
37 matrix[14] = vec[2];
38 }
40 void mult_matrix(float *dest, const float *m1, const float *m2)
41 {
42 int i, j;
43 float tmp[16];
45 for(i=0; i<4; i++) {
46 for(j=0; j<4; j++) {
47 tmp[M(i, j)] = m1[M(i, 0)] * m2[M(0, j)] + m1[M(i, 1)] * m2[M(1, j)] +
48 m1[M(i, 2)] * m2[M(2, j)] + m1[M(i, 3)] * m2[M(3, j)];
49 }
50 }
51 memcpy(dest, tmp, sizeof tmp);
52 }
54 void transpose_matrix(float *matrix)
55 {
56 int i, j;
57 float tmp[16];
59 for(i=0; i<4; i++) {
60 for(j=0; j<4; j++) {
61 tmp[M(i, j)] = matrix[M(j, i)];
62 }
63 }
64 memcpy(matrix, tmp, sizeof tmp);
65 }
67 void print_matrix(const float *matrix)
68 {
69 int i, j;
71 for(i=0; i<4; i++) {
72 putchar('[');
73 for(j=0; j<4; j++) {
74 printf("%6.3f ", matrix[M(i, j)]);
75 }
76 printf("]\n");
77 }
78 }