istereo
view libs/vmath/matrix_c.c @ 28:c0ae8e668447
added vmath library
| author | John Tsiombikas <nuclear@mutantstargoat.com> |
|---|---|
| date | Thu, 08 Sep 2011 08:30:42 +0300 |
| parents | |
| children | ff055bff6a15 |
line source
1 #include <stdio.h>
2 #include "matrix.h"
3 #include "vector.h"
4 #include "quat.h"
6 void m3_to_m4(mat4_t dest, mat3_t src)
7 {
8 int i, j;
10 memset(dest, 0, sizeof(mat4_t));
11 for(i=0; i<3; i++) {
12 for(j=0; j<3; j++) {
13 dest[i][j] = src[i][j];
14 }
15 }
16 dest[3][3] = 1.0;
17 }
19 void m3_print(FILE *fp, mat3_t m)
20 {
21 int i;
22 for(i=0; i<3; i++) {
23 fprintf(fp, "[ %12.5f %12.5f %12.5f ]\n", (float)m[i][0], (float)m[i][1], (float)m[i][2]);
24 }
25 }
27 /* C matrix 4x4 functions */
28 void m4_to_m3(mat3_t dest, mat4_t src)
29 {
30 int i, j;
31 for(i=0; i<3; i++) {
32 for(j=0; j<3; j++) {
33 dest[i][j] = src[i][j];
34 }
35 }
36 }
38 void m4_translate(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
39 {
40 mat4_t tm;
41 m4_identity(tm);
42 tm[0][3] = x;
43 tm[1][3] = y;
44 tm[2][3] = z;
45 m4_mult(m, m, tm);
46 }
48 void m4_rotate(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
49 {
50 m4_rotate_x(m, x);
51 m4_rotate_y(m, y);
52 m4_rotate_z(m, z);
53 }
55 void m4_rotate_x(mat4_t m, scalar_t angle)
56 {
57 mat4_t rm;
58 m4_identity(rm);
59 rm[1][1] = cos(angle); rm[1][2] = -sin(angle);
60 rm[2][1] = sin(angle); rm[2][2] = cos(angle);
61 m4_mult(m, m, rm);
62 }
64 void m4_rotate_y(mat4_t m, scalar_t angle)
65 {
66 mat4_t rm;
67 m4_identity(rm);
68 rm[0][0] = cos(angle); rm[0][2] = sin(angle);
69 rm[2][0] = -sin(angle); rm[2][2] = cos(angle);
70 m4_mult(m, m, rm);
71 }
73 void m4_rotate_z(mat4_t m, scalar_t angle)
74 {
75 mat4_t rm;
76 m4_identity(rm);
77 rm[0][0] = cos(angle); rm[0][1] = -sin(angle);
78 rm[1][0] = sin(angle); rm[1][1] = cos(angle);
79 m4_mult(m, m, rm);
80 }
82 void m4_rotate_axis(mat4_t m, scalar_t angle, scalar_t x, scalar_t y, scalar_t z)
83 {
84 mat4_t xform;
85 scalar_t sina = sin(angle);
86 scalar_t cosa = cos(angle);
87 scalar_t one_minus_cosa = 1.0 - cosa;
88 scalar_t nxsq = x * x;
89 scalar_t nysq = y * y;
90 scalar_t nzsq = z * z;
92 m4_identity(xform);
93 xform[0][0] = nxsq + (1.0 - nxsq) * cosa;
94 xform[0][1] = x * y * one_minus_cosa - z * sina;
95 xform[0][2] = x * z * one_minus_cosa + y * sina;
96 xform[1][0] = x * y * one_minus_cosa + z * sina;
97 xform[1][1] = nysq + (1.0 - nysq) * cosa;
98 xform[1][2] = y * z * one_minus_cosa - x * sina;
99 xform[2][0] = x * z * one_minus_cosa - y * sina;
100 xform[2][1] = y * z * one_minus_cosa + x * sina;
101 xform[2][2] = nzsq + (1.0 - nzsq) * cosa;
103 m4_mult(m, m, xform);
104 }
106 void m4_rotate_quat(mat4_t m, quat_t q)
107 {
108 mat4_t rm;
109 quat_to_mat4(rm, q);
110 m4_mult(m, m, rm);
111 }
113 void m4_scale(mat4_t m, scalar_t x, scalar_t y, scalar_t z)
114 {
115 mat4_t sm;
116 m4_identity(sm);
117 sm[0][0] = x;
118 sm[1][1] = y;
119 sm[2][2] = z;
120 m4_mult(m, m, sm);
121 }
123 void m4_transpose(mat4_t res, mat4_t m)
124 {
125 int i, j;
126 mat4_t tmp;
127 m4_copy(tmp, m);
129 for(i=0; i<4; i++) {
130 for(j=0; j<4; j++) {
131 res[i][j] = tmp[j][i];
132 }
133 }
134 }
136 scalar_t m4_determinant(mat4_t m)
137 {
138 scalar_t det11 = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
139 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
140 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
142 scalar_t det12 = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
143 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
144 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
146 scalar_t det13 = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
147 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
148 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
150 scalar_t det14 = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
151 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
152 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
154 return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
155 }
157 void m4_adjoint(mat4_t res, mat4_t m)
158 {
159 int i, j;
160 mat4_t coef;
162 coef[0][0] = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
163 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
164 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
165 coef[0][1] = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
166 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
167 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
168 coef[0][2] = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
169 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
170 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
171 coef[0][3] = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
172 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
173 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
175 coef[1][0] = (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
176 (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
177 (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
178 coef[1][1] = (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
179 (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
180 (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
181 coef[1][2] = (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
182 (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
183 (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
184 coef[1][3] = (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
185 (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
186 (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
188 coef[2][0] = (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
189 (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
190 (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
191 coef[2][1] = (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
192 (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
193 (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
194 coef[2][2] = (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
195 (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
196 (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
197 coef[2][3] = (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
198 (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
199 (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
201 coef[3][0] = (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
202 (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
203 (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
204 coef[3][1] = (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
205 (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
206 (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
207 coef[3][2] = (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
208 (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
209 (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
210 coef[3][3] = (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
211 (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
212 (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
214 m4_transpose(res, coef);
216 for(i=0; i<4; i++) {
217 for(j=0; j<4; j++) {
218 res[i][j] = j % 2 ? -res[i][j] : res[i][j];
219 if(i % 2) res[i][j] = -res[i][j];
220 }
221 }
222 }
224 void m4_inverse(mat4_t res, mat4_t m)
225 {
226 int i, j;
227 mat4_t adj;
228 scalar_t det;
230 m4_adjoint(adj, m);
231 det = m4_determinant(m);
233 for(i=0; i<4; i++) {
234 for(j=0; j<4; j++) {
235 res[i][j] = adj[i][j] / det;
236 }
237 }
238 }
240 void m4_print(FILE *fp, mat4_t m)
241 {
242 int i;
243 for(i=0; i<4; i++) {
244 fprintf(fp, "[ %12.5f %12.5f %12.5f %12.5f ]\n", (float)m[i][0], (float)m[i][1], (float)m[i][2], (float)m[i][3]);
245 }
246 }