merged matrix4x4 and matrix3x4 into matrixlib, updated makefiles and dsp, some cleanu...
[divverent/darkplaces.git] / matrixlib.c
1
2 #include "matrixlib.h"
3 #include <math.h>
4
5 void Matrix4x4_Copy (matrix4x4_t *out, matrix4x4_t *in)
6 {
7         *out = *in;
8 }
9
10 void Matrix4x4_CopyRotateOnly (matrix4x4_t *out, matrix4x4_t *in)
11 {
12         out->m[0][0] = in->m[0][0];
13         out->m[0][1] = in->m[0][1];
14         out->m[0][2] = in->m[0][2];
15         out->m[0][3] = 0.0f;
16         out->m[1][0] = in->m[1][0];
17         out->m[1][1] = in->m[1][1];
18         out->m[1][2] = in->m[1][2];
19         out->m[1][3] = 0.0f;
20         out->m[2][0] = in->m[2][0];
21         out->m[2][1] = in->m[2][1];
22         out->m[2][2] = in->m[2][2];
23         out->m[2][3] = 0.0f;
24         out->m[3][0] = 0.0f;
25         out->m[3][1] = 0.0f;
26         out->m[3][2] = 0.0f;
27         out->m[3][3] = 1.0f;
28 }
29
30 void Matrix4x4_CopyTranslateOnly (matrix4x4_t *out, matrix4x4_t *in)
31 {
32         out->m[0][0] = 0.0f;
33         out->m[0][1] = 0.0f;
34         out->m[0][2] = 0.0f;
35         out->m[0][3] = in->m[0][3];
36         out->m[1][0] = 0.0f;
37         out->m[1][1] = 0.0f;
38         out->m[1][2] = 0.0f;
39         out->m[1][3] = in->m[0][3];
40         out->m[2][0] = 0.0f;
41         out->m[2][1] = 0.0f;
42         out->m[2][2] = 0.0f;
43         out->m[2][3] = in->m[0][3];
44         out->m[3][0] = 0.0f;
45         out->m[3][1] = 0.0f;
46         out->m[3][2] = 0.0f;
47         out->m[3][3] = 1.0f;
48 }
49
50 void Matrix4x4_FromMatrix3x4 (matrix4x4_t *out, matrix3x4_t *in)
51 {
52         out->m[0][0] = in->m[0][0];
53         out->m[0][1] = in->m[0][1];
54         out->m[0][2] = in->m[0][2];
55         out->m[0][3] = in->m[0][3];
56         out->m[1][0] = in->m[1][0];
57         out->m[1][1] = in->m[1][1];
58         out->m[1][2] = in->m[1][2];
59         out->m[1][3] = in->m[1][3];
60         out->m[2][0] = in->m[2][0];
61         out->m[2][1] = in->m[2][1];
62         out->m[2][2] = in->m[2][2];
63         out->m[2][3] = in->m[2][3];
64         out->m[3][0] = 0.0f;
65         out->m[3][1] = 0.0f;
66         out->m[3][2] = 0.0f;
67         out->m[3][3] = 1.0f;
68 }
69
70 void Matrix4x4_Concat (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2)
71 {
72         out->m[0][0] = in1->m[0][0] * in2->m[0][0] + in1->m[0][1] * in2->m[1][0] + in1->m[0][2] * in2->m[2][0] + in1->m[0][3] * in2->m[3][0];
73         out->m[0][1] = in1->m[0][0] * in2->m[0][1] + in1->m[0][1] * in2->m[1][1] + in1->m[0][2] * in2->m[2][1] + in1->m[0][3] * in2->m[3][1];
74         out->m[0][2] = in1->m[0][0] * in2->m[0][2] + in1->m[0][1] * in2->m[1][2] + in1->m[0][2] * in2->m[2][2] + in1->m[0][3] * in2->m[3][2];
75         out->m[0][3] = in1->m[0][0] * in2->m[0][3] + in1->m[0][1] * in2->m[1][3] + in1->m[0][2] * in2->m[2][3] + in1->m[0][3] * in2->m[3][3];
76         out->m[1][0] = in1->m[1][0] * in2->m[0][0] + in1->m[1][1] * in2->m[1][0] + in1->m[1][2] * in2->m[2][0] + in1->m[1][3] * in2->m[3][0];
77         out->m[1][1] = in1->m[1][0] * in2->m[0][1] + in1->m[1][1] * in2->m[1][1] + in1->m[1][2] * in2->m[2][1] + in1->m[1][3] * in2->m[3][1];
78         out->m[1][2] = in1->m[1][0] * in2->m[0][2] + in1->m[1][1] * in2->m[1][2] + in1->m[1][2] * in2->m[2][2] + in1->m[1][3] * in2->m[3][2];
79         out->m[1][3] = in1->m[1][0] * in2->m[0][3] + in1->m[1][1] * in2->m[1][3] + in1->m[1][2] * in2->m[2][3] + in1->m[1][3] * in2->m[3][3];
80         out->m[2][0] = in1->m[2][0] * in2->m[0][0] + in1->m[2][1] * in2->m[1][0] + in1->m[2][2] * in2->m[2][0] + in1->m[2][3] * in2->m[3][0];
81         out->m[2][1] = in1->m[2][0] * in2->m[0][1] + in1->m[2][1] * in2->m[1][1] + in1->m[2][2] * in2->m[2][1] + in1->m[2][3] * in2->m[3][1];
82         out->m[2][2] = in1->m[2][0] * in2->m[0][2] + in1->m[2][1] * in2->m[1][2] + in1->m[2][2] * in2->m[2][2] + in1->m[2][3] * in2->m[3][2];
83         out->m[2][3] = in1->m[2][0] * in2->m[0][3] + in1->m[2][1] * in2->m[1][3] + in1->m[2][2] * in2->m[2][3] + in1->m[2][3] * in2->m[3][3];
84         out->m[3][0] = in1->m[3][0] * in2->m[0][0] + in1->m[3][1] * in2->m[1][0] + in1->m[3][2] * in2->m[2][0] + in1->m[3][3] * in2->m[3][0];
85         out->m[3][1] = in1->m[3][0] * in2->m[0][1] + in1->m[3][1] * in2->m[1][1] + in1->m[3][2] * in2->m[2][1] + in1->m[3][3] * in2->m[3][1];
86         out->m[3][2] = in1->m[3][0] * in2->m[0][2] + in1->m[3][1] * in2->m[1][2] + in1->m[3][2] * in2->m[2][2] + in1->m[3][3] * in2->m[3][2];
87         out->m[3][3] = in1->m[3][0] * in2->m[0][3] + in1->m[3][1] * in2->m[1][3] + in1->m[3][2] * in2->m[2][3] + in1->m[3][3] * in2->m[3][3];
88 }
89
90 void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1)
91 {
92         float scale;
93         scale = 3.0 / (in1->m[0][0] * in1->m[0][0]
94                      + in1->m[0][1] * in1->m[0][1]
95                      + in1->m[0][2] * in1->m[0][2]
96                      + in1->m[1][0] * in1->m[1][0]
97                      + in1->m[1][1] * in1->m[1][1]
98                      + in1->m[1][2] * in1->m[1][2]
99                      + in1->m[2][0] * in1->m[2][0]
100                      + in1->m[2][1] * in1->m[2][1]
101                      + in1->m[2][2] * in1->m[2][2]);
102         out->m[0][0] = in1->m[0][0] * scale;
103         out->m[0][1] = in1->m[1][0] * scale;
104         out->m[0][2] = in1->m[2][0] * scale;
105         out->m[0][3] = in1->m[3][0];
106         out->m[1][0] = in1->m[0][1] * scale;
107         out->m[1][1] = in1->m[1][1] * scale;
108         out->m[1][2] = in1->m[2][1] * scale;
109         out->m[1][3] = in1->m[3][1];
110         out->m[2][0] = in1->m[0][2] * scale;
111         out->m[2][1] = in1->m[1][2] * scale;
112         out->m[2][2] = in1->m[2][2] * scale;
113         out->m[2][3] = in1->m[3][2];
114         out->m[3][0] = in1->m[0][3];
115         out->m[3][1] = in1->m[1][3];
116         out->m[3][2] = in1->m[2][3];
117         out->m[3][3] = in1->m[3][3];
118 }
119
120 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
121 {
122         out->m[0][0]=1.0f;
123         out->m[0][1]=0.0f;
124         out->m[0][2]=0.0f;
125         out->m[0][3]=0.0f;
126         out->m[1][0]=0.0f;
127         out->m[1][1]=1.0f;
128         out->m[1][2]=0.0f;
129         out->m[1][3]=0.0f;
130         out->m[2][0]=0.0f;
131         out->m[2][1]=0.0f;
132         out->m[2][2]=1.0f;
133         out->m[2][3]=0.0f;
134         out->m[3][0]=0.0f;
135         out->m[3][1]=0.0f;
136         out->m[3][2]=0.0f;
137         out->m[3][3]=1.0f;
138 }
139
140 void Matrix4x4_CreateTranslate (matrix4x4_t *out, float x, float y, float z)
141 {
142         out->m[0][0]=1.0f;
143         out->m[0][1]=0.0f;
144         out->m[0][2]=0.0f;
145         out->m[0][3]=x;
146         out->m[1][0]=0.0f;
147         out->m[1][1]=1.0f;
148         out->m[1][2]=0.0f;
149         out->m[1][3]=y;
150         out->m[2][0]=0.0f;
151         out->m[2][1]=0.0f;
152         out->m[2][2]=1.0f;
153         out->m[2][3]=z;
154         out->m[3][0]=0.0f;
155         out->m[3][1]=0.0f;
156         out->m[3][2]=0.0f;
157         out->m[3][3]=1.0f;
158 }
159
160 void Matrix4x4_CreateRotate (matrix4x4_t *out, float angle, float x, float y, float z)
161 {
162         float len, c, s;
163
164         len = x*x+y*y+z*z;
165         if (len != 0.0f)
166                 len = 1.0f / sqrt(len);
167         x *= len;
168         y *= len;
169         z *= len;
170
171         angle *= M_PI / 180.0;
172         c = cos(angle);
173         s = sin(angle);
174
175         out->m[0][0]=x * x + c * (1 - x * x);
176         out->m[0][1]=x * y * (1 - c) + z * s;
177         out->m[0][2]=z * x * (1 - c) - y * s;
178         out->m[0][3]=0.0f;
179         out->m[1][0]=x * y * (1 - c) - z * s;
180         out->m[1][1]=y * y + c * (1 - y * y);
181         out->m[1][2]=y * z * (1 - c) + x * s;
182         out->m[1][3]=0.0f;
183         out->m[2][0]=z * x * (1 - c) + y * s;
184         out->m[2][1]=y * z * (1 - c) - x * s;
185         out->m[2][2]=z * z + c * (1 - z * z);
186         out->m[2][3]=0.0f;
187         out->m[3][0]=0.0f;
188         out->m[3][1]=0.0f;
189         out->m[3][2]=0.0f;
190         out->m[3][3]=1.0f;
191 }
192
193 void Matrix4x4_CreateScale (matrix4x4_t *out, float x)
194 {
195         out->m[0][0]=x;
196         out->m[0][1]=0.0f;
197         out->m[0][2]=0.0f;
198         out->m[0][3]=0.0f;
199         out->m[1][0]=0.0f;
200         out->m[1][1]=x;
201         out->m[1][2]=0.0f;
202         out->m[1][3]=0.0f;
203         out->m[2][0]=0.0f;
204         out->m[2][1]=0.0f;
205         out->m[2][2]=x;
206         out->m[2][3]=0.0f;
207         out->m[3][0]=0.0f;
208         out->m[3][1]=0.0f;
209         out->m[3][2]=0.0f;
210         out->m[3][3]=1.0f;
211 }
212
213 void Matrix4x4_CreateScale3 (matrix4x4_t *out, float x, float y, float z)
214 {
215         out->m[0][0]=x;
216         out->m[0][1]=0.0f;
217         out->m[0][2]=0.0f;
218         out->m[0][3]=0.0f;
219         out->m[1][0]=0.0f;
220         out->m[1][1]=y;
221         out->m[1][2]=0.0f;
222         out->m[1][3]=0.0f;
223         out->m[2][0]=0.0f;
224         out->m[2][1]=0.0f;
225         out->m[2][2]=z;
226         out->m[2][3]=0.0f;
227         out->m[3][0]=0.0f;
228         out->m[3][1]=0.0f;
229         out->m[3][2]=0.0f;
230         out->m[3][3]=1.0f;
231 }
232
233 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
234 {
235         vx[0] = in->m[0][0];
236         vx[1] = in->m[1][0];
237         vx[2] = in->m[2][0];
238         vy[0] = in->m[0][1];
239         vy[1] = in->m[1][1];
240         vy[2] = in->m[2][1];
241         vz[0] = in->m[0][2];
242         vz[1] = in->m[1][2];
243         vz[2] = in->m[2][2];
244         t[0] = in->m[0][3];
245         t[1] = in->m[1][3];
246         t[2] = in->m[2][3];
247 }
248
249 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
250 {
251         out->m[0][0] = vx[0];
252         out->m[0][1] = vy[0];
253         out->m[0][2] = vz[0];
254         out->m[0][3] = t[0];
255         out->m[1][0] = vx[1];
256         out->m[1][1] = vy[1];
257         out->m[1][2] = vz[1];
258         out->m[1][3] = t[1];
259         out->m[2][0] = vx[2];
260         out->m[2][1] = vy[2];
261         out->m[2][2] = vz[2];
262         out->m[2][3] = t[2];
263         out->m[3][0] = 0.0f;
264         out->m[3][1] = 0.0f;
265         out->m[3][2] = 0.0f;
266         out->m[3][3] = 1.0f;
267 }
268
269 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
270 {
271         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
272         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
273         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
274 }
275
276 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
277 {
278         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + v[3] * in->m[0][3];
279         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + v[3] * in->m[1][3];
280         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + v[3] * in->m[2][3];
281         out[3] = v[0] * in->m[3][0] + v[1] * in->m[3][1] + v[2] * in->m[3][2] + v[3] * in->m[3][3];
282 }
283
284 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
285 {
286         float t[3];
287         t[0] = v[0] - in->m[0][3];
288         t[1] = v[1] - in->m[1][3];
289         t[2] = v[2] - in->m[2][3];
290         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
291         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
292         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
293 }
294
295 // FIXME: optimize
296 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, float x, float y, float z)
297 {
298         matrix4x4_t base, temp;
299         Matrix4x4_Copy(out, &base);
300         Matrix4x4_CreateTranslate(&temp, x, y, z);
301         Matrix4x4_Concat(out, &base, &temp);
302 }
303
304 // FIXME: optimize
305 void Matrix4x4_ConcatRotate (matrix4x4_t *out, float angle, float x, float y, float z)
306 {
307         matrix4x4_t base, temp;
308         Matrix4x4_Copy(out, &base);
309         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
310         Matrix4x4_Concat(out, &base, &temp);
311 }
312
313 // FIXME: optimize
314 void Matrix4x4_ConcatScale (matrix4x4_t *out, float x)
315 {
316         matrix4x4_t base, temp;
317         Matrix4x4_Copy(out, &base);
318         Matrix4x4_CreateScale(&temp, x);
319         Matrix4x4_Concat(out, &base, &temp);
320 }
321
322 // FIXME: optimize
323 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, float x, float y, float z)
324 {
325         matrix4x4_t base, temp;
326         Matrix4x4_Copy(out, &base);
327         Matrix4x4_CreateScale3(&temp, x, y, z);
328         Matrix4x4_Concat(out, &base, &temp);
329 }
330
331
332
333
334
335
336
337
338 void Matrix3x4_Copy (matrix3x4_t *out, matrix3x4_t *in)
339 {
340         *out = *in;
341 }
342
343 void Matrix3x4_CopyRotateOnly (matrix3x4_t *out, matrix3x4_t *in)
344 {
345         out->m[0][0] = in->m[0][0];
346         out->m[0][1] = in->m[0][1];
347         out->m[0][2] = in->m[0][2];
348         out->m[0][3] = 0.0f;
349         out->m[1][0] = in->m[1][0];
350         out->m[1][1] = in->m[1][1];
351         out->m[1][2] = in->m[1][2];
352         out->m[1][3] = 0.0f;
353         out->m[2][0] = in->m[2][0];
354         out->m[2][1] = in->m[2][1];
355         out->m[2][2] = in->m[2][2];
356         out->m[2][3] = 0.0f;
357 }
358
359 void Matrix3x4_CopyTranslateOnly (matrix3x4_t *out, matrix3x4_t *in)
360 {
361         out->m[0][0] = 0.0f;
362         out->m[0][1] = 0.0f;
363         out->m[0][2] = 0.0f;
364         out->m[0][3] = in->m[0][3];
365         out->m[1][0] = 0.0f;
366         out->m[1][1] = 0.0f;
367         out->m[1][2] = 0.0f;
368         out->m[1][3] = in->m[0][3];
369         out->m[2][0] = 0.0f;
370         out->m[2][1] = 0.0f;
371         out->m[2][2] = 0.0f;
372         out->m[2][3] = in->m[0][3];
373 }
374
375 void Matrix3x4_FromMatrix4x4 (matrix3x4_t *out, matrix4x4_t *in)
376 {
377         out->m[0][0] = in->m[0][0];
378         out->m[0][1] = in->m[0][1];
379         out->m[0][2] = in->m[0][2];
380         out->m[0][3] = in->m[0][3];
381         out->m[1][0] = in->m[1][0];
382         out->m[1][1] = in->m[1][1];
383         out->m[1][2] = in->m[1][2];
384         out->m[1][3] = in->m[1][3];
385         out->m[2][0] = in->m[2][0];
386         out->m[2][1] = in->m[2][1];
387         out->m[2][2] = in->m[2][2];
388         out->m[2][3] = in->m[2][3];
389 }
390
391 void Matrix3x4_Concat (matrix3x4_t *out, const matrix3x4_t *in1, const matrix3x4_t *in2)
392 {
393         out->m[0][0] = in1->m[0][0] * in2->m[0][0] + in1->m[0][1] * in2->m[1][0] + in1->m[0][2] * in2->m[2][0];
394         out->m[0][1] = in1->m[0][0] * in2->m[0][1] + in1->m[0][1] * in2->m[1][1] + in1->m[0][2] * in2->m[2][1];
395         out->m[0][2] = in1->m[0][0] * in2->m[0][2] + in1->m[0][1] * in2->m[1][2] + in1->m[0][2] * in2->m[2][2];
396         out->m[0][3] = in1->m[0][0] * in2->m[0][3] + in1->m[0][1] * in2->m[1][3] + in1->m[0][2] * in2->m[2][3] + in1->m[0][3];
397         out->m[1][0] = in1->m[1][0] * in2->m[0][0] + in1->m[1][1] * in2->m[1][0] + in1->m[1][2] * in2->m[2][0];
398         out->m[1][1] = in1->m[1][0] * in2->m[0][1] + in1->m[1][1] * in2->m[1][1] + in1->m[1][2] * in2->m[2][1];
399         out->m[1][2] = in1->m[1][0] * in2->m[0][2] + in1->m[1][1] * in2->m[1][2] + in1->m[1][2] * in2->m[2][2];
400         out->m[1][3] = in1->m[1][0] * in2->m[0][3] + in1->m[1][1] * in2->m[1][3] + in1->m[1][2] * in2->m[2][3] + in1->m[1][3];
401         out->m[2][0] = in1->m[2][0] * in2->m[0][0] + in1->m[2][1] * in2->m[1][0] + in1->m[2][2] * in2->m[2][0];
402         out->m[2][1] = in1->m[2][0] * in2->m[0][1] + in1->m[2][1] * in2->m[1][1] + in1->m[2][2] * in2->m[2][1];
403         out->m[2][2] = in1->m[2][0] * in2->m[0][2] + in1->m[2][1] * in2->m[1][2] + in1->m[2][2] * in2->m[2][2];
404         out->m[2][3] = in1->m[2][0] * in2->m[0][3] + in1->m[2][1] * in2->m[1][3] + in1->m[2][2] * in2->m[2][3] + in1->m[2][3];
405 }
406
407 void Matrix3x4_Transpose3x3 (matrix3x4_t *out, const matrix3x4_t *in1)
408 {
409         float scale;
410         scale = 3.0 / (in1->m[0][0] * in1->m[0][0]
411                      + in1->m[0][1] * in1->m[0][1]
412                      + in1->m[0][2] * in1->m[0][2]
413                      + in1->m[1][0] * in1->m[1][0]
414                      + in1->m[1][1] * in1->m[1][1]
415                      + in1->m[1][2] * in1->m[1][2]
416                      + in1->m[2][0] * in1->m[2][0]
417                      + in1->m[2][1] * in1->m[2][1]
418                      + in1->m[2][2] * in1->m[2][2]);
419         out->m[0][0] = in1->m[0][0] * scale;
420         out->m[0][1] = in1->m[1][0] * scale;
421         out->m[0][2] = in1->m[2][0] * scale;
422         out->m[0][3] = 0.0f;
423         out->m[1][0] = in1->m[0][1] * scale;
424         out->m[1][1] = in1->m[1][1] * scale;
425         out->m[1][2] = in1->m[2][1] * scale;
426         out->m[1][3] = 0.0f;
427         out->m[2][0] = in1->m[0][2] * scale;
428         out->m[2][1] = in1->m[1][2] * scale;
429         out->m[2][2] = in1->m[2][2] * scale;
430         out->m[2][3] = 0.0f;
431 }
432
433 void Matrix3x4_CreateIdentity (matrix3x4_t *out)
434 {
435         out->m[0][0]=1.0f;
436         out->m[0][1]=0.0f;
437         out->m[0][2]=0.0f;
438         out->m[0][3]=0.0f;
439         out->m[1][0]=0.0f;
440         out->m[1][1]=1.0f;
441         out->m[1][2]=0.0f;
442         out->m[1][3]=0.0f;
443         out->m[2][0]=0.0f;
444         out->m[2][1]=0.0f;
445         out->m[2][2]=1.0f;
446         out->m[2][3]=0.0f;
447 }
448
449 void Matrix3x4_CreateTranslate (matrix3x4_t *out, float x, float y, float z)
450 {
451         out->m[0][0]=1.0f;
452         out->m[0][1]=0.0f;
453         out->m[0][2]=0.0f;
454         out->m[0][3]=x;
455         out->m[1][0]=0.0f;
456         out->m[1][1]=1.0f;
457         out->m[1][2]=0.0f;
458         out->m[1][3]=y;
459         out->m[2][0]=0.0f;
460         out->m[2][1]=0.0f;
461         out->m[2][2]=1.0f;
462         out->m[2][3]=z;
463 }
464
465 void Matrix3x4_CreateRotate (matrix3x4_t *out, float angle, float x, float y, float z)
466 {
467         float len, c, s;
468
469         len = x*x+y*y+z*z;
470         if (len != 0.0f)
471                 len = 1.0f / sqrt(len);
472         x *= len;
473         y *= len;
474         z *= len;
475
476         angle *= M_PI / 180.0;
477         c = cos(angle);
478         s = sin(angle);
479
480         out->m[0][0]=x * x + c * (1 - x * x);
481         out->m[0][1]=x * y * (1 - c) + z * s;
482         out->m[0][2]=z * x * (1 - c) - y * s;
483         out->m[0][3]=0.0f;
484         out->m[1][0]=x * y * (1 - c) - z * s;
485         out->m[1][1]=y * y + c * (1 - y * y);
486         out->m[1][2]=y * z * (1 - c) + x * s;
487         out->m[1][3]=0.0f;
488         out->m[2][0]=z * x * (1 - c) + y * s;
489         out->m[2][1]=y * z * (1 - c) - x * s;
490         out->m[2][2]=z * z + c * (1 - z * z);
491         out->m[2][3]=0.0f;
492 }
493
494 void Matrix3x4_CreateScale (matrix3x4_t *out, float x)
495 {
496         out->m[0][0]=x;
497         out->m[0][1]=0.0f;
498         out->m[0][2]=0.0f;
499         out->m[0][3]=0.0f;
500         out->m[1][0]=0.0f;
501         out->m[1][1]=x;
502         out->m[1][2]=0.0f;
503         out->m[1][3]=0.0f;
504         out->m[2][0]=0.0f;
505         out->m[2][1]=0.0f;
506         out->m[2][2]=x;
507         out->m[2][3]=0.0f;
508 }
509
510 void Matrix3x4_CreateScale3 (matrix3x4_t *out, float x, float y, float z)
511 {
512         out->m[0][0]=x;
513         out->m[0][1]=0.0f;
514         out->m[0][2]=0.0f;
515         out->m[0][3]=0.0f;
516         out->m[1][0]=0.0f;
517         out->m[1][1]=y;
518         out->m[1][2]=0.0f;
519         out->m[1][3]=0.0f;
520         out->m[2][0]=0.0f;
521         out->m[2][1]=0.0f;
522         out->m[2][2]=z;
523         out->m[2][3]=0.0f;
524 }
525
526 void Matrix3x4_ToVectors(const matrix3x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
527 {
528         vx[0] = in->m[0][0];
529         vx[1] = in->m[1][0];
530         vx[2] = in->m[2][0];
531         vy[0] = in->m[0][1];
532         vy[1] = in->m[1][1];
533         vy[2] = in->m[2][1];
534         vz[0] = in->m[0][2];
535         vz[1] = in->m[1][2];
536         vz[2] = in->m[2][2];
537         t[0] = in->m[0][3];
538         t[1] = in->m[1][3];
539         t[2] = in->m[2][3];
540 }
541
542 void Matrix3x4_FromVectors(matrix3x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
543 {
544         out->m[0][0] = vx[0];
545         out->m[0][1] = vy[0];
546         out->m[0][2] = vz[0];
547         out->m[0][3] = t[0];
548         out->m[1][0] = vx[1];
549         out->m[1][1] = vy[1];
550         out->m[1][2] = vz[1];
551         out->m[1][3] = t[1];
552         out->m[2][0] = vx[2];
553         out->m[2][1] = vy[2];
554         out->m[2][2] = vz[2];
555         out->m[2][3] = t[2];
556 }
557
558 void Matrix3x4_Transform (const matrix3x4_t *in, const float v[3], float out[3])
559 {
560         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
561         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
562         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
563 }
564
565 void Matrix3x4_SimpleUntransform (const matrix3x4_t *in, const float v[3], float out[3])
566 {
567         float t[3];
568         t[0] = v[0] - in->m[0][3];
569         t[1] = v[1] - in->m[1][3];
570         t[2] = v[2] - in->m[2][3];
571         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
572         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
573         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
574 }
575
576 // FIXME: optimize
577 void Matrix3x4_ConcatTranslate (matrix3x4_t *out, float x, float y, float z)
578 {
579         matrix3x4_t base, temp;
580         Matrix3x4_Copy(out, &base);
581         Matrix3x4_CreateTranslate(&temp, x, y, z);
582         Matrix3x4_Concat(out, &base, &temp);
583 }
584
585 // FIXME: optimize
586 void Matrix3x4_ConcatRotate (matrix3x4_t *out, float angle, float x, float y, float z)
587 {
588         matrix3x4_t base, temp;
589         Matrix3x4_Copy(out, &base);
590         Matrix3x4_CreateRotate(&temp, angle, x, y, z);
591         Matrix3x4_Concat(out, &base, &temp);
592 }
593
594 // FIXME: optimize
595 void Matrix3x4_ConcatScale (matrix3x4_t *out, float x)
596 {
597         matrix3x4_t base, temp;
598         Matrix3x4_Copy(out, &base);
599         Matrix3x4_CreateScale(&temp, x);
600         Matrix3x4_Concat(out, &base, &temp);
601 }
602
603 // FIXME: optimize
604 void Matrix3x4_ConcatScale3 (matrix3x4_t *out, float x, float y, float z)
605 {
606         matrix3x4_t base, temp;
607         Matrix3x4_Copy(out, &base);
608         Matrix3x4_CreateScale3(&temp, x, y, z);
609         Matrix3x4_Concat(out, &base, &temp);
610 }