most of the framework for hardware accelerated transforms is back, just the actual...
[divverent/darkplaces.git] / matrixlib.c
1
2 #include "quakedef.h"
3 #include "matrixlib.h"
4 #include <math.h>
5
6 void Matrix4x4_Copy (matrix4x4_t *out, const matrix4x4_t *in)
7 {
8         *out = *in;
9 }
10
11 void Matrix4x4_CopyRotateOnly (matrix4x4_t *out, const matrix4x4_t *in)
12 {
13         out->m[0][0] = in->m[0][0];
14         out->m[0][1] = in->m[0][1];
15         out->m[0][2] = in->m[0][2];
16         out->m[0][3] = 0.0f;
17         out->m[1][0] = in->m[1][0];
18         out->m[1][1] = in->m[1][1];
19         out->m[1][2] = in->m[1][2];
20         out->m[1][3] = 0.0f;
21         out->m[2][0] = in->m[2][0];
22         out->m[2][1] = in->m[2][1];
23         out->m[2][2] = in->m[2][2];
24         out->m[2][3] = 0.0f;
25         out->m[3][0] = 0.0f;
26         out->m[3][1] = 0.0f;
27         out->m[3][2] = 0.0f;
28         out->m[3][3] = 1.0f;
29 }
30
31 void Matrix4x4_CopyTranslateOnly (matrix4x4_t *out, const matrix4x4_t *in)
32 {
33         out->m[0][0] = 0.0f;
34         out->m[0][1] = 0.0f;
35         out->m[0][2] = 0.0f;
36         out->m[0][3] = in->m[0][3];
37         out->m[1][0] = 0.0f;
38         out->m[1][1] = 0.0f;
39         out->m[1][2] = 0.0f;
40         out->m[1][3] = in->m[0][3];
41         out->m[2][0] = 0.0f;
42         out->m[2][1] = 0.0f;
43         out->m[2][2] = 0.0f;
44         out->m[2][3] = in->m[0][3];
45         out->m[3][0] = 0.0f;
46         out->m[3][1] = 0.0f;
47         out->m[3][2] = 0.0f;
48         out->m[3][3] = 1.0f;
49 }
50
51 void Matrix4x4_FromMatrix3x4 (matrix4x4_t *out, const matrix3x4_t *in)
52 {
53         out->m[0][0] = in->m[0][0];
54         out->m[0][1] = in->m[0][1];
55         out->m[0][2] = in->m[0][2];
56         out->m[0][3] = in->m[0][3];
57         out->m[1][0] = in->m[1][0];
58         out->m[1][1] = in->m[1][1];
59         out->m[1][2] = in->m[1][2];
60         out->m[1][3] = in->m[1][3];
61         out->m[2][0] = in->m[2][0];
62         out->m[2][1] = in->m[2][1];
63         out->m[2][2] = in->m[2][2];
64         out->m[2][3] = in->m[2][3];
65         out->m[3][0] = 0.0f;
66         out->m[3][1] = 0.0f;
67         out->m[3][2] = 0.0f;
68         out->m[3][3] = 1.0f;
69 }
70
71 void Matrix4x4_Concat (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2)
72 {
73         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];
74         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];
75         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];
76         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];
77         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];
78         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];
79         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];
80         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];
81         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];
82         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];
83         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];
84         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];
85         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];
86         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];
87         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];
88         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];
89 }
90
91 void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1)
92 {
93         out->m[0][0] = in1->m[0][0];
94         out->m[0][1] = in1->m[1][0];
95         out->m[0][2] = in1->m[2][0];
96         out->m[0][3] = in1->m[3][0];
97         out->m[1][0] = in1->m[0][1];
98         out->m[1][1] = in1->m[1][1];
99         out->m[1][2] = in1->m[2][1];
100         out->m[1][3] = in1->m[3][1];
101         out->m[2][0] = in1->m[0][2];
102         out->m[2][1] = in1->m[1][2];
103         out->m[2][2] = in1->m[2][2];
104         out->m[2][3] = in1->m[3][2];
105         out->m[3][0] = in1->m[0][3];
106         out->m[3][1] = in1->m[1][3];
107         out->m[3][2] = in1->m[2][3];
108         out->m[3][3] = in1->m[3][3];
109 }
110
111 void Matrix4x4_Transpose3x3 (matrix4x4_t *out, const matrix4x4_t *in1)
112 {
113         out->m[0][0] = in1->m[0][0];
114         out->m[0][1] = in1->m[1][0];
115         out->m[0][2] = in1->m[2][0];
116         out->m[1][0] = in1->m[0][1];
117         out->m[1][1] = in1->m[1][1];
118         out->m[1][2] = in1->m[2][1];
119         out->m[2][0] = in1->m[0][2];
120         out->m[2][1] = in1->m[1][2];
121         out->m[2][2] = in1->m[2][2];
122
123         out->m[0][3] = in1->m[0][3];
124         out->m[1][3] = in1->m[1][3];
125         out->m[2][3] = in1->m[2][3];
126         out->m[3][0] = in1->m[0][3];
127         out->m[3][1] = in1->m[1][3];
128         out->m[3][2] = in1->m[2][3];
129         out->m[3][3] = in1->m[3][3];
130 }
131
132 void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1)
133 {
134         // we only support uniform scaling, so assume the first row is enough
135         // (note the lack of sqrt here, because we're trying to undo the scaling,
136         // this means multiplying by the inverse scale twice - squaring it, which
137         // makes the sqrt a waste of time)
138         double scale = 1.0 / (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]);
139
140         // invert the rotation by transposing and multiplying by the squared
141         // recipricol of the input matrix scale as described above
142         out->m[0][0] = in1->m[0][0] * scale;
143         out->m[0][1] = in1->m[1][0] * scale;
144         out->m[0][2] = in1->m[2][0] * scale;
145         out->m[1][0] = in1->m[0][1] * scale;
146         out->m[1][1] = in1->m[1][1] * scale;
147         out->m[1][2] = in1->m[2][1] * scale;
148         out->m[2][0] = in1->m[0][2] * scale;
149         out->m[2][1] = in1->m[1][2] * scale;
150         out->m[2][2] = in1->m[2][2] * scale;
151
152         // invert the translate
153         out->m[0][3] = -(in1->m[0][3] * out->m[0][0] + in1->m[1][3] * out->m[0][1] + in1->m[2][3] * out->m[0][2]);
154         out->m[1][3] = -(in1->m[0][3] * out->m[1][0] + in1->m[1][3] * out->m[1][1] + in1->m[2][3] * out->m[1][2]);
155         out->m[2][3] = -(in1->m[0][3] * out->m[2][0] + in1->m[1][3] * out->m[2][1] + in1->m[2][3] * out->m[2][2]);
156
157         // don't know if there's anything worth doing here
158         out->m[3][0] = 0;
159         out->m[3][1] = 0;
160         out->m[3][2] = 0;
161         out->m[3][3] = 1;
162 }
163
164 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
165 {
166         out->m[0][0]=1.0f;
167         out->m[0][1]=0.0f;
168         out->m[0][2]=0.0f;
169         out->m[0][3]=0.0f;
170         out->m[1][0]=0.0f;
171         out->m[1][1]=1.0f;
172         out->m[1][2]=0.0f;
173         out->m[1][3]=0.0f;
174         out->m[2][0]=0.0f;
175         out->m[2][1]=0.0f;
176         out->m[2][2]=1.0f;
177         out->m[2][3]=0.0f;
178         out->m[3][0]=0.0f;
179         out->m[3][1]=0.0f;
180         out->m[3][2]=0.0f;
181         out->m[3][3]=1.0f;
182 }
183
184 void Matrix4x4_CreateTranslate (matrix4x4_t *out, float x, float y, float z)
185 {
186         out->m[0][0]=1.0f;
187         out->m[0][1]=0.0f;
188         out->m[0][2]=0.0f;
189         out->m[0][3]=x;
190         out->m[1][0]=0.0f;
191         out->m[1][1]=1.0f;
192         out->m[1][2]=0.0f;
193         out->m[1][3]=y;
194         out->m[2][0]=0.0f;
195         out->m[2][1]=0.0f;
196         out->m[2][2]=1.0f;
197         out->m[2][3]=z;
198         out->m[3][0]=0.0f;
199         out->m[3][1]=0.0f;
200         out->m[3][2]=0.0f;
201         out->m[3][3]=1.0f;
202 }
203
204 void Matrix4x4_CreateRotate (matrix4x4_t *out, float angle, float x, float y, float z)
205 {
206         float len, c, s;
207
208         len = x*x+y*y+z*z;
209         if (len != 0.0f)
210                 len = 1.0f / sqrt(len);
211         x *= len;
212         y *= len;
213         z *= len;
214
215         angle *= M_PI / 180.0;
216         c = cos(angle);
217         s = sin(angle);
218
219         out->m[0][0]=x * x + c * (1 - x * x);
220         out->m[0][1]=x * y * (1 - c) + z * s;
221         out->m[0][2]=z * x * (1 - c) - y * s;
222         out->m[0][3]=0.0f;
223         out->m[1][0]=x * y * (1 - c) - z * s;
224         out->m[1][1]=y * y + c * (1 - y * y);
225         out->m[1][2]=y * z * (1 - c) + x * s;
226         out->m[1][3]=0.0f;
227         out->m[2][0]=z * x * (1 - c) + y * s;
228         out->m[2][1]=y * z * (1 - c) - x * s;
229         out->m[2][2]=z * z + c * (1 - z * z);
230         out->m[2][3]=0.0f;
231         out->m[3][0]=0.0f;
232         out->m[3][1]=0.0f;
233         out->m[3][2]=0.0f;
234         out->m[3][3]=1.0f;
235 }
236
237 void Matrix4x4_CreateScale (matrix4x4_t *out, float x)
238 {
239         out->m[0][0]=x;
240         out->m[0][1]=0.0f;
241         out->m[0][2]=0.0f;
242         out->m[0][3]=0.0f;
243         out->m[1][0]=0.0f;
244         out->m[1][1]=x;
245         out->m[1][2]=0.0f;
246         out->m[1][3]=0.0f;
247         out->m[2][0]=0.0f;
248         out->m[2][1]=0.0f;
249         out->m[2][2]=x;
250         out->m[2][3]=0.0f;
251         out->m[3][0]=0.0f;
252         out->m[3][1]=0.0f;
253         out->m[3][2]=0.0f;
254         out->m[3][3]=1.0f;
255 }
256
257 void Matrix4x4_CreateScale3 (matrix4x4_t *out, float x, float y, float z)
258 {
259         out->m[0][0]=x;
260         out->m[0][1]=0.0f;
261         out->m[0][2]=0.0f;
262         out->m[0][3]=0.0f;
263         out->m[1][0]=0.0f;
264         out->m[1][1]=y;
265         out->m[1][2]=0.0f;
266         out->m[1][3]=0.0f;
267         out->m[2][0]=0.0f;
268         out->m[2][1]=0.0f;
269         out->m[2][2]=z;
270         out->m[2][3]=0.0f;
271         out->m[3][0]=0.0f;
272         out->m[3][1]=0.0f;
273         out->m[3][2]=0.0f;
274         out->m[3][3]=1.0f;
275 }
276
277 void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, float x, float y, float z, float pitch, float yaw, float roll, float scale)
278 {
279         double angle, sr, sp, sy, cr, cp, cy;
280
281         angle = yaw * (M_PI*2 / 360);
282         sy = sin(angle);
283         cy = cos(angle);
284         angle = pitch * (M_PI*2 / 360);
285         sp = sin(angle);
286         cp = cos(angle);
287         angle = roll * (M_PI*2 / 360);
288         sr = sin(angle);
289         cr = cos(angle);
290         out->m[0][0] = cp*cy * scale;
291         out->m[0][1] = sr*sp*cy+cr*-sy * scale;
292         out->m[0][2] = cr*sp*cy+-sr*-sy * scale;
293         out->m[0][3] = x;
294         out->m[1][0] = cp*sy * scale;
295         out->m[1][1] = sr*sp*sy+cr*cy * scale;
296         out->m[1][2] = cr*sp*sy+-sr*cy * scale;
297         out->m[1][3] = y;
298         out->m[2][0] = -sp * scale;
299         out->m[2][1] = sr*cp * scale;
300         out->m[2][2] = cr*cp * scale;
301         out->m[2][3] = z;
302         out->m[3][0] = 0;
303         out->m[3][1] = 0;
304         out->m[3][2] = 0;
305         out->m[3][3] = 1;
306 }
307
308 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
309 {
310         vx[0] = in->m[0][0];
311         vx[1] = in->m[1][0];
312         vx[2] = in->m[2][0];
313         vy[0] = in->m[0][1];
314         vy[1] = in->m[1][1];
315         vy[2] = in->m[2][1];
316         vz[0] = in->m[0][2];
317         vz[1] = in->m[1][2];
318         vz[2] = in->m[2][2];
319         t[0] = in->m[0][3];
320         t[1] = in->m[1][3];
321         t[2] = in->m[2][3];
322 }
323
324 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
325 {
326         out->m[0][0] = vx[0];
327         out->m[0][1] = vy[0];
328         out->m[0][2] = vz[0];
329         out->m[0][3] = t[0];
330         out->m[1][0] = vx[1];
331         out->m[1][1] = vy[1];
332         out->m[1][2] = vz[1];
333         out->m[1][3] = t[1];
334         out->m[2][0] = vx[2];
335         out->m[2][1] = vy[2];
336         out->m[2][2] = vz[2];
337         out->m[2][3] = t[2];
338         out->m[3][0] = 0.0f;
339         out->m[3][1] = 0.0f;
340         out->m[3][2] = 0.0f;
341         out->m[3][3] = 1.0f;
342 }
343
344 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
345 {
346         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
347         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
348         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
349 }
350
351 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
352 {
353         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];
354         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];
355         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];
356         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];
357 }
358
359 /*
360 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
361 {
362         float t[3];
363         t[0] = v[0] - in->m[0][3];
364         t[1] = v[1] - in->m[1][3];
365         t[2] = v[2] - in->m[2][3];
366         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
367         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
368         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
369 }
370 */
371
372 // FIXME: optimize
373 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, float x, float y, float z)
374 {
375         matrix4x4_t base, temp;
376         Matrix4x4_Copy(out, &base);
377         Matrix4x4_CreateTranslate(&temp, x, y, z);
378         Matrix4x4_Concat(out, &base, &temp);
379 }
380
381 // FIXME: optimize
382 void Matrix4x4_ConcatRotate (matrix4x4_t *out, float angle, float x, float y, float z)
383 {
384         matrix4x4_t base, temp;
385         Matrix4x4_Copy(out, &base);
386         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
387         Matrix4x4_Concat(out, &base, &temp);
388 }
389
390 // FIXME: optimize
391 void Matrix4x4_ConcatScale (matrix4x4_t *out, float x)
392 {
393         matrix4x4_t base, temp;
394         Matrix4x4_Copy(out, &base);
395         Matrix4x4_CreateScale(&temp, x);
396         Matrix4x4_Concat(out, &base, &temp);
397 }
398
399 // FIXME: optimize
400 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, float x, float y, float z)
401 {
402         matrix4x4_t base, temp;
403         Matrix4x4_Copy(out, &base);
404         Matrix4x4_CreateScale3(&temp, x, y, z);
405         Matrix4x4_Concat(out, &base, &temp);
406 }
407
408
409
410
411
412
413
414
415 void Matrix3x4_Copy (matrix3x4_t *out, const matrix3x4_t *in)
416 {
417         *out = *in;
418 }
419
420 void Matrix3x4_CopyRotateOnly (matrix3x4_t *out, const matrix3x4_t *in)
421 {
422         out->m[0][0] = in->m[0][0];
423         out->m[0][1] = in->m[0][1];
424         out->m[0][2] = in->m[0][2];
425         out->m[0][3] = 0.0f;
426         out->m[1][0] = in->m[1][0];
427         out->m[1][1] = in->m[1][1];
428         out->m[1][2] = in->m[1][2];
429         out->m[1][3] = 0.0f;
430         out->m[2][0] = in->m[2][0];
431         out->m[2][1] = in->m[2][1];
432         out->m[2][2] = in->m[2][2];
433         out->m[2][3] = 0.0f;
434 }
435
436 void Matrix3x4_CopyTranslateOnly (matrix3x4_t *out, const matrix3x4_t *in)
437 {
438         out->m[0][0] = 0.0f;
439         out->m[0][1] = 0.0f;
440         out->m[0][2] = 0.0f;
441         out->m[0][3] = in->m[0][3];
442         out->m[1][0] = 0.0f;
443         out->m[1][1] = 0.0f;
444         out->m[1][2] = 0.0f;
445         out->m[1][3] = in->m[0][3];
446         out->m[2][0] = 0.0f;
447         out->m[2][1] = 0.0f;
448         out->m[2][2] = 0.0f;
449         out->m[2][3] = in->m[0][3];
450 }
451
452 void Matrix3x4_FromMatrix4x4 (matrix3x4_t *out, const matrix4x4_t *in)
453 {
454         out->m[0][0] = in->m[0][0];
455         out->m[0][1] = in->m[0][1];
456         out->m[0][2] = in->m[0][2];
457         out->m[0][3] = in->m[0][3];
458         out->m[1][0] = in->m[1][0];
459         out->m[1][1] = in->m[1][1];
460         out->m[1][2] = in->m[1][2];
461         out->m[1][3] = in->m[1][3];
462         out->m[2][0] = in->m[2][0];
463         out->m[2][1] = in->m[2][1];
464         out->m[2][2] = in->m[2][2];
465         out->m[2][3] = in->m[2][3];
466 }
467
468 void Matrix3x4_Concat (matrix3x4_t *out, const matrix3x4_t *in1, const matrix3x4_t *in2)
469 {
470         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];
471         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];
472         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];
473         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];
474         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];
475         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];
476         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];
477         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];
478         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];
479         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];
480         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];
481         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];
482 }
483
484 void Matrix3x4_Transpose3x3 (matrix3x4_t *out, const matrix3x4_t *in1)
485 {
486         out->m[0][0] = in1->m[0][0];
487         out->m[0][1] = in1->m[1][0];
488         out->m[0][2] = in1->m[2][0];
489         out->m[0][3] = 0.0f;
490         out->m[1][0] = in1->m[0][1];
491         out->m[1][1] = in1->m[1][1];
492         out->m[1][2] = in1->m[2][1];
493         out->m[1][3] = 0.0f;
494         out->m[2][0] = in1->m[0][2];
495         out->m[2][1] = in1->m[1][2];
496         out->m[2][2] = in1->m[2][2];
497         out->m[2][3] = 0.0f;
498 }
499
500 void Matrix3x4_Invert_Simple (matrix3x4_t *out, const matrix3x4_t *in1)
501 {
502         // we only support uniform scaling, so assume the first row is enough
503         // (note the lack of sqrt here, because we're trying to undo the scaling,
504         // this means multiplying by the inverse scale twice - squaring it, which
505         // makes the sqrt a waste of time)
506         double scale = 1.0 / (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]);
507
508         // invert the rotation by transposing and multiplying by the squared
509         // recipricol of the input matrix scale as described above
510         out->m[0][0] = in1->m[0][0] * scale;
511         out->m[0][1] = in1->m[1][0] * scale;
512         out->m[0][2] = in1->m[2][0] * scale;
513         out->m[1][0] = in1->m[0][1] * scale;
514         out->m[1][1] = in1->m[1][1] * scale;
515         out->m[1][2] = in1->m[2][1] * scale;
516         out->m[2][0] = in1->m[0][2] * scale;
517         out->m[2][1] = in1->m[1][2] * scale;
518         out->m[2][2] = in1->m[2][2] * scale;
519
520         // invert the translate
521         out->m[0][3] = -(in1->m[0][3] * out->m[0][0] + in1->m[1][3] * out->m[0][1] + in1->m[2][3] * out->m[0][2]);
522         out->m[1][3] = -(in1->m[0][3] * out->m[1][0] + in1->m[1][3] * out->m[1][1] + in1->m[2][3] * out->m[1][2]);
523         out->m[2][3] = -(in1->m[0][3] * out->m[2][0] + in1->m[1][3] * out->m[2][1] + in1->m[2][3] * out->m[2][2]);
524 }
525
526
527 void Matrix3x4_CreateIdentity (matrix3x4_t *out)
528 {
529         out->m[0][0]=1.0f;
530         out->m[0][1]=0.0f;
531         out->m[0][2]=0.0f;
532         out->m[0][3]=0.0f;
533         out->m[1][0]=0.0f;
534         out->m[1][1]=1.0f;
535         out->m[1][2]=0.0f;
536         out->m[1][3]=0.0f;
537         out->m[2][0]=0.0f;
538         out->m[2][1]=0.0f;
539         out->m[2][2]=1.0f;
540         out->m[2][3]=0.0f;
541 }
542
543 void Matrix3x4_CreateTranslate (matrix3x4_t *out, float x, float y, float z)
544 {
545         out->m[0][0]=1.0f;
546         out->m[0][1]=0.0f;
547         out->m[0][2]=0.0f;
548         out->m[0][3]=x;
549         out->m[1][0]=0.0f;
550         out->m[1][1]=1.0f;
551         out->m[1][2]=0.0f;
552         out->m[1][3]=y;
553         out->m[2][0]=0.0f;
554         out->m[2][1]=0.0f;
555         out->m[2][2]=1.0f;
556         out->m[2][3]=z;
557 }
558
559 void Matrix3x4_CreateRotate (matrix3x4_t *out, float angle, float x, float y, float z)
560 {
561         float len, c, s;
562
563         len = x*x+y*y+z*z;
564         if (len != 0.0f)
565                 len = 1.0f / sqrt(len);
566         x *= len;
567         y *= len;
568         z *= len;
569
570         angle *= M_PI / 180.0;
571         c = cos(angle);
572         s = sin(angle);
573
574         out->m[0][0]=x * x + c * (1 - x * x);
575         out->m[0][1]=x * y * (1 - c) + z * s;
576         out->m[0][2]=z * x * (1 - c) - y * s;
577         out->m[0][3]=0.0f;
578         out->m[1][0]=x * y * (1 - c) - z * s;
579         out->m[1][1]=y * y + c * (1 - y * y);
580         out->m[1][2]=y * z * (1 - c) + x * s;
581         out->m[1][3]=0.0f;
582         out->m[2][0]=z * x * (1 - c) + y * s;
583         out->m[2][1]=y * z * (1 - c) - x * s;
584         out->m[2][2]=z * z + c * (1 - z * z);
585         out->m[2][3]=0.0f;
586 }
587
588 void Matrix3x4_CreateScale (matrix3x4_t *out, float x)
589 {
590         out->m[0][0]=x;
591         out->m[0][1]=0.0f;
592         out->m[0][2]=0.0f;
593         out->m[0][3]=0.0f;
594         out->m[1][0]=0.0f;
595         out->m[1][1]=x;
596         out->m[1][2]=0.0f;
597         out->m[1][3]=0.0f;
598         out->m[2][0]=0.0f;
599         out->m[2][1]=0.0f;
600         out->m[2][2]=x;
601         out->m[2][3]=0.0f;
602 }
603
604 void Matrix3x4_CreateScale3 (matrix3x4_t *out, float x, float y, float z)
605 {
606         out->m[0][0]=x;
607         out->m[0][1]=0.0f;
608         out->m[0][2]=0.0f;
609         out->m[0][3]=0.0f;
610         out->m[1][0]=0.0f;
611         out->m[1][1]=y;
612         out->m[1][2]=0.0f;
613         out->m[1][3]=0.0f;
614         out->m[2][0]=0.0f;
615         out->m[2][1]=0.0f;
616         out->m[2][2]=z;
617         out->m[2][3]=0.0f;
618 }
619
620 void Matrix3x4_CreateFromQuakeEntity(matrix3x4_t *out, float x, float y, float z, float pitch, float yaw, float roll, float scale)
621 {
622         double angle, sr, sp, sy, cr, cp, cy;
623
624         angle = yaw * (M_PI*2 / 360);
625         sy = sin(angle);
626         cy = cos(angle);
627         angle = pitch * (M_PI*2 / 360);
628         sp = sin(angle);
629         cp = cos(angle);
630         angle = roll * (M_PI*2 / 360);
631         sr = sin(angle);
632         cr = cos(angle);
633         out->m[0][0] = cp*cy * scale;
634         out->m[0][1] = sr*sp*cy+cr*-sy * scale;
635         out->m[0][2] = cr*sp*cy+-sr*-sy * scale;
636         out->m[0][3] = x;
637         out->m[1][0] = cp*sy * scale;
638         out->m[1][1] = sr*sp*sy+cr*cy * scale;
639         out->m[1][2] = cr*sp*sy+-sr*cy * scale;
640         out->m[1][3] = y;
641         out->m[2][0] = -sp * scale;
642         out->m[2][1] = sr*cp * scale;
643         out->m[2][2] = cr*cp * scale;
644         out->m[2][3] = z;
645 }
646
647 void Matrix3x4_ToVectors(const matrix3x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
648 {
649         vx[0] = in->m[0][0];
650         vx[1] = in->m[1][0];
651         vx[2] = in->m[2][0];
652         vy[0] = in->m[0][1];
653         vy[1] = in->m[1][1];
654         vy[2] = in->m[2][1];
655         vz[0] = in->m[0][2];
656         vz[1] = in->m[1][2];
657         vz[2] = in->m[2][2];
658         t[0] = in->m[0][3];
659         t[1] = in->m[1][3];
660         t[2] = in->m[2][3];
661 }
662
663 void Matrix3x4_FromVectors(matrix3x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
664 {
665         out->m[0][0] = vx[0];
666         out->m[0][1] = vy[0];
667         out->m[0][2] = vz[0];
668         out->m[0][3] = t[0];
669         out->m[1][0] = vx[1];
670         out->m[1][1] = vy[1];
671         out->m[1][2] = vz[1];
672         out->m[1][3] = t[1];
673         out->m[2][0] = vx[2];
674         out->m[2][1] = vy[2];
675         out->m[2][2] = vz[2];
676         out->m[2][3] = t[2];
677 }
678
679 void Matrix3x4_Transform (const matrix3x4_t *in, const float v[3], float out[3])
680 {
681         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
682         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
683         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
684 }
685
686 void Matrix3x4_SimpleUntransform (const matrix3x4_t *in, const float v[3], float out[3])
687 {
688         float t[3];
689         t[0] = v[0] - in->m[0][3];
690         t[1] = v[1] - in->m[1][3];
691         t[2] = v[2] - in->m[2][3];
692         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
693         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
694         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
695 }
696
697 // FIXME: optimize
698 void Matrix3x4_ConcatTranslate (matrix3x4_t *out, float x, float y, float z)
699 {
700         matrix3x4_t base, temp;
701         Matrix3x4_Copy(out, &base);
702         Matrix3x4_CreateTranslate(&temp, x, y, z);
703         Matrix3x4_Concat(out, &base, &temp);
704 }
705
706 // FIXME: optimize
707 void Matrix3x4_ConcatRotate (matrix3x4_t *out, float angle, float x, float y, float z)
708 {
709         matrix3x4_t base, temp;
710         Matrix3x4_Copy(out, &base);
711         Matrix3x4_CreateRotate(&temp, angle, x, y, z);
712         Matrix3x4_Concat(out, &base, &temp);
713 }
714
715 // FIXME: optimize
716 void Matrix3x4_ConcatScale (matrix3x4_t *out, float x)
717 {
718         matrix3x4_t base, temp;
719         Matrix3x4_Copy(out, &base);
720         Matrix3x4_CreateScale(&temp, x);
721         Matrix3x4_Concat(out, &base, &temp);
722 }
723
724 // FIXME: optimize
725 void Matrix3x4_ConcatScale3 (matrix3x4_t *out, float x, float y, float z)
726 {
727         matrix3x4_t base, temp;
728         Matrix3x4_Copy(out, &base);
729         Matrix3x4_CreateScale3(&temp, x, y, z);
730         Matrix3x4_Concat(out, &base, &temp);
731 }