now uses hardware transforms
[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 #if 1
139         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]);
140 #else
141         double scale = 3.0 / sqrt
142                  (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
143                 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
144                 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
145         scale *= scale;
146 #endif
147
148         // invert the rotation by transposing and multiplying by the squared
149         // recipricol of the input matrix scale as described above
150         out->m[0][0] = in1->m[0][0] * scale;
151         out->m[0][1] = in1->m[1][0] * scale;
152         out->m[0][2] = in1->m[2][0] * scale;
153         out->m[1][0] = in1->m[0][1] * scale;
154         out->m[1][1] = in1->m[1][1] * scale;
155         out->m[1][2] = in1->m[2][1] * scale;
156         out->m[2][0] = in1->m[0][2] * scale;
157         out->m[2][1] = in1->m[1][2] * scale;
158         out->m[2][2] = in1->m[2][2] * scale;
159
160         // invert the translate
161         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]);
162         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]);
163         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]);
164
165         // don't know if there's anything worth doing here
166         out->m[3][0] = 0;
167         out->m[3][1] = 0;
168         out->m[3][2] = 0;
169         out->m[3][3] = 1;
170 }
171
172 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
173 {
174         out->m[0][0]=1.0f;
175         out->m[0][1]=0.0f;
176         out->m[0][2]=0.0f;
177         out->m[0][3]=0.0f;
178         out->m[1][0]=0.0f;
179         out->m[1][1]=1.0f;
180         out->m[1][2]=0.0f;
181         out->m[1][3]=0.0f;
182         out->m[2][0]=0.0f;
183         out->m[2][1]=0.0f;
184         out->m[2][2]=1.0f;
185         out->m[2][3]=0.0f;
186         out->m[3][0]=0.0f;
187         out->m[3][1]=0.0f;
188         out->m[3][2]=0.0f;
189         out->m[3][3]=1.0f;
190 }
191
192 void Matrix4x4_CreateTranslate (matrix4x4_t *out, float x, float y, float z)
193 {
194         out->m[0][0]=1.0f;
195         out->m[0][1]=0.0f;
196         out->m[0][2]=0.0f;
197         out->m[0][3]=x;
198         out->m[1][0]=0.0f;
199         out->m[1][1]=1.0f;
200         out->m[1][2]=0.0f;
201         out->m[1][3]=y;
202         out->m[2][0]=0.0f;
203         out->m[2][1]=0.0f;
204         out->m[2][2]=1.0f;
205         out->m[2][3]=z;
206         out->m[3][0]=0.0f;
207         out->m[3][1]=0.0f;
208         out->m[3][2]=0.0f;
209         out->m[3][3]=1.0f;
210 }
211
212 void Matrix4x4_CreateRotate (matrix4x4_t *out, float angle, float x, float y, float z)
213 {
214         float len, c, s;
215
216         len = x*x+y*y+z*z;
217         if (len != 0.0f)
218                 len = 1.0f / sqrt(len);
219         x *= len;
220         y *= len;
221         z *= len;
222
223         angle *= -M_PI / 180.0;
224         c = cos(angle);
225         s = sin(angle);
226
227         out->m[0][0]=x * x + c * (1 - x * x);
228         out->m[0][1]=x * y * (1 - c) + z * s;
229         out->m[0][2]=z * x * (1 - c) - y * s;
230         out->m[0][3]=0.0f;
231         out->m[1][0]=x * y * (1 - c) - z * s;
232         out->m[1][1]=y * y + c * (1 - y * y);
233         out->m[1][2]=y * z * (1 - c) + x * s;
234         out->m[1][3]=0.0f;
235         out->m[2][0]=z * x * (1 - c) + y * s;
236         out->m[2][1]=y * z * (1 - c) - x * s;
237         out->m[2][2]=z * z + c * (1 - z * z);
238         out->m[2][3]=0.0f;
239         out->m[3][0]=0.0f;
240         out->m[3][1]=0.0f;
241         out->m[3][2]=0.0f;
242         out->m[3][3]=1.0f;
243 }
244
245 void Matrix4x4_CreateScale (matrix4x4_t *out, float x)
246 {
247         out->m[0][0]=x;
248         out->m[0][1]=0.0f;
249         out->m[0][2]=0.0f;
250         out->m[0][3]=0.0f;
251         out->m[1][0]=0.0f;
252         out->m[1][1]=x;
253         out->m[1][2]=0.0f;
254         out->m[1][3]=0.0f;
255         out->m[2][0]=0.0f;
256         out->m[2][1]=0.0f;
257         out->m[2][2]=x;
258         out->m[2][3]=0.0f;
259         out->m[3][0]=0.0f;
260         out->m[3][1]=0.0f;
261         out->m[3][2]=0.0f;
262         out->m[3][3]=1.0f;
263 }
264
265 void Matrix4x4_CreateScale3 (matrix4x4_t *out, float x, float y, float z)
266 {
267         out->m[0][0]=x;
268         out->m[0][1]=0.0f;
269         out->m[0][2]=0.0f;
270         out->m[0][3]=0.0f;
271         out->m[1][0]=0.0f;
272         out->m[1][1]=y;
273         out->m[1][2]=0.0f;
274         out->m[1][3]=0.0f;
275         out->m[2][0]=0.0f;
276         out->m[2][1]=0.0f;
277         out->m[2][2]=z;
278         out->m[2][3]=0.0f;
279         out->m[3][0]=0.0f;
280         out->m[3][1]=0.0f;
281         out->m[3][2]=0.0f;
282         out->m[3][3]=1.0f;
283 }
284
285 void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, float x, float y, float z, float pitch, float yaw, float roll, float scale)
286 {
287         double angle, sr, sp, sy, cr, cp, cy;
288
289         angle = yaw * (M_PI*2 / 360);
290         sy = sin(angle);
291         cy = cos(angle);
292         angle = pitch * (M_PI*2 / 360);
293         sp = sin(angle);
294         cp = cos(angle);
295         angle = roll * (M_PI*2 / 360);
296         sr = sin(angle);
297         cr = cos(angle);
298         out->m[0][0] = (cp*cy) * scale;
299         out->m[0][1] = (sr*sp*cy+cr*-sy) * scale;
300         out->m[0][2] = (cr*sp*cy+-sr*-sy) * scale;
301         out->m[0][3] = x;
302         out->m[1][0] = (cp*sy) * scale;
303         out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
304         out->m[1][2] = (cr*sp*sy+-sr*cy) * scale;
305         out->m[1][3] = y;
306         out->m[2][0] = (-sp) * scale;
307         out->m[2][1] = (sr*cp) * scale;
308         out->m[2][2] = (cr*cp) * scale;
309         out->m[2][3] = z;
310         out->m[3][0] = 0;
311         out->m[3][1] = 0;
312         out->m[3][2] = 0;
313         out->m[3][3] = 1;
314 }
315
316 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
317 {
318         vx[0] = in->m[0][0];
319         vx[1] = in->m[1][0];
320         vx[2] = in->m[2][0];
321         vy[0] = in->m[0][1];
322         vy[1] = in->m[1][1];
323         vy[2] = in->m[2][1];
324         vz[0] = in->m[0][2];
325         vz[1] = in->m[1][2];
326         vz[2] = in->m[2][2];
327         t[0] = in->m[0][3];
328         t[1] = in->m[1][3];
329         t[2] = in->m[2][3];
330 }
331
332 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
333 {
334         out->m[0][0] = vx[0];
335         out->m[0][1] = vy[0];
336         out->m[0][2] = vz[0];
337         out->m[0][3] = t[0];
338         out->m[1][0] = vx[1];
339         out->m[1][1] = vy[1];
340         out->m[1][2] = vz[1];
341         out->m[1][3] = t[1];
342         out->m[2][0] = vx[2];
343         out->m[2][1] = vy[2];
344         out->m[2][2] = vz[2];
345         out->m[2][3] = t[2];
346         out->m[3][0] = 0.0f;
347         out->m[3][1] = 0.0f;
348         out->m[3][2] = 0.0f;
349         out->m[3][3] = 1.0f;
350 }
351
352 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
353 {
354         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
355         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
356         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
357 }
358
359 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
360 {
361         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];
362         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];
363         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];
364         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];
365 }
366
367 /*
368 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
369 {
370         float t[3];
371         t[0] = v[0] - in->m[0][3];
372         t[1] = v[1] - in->m[1][3];
373         t[2] = v[2] - in->m[2][3];
374         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
375         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
376         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
377 }
378 */
379
380 // FIXME: optimize
381 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, float x, float y, float z)
382 {
383         matrix4x4_t base, temp;
384         base = *out;
385         Matrix4x4_CreateTranslate(&temp, x, y, z);
386         Matrix4x4_Concat(out, &base, &temp);
387 }
388
389 // FIXME: optimize
390 void Matrix4x4_ConcatRotate (matrix4x4_t *out, float angle, float x, float y, float z)
391 {
392         matrix4x4_t base, temp;
393         base = *out;
394         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
395         Matrix4x4_Concat(out, &base, &temp);
396 }
397
398 // FIXME: optimize
399 void Matrix4x4_ConcatScale (matrix4x4_t *out, float x)
400 {
401         matrix4x4_t base, temp;
402         base = *out;
403         Matrix4x4_CreateScale(&temp, x);
404         Matrix4x4_Concat(out, &base, &temp);
405 }
406
407 // FIXME: optimize
408 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, float x, float y, float z)
409 {
410         matrix4x4_t base, temp;
411         base = *out;
412         Matrix4x4_CreateScale3(&temp, x, y, z);
413         Matrix4x4_Concat(out, &base, &temp);
414 }
415
416 void Matrix4x4_Print (const matrix4x4_t *in)
417 {
418         Con_Printf("%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n"
419         , in->m[0][0], in->m[0][1], in->m[0][2], in->m[0][3]
420         , in->m[1][0], in->m[1][1], in->m[1][2], in->m[1][3]
421         , in->m[2][0], in->m[2][1], in->m[2][2], in->m[2][3]
422         , in->m[3][0], in->m[3][1], in->m[3][2], in->m[3][3]);
423 }
424
425
426
427
428
429
430
431 void Matrix3x4_Copy (matrix3x4_t *out, const matrix3x4_t *in)
432 {
433         *out = *in;
434 }
435
436 void Matrix3x4_CopyRotateOnly (matrix3x4_t *out, const matrix3x4_t *in)
437 {
438         out->m[0][0] = in->m[0][0];
439         out->m[0][1] = in->m[0][1];
440         out->m[0][2] = in->m[0][2];
441         out->m[0][3] = 0.0f;
442         out->m[1][0] = in->m[1][0];
443         out->m[1][1] = in->m[1][1];
444         out->m[1][2] = in->m[1][2];
445         out->m[1][3] = 0.0f;
446         out->m[2][0] = in->m[2][0];
447         out->m[2][1] = in->m[2][1];
448         out->m[2][2] = in->m[2][2];
449         out->m[2][3] = 0.0f;
450 }
451
452 void Matrix3x4_CopyTranslateOnly (matrix3x4_t *out, const matrix3x4_t *in)
453 {
454         out->m[0][0] = 0.0f;
455         out->m[0][1] = 0.0f;
456         out->m[0][2] = 0.0f;
457         out->m[0][3] = in->m[0][3];
458         out->m[1][0] = 0.0f;
459         out->m[1][1] = 0.0f;
460         out->m[1][2] = 0.0f;
461         out->m[1][3] = in->m[0][3];
462         out->m[2][0] = 0.0f;
463         out->m[2][1] = 0.0f;
464         out->m[2][2] = 0.0f;
465         out->m[2][3] = in->m[0][3];
466 }
467
468 void Matrix3x4_FromMatrix4x4 (matrix3x4_t *out, const matrix4x4_t *in)
469 {
470         out->m[0][0] = in->m[0][0];
471         out->m[0][1] = in->m[0][1];
472         out->m[0][2] = in->m[0][2];
473         out->m[0][3] = in->m[0][3];
474         out->m[1][0] = in->m[1][0];
475         out->m[1][1] = in->m[1][1];
476         out->m[1][2] = in->m[1][2];
477         out->m[1][3] = in->m[1][3];
478         out->m[2][0] = in->m[2][0];
479         out->m[2][1] = in->m[2][1];
480         out->m[2][2] = in->m[2][2];
481         out->m[2][3] = in->m[2][3];
482 }
483
484 void Matrix3x4_Concat (matrix3x4_t *out, const matrix3x4_t *in1, const matrix3x4_t *in2)
485 {
486         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];
487         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];
488         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];
489         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];
490         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];
491         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];
492         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];
493         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];
494         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];
495         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];
496         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];
497         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];
498 }
499
500 void Matrix3x4_Transpose3x3 (matrix3x4_t *out, const matrix3x4_t *in1)
501 {
502         out->m[0][0] = in1->m[0][0];
503         out->m[0][1] = in1->m[1][0];
504         out->m[0][2] = in1->m[2][0];
505         out->m[0][3] = 0.0f;
506         out->m[1][0] = in1->m[0][1];
507         out->m[1][1] = in1->m[1][1];
508         out->m[1][2] = in1->m[2][1];
509         out->m[1][3] = 0.0f;
510         out->m[2][0] = in1->m[0][2];
511         out->m[2][1] = in1->m[1][2];
512         out->m[2][2] = in1->m[2][2];
513         out->m[2][3] = 0.0f;
514 }
515
516 void Matrix3x4_Invert_Simple (matrix3x4_t *out, const matrix3x4_t *in1)
517 {
518         // we only support uniform scaling, so assume the first row is enough
519         // (note the lack of sqrt here, because we're trying to undo the scaling,
520         // this means multiplying by the inverse scale twice - squaring it, which
521         // makes the sqrt a waste of time)
522 #if 1
523         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]);
524 #else
525         double scale = 3.0 / sqrt
526                  (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
527                 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
528                 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
529         scale *= scale;
530 #endif
531
532         // invert the rotation by transposing and multiplying by the squared
533         // recipricol of the input matrix scale as described above
534         out->m[0][0] = in1->m[0][0] * scale;
535         out->m[0][1] = in1->m[1][0] * scale;
536         out->m[0][2] = in1->m[2][0] * scale;
537         out->m[1][0] = in1->m[0][1] * scale;
538         out->m[1][1] = in1->m[1][1] * scale;
539         out->m[1][2] = in1->m[2][1] * scale;
540         out->m[2][0] = in1->m[0][2] * scale;
541         out->m[2][1] = in1->m[1][2] * scale;
542         out->m[2][2] = in1->m[2][2] * scale;
543
544         // invert the translate
545         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]);
546         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]);
547         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]);
548 }
549
550
551 void Matrix3x4_CreateIdentity (matrix3x4_t *out)
552 {
553         out->m[0][0]=1.0f;
554         out->m[0][1]=0.0f;
555         out->m[0][2]=0.0f;
556         out->m[0][3]=0.0f;
557         out->m[1][0]=0.0f;
558         out->m[1][1]=1.0f;
559         out->m[1][2]=0.0f;
560         out->m[1][3]=0.0f;
561         out->m[2][0]=0.0f;
562         out->m[2][1]=0.0f;
563         out->m[2][2]=1.0f;
564         out->m[2][3]=0.0f;
565 }
566
567 void Matrix3x4_CreateTranslate (matrix3x4_t *out, float x, float y, float z)
568 {
569         out->m[0][0]=1.0f;
570         out->m[0][1]=0.0f;
571         out->m[0][2]=0.0f;
572         out->m[0][3]=x;
573         out->m[1][0]=0.0f;
574         out->m[1][1]=1.0f;
575         out->m[1][2]=0.0f;
576         out->m[1][3]=y;
577         out->m[2][0]=0.0f;
578         out->m[2][1]=0.0f;
579         out->m[2][2]=1.0f;
580         out->m[2][3]=z;
581 }
582
583 void Matrix3x4_CreateRotate (matrix3x4_t *out, float angle, float x, float y, float z)
584 {
585         float len, c, s;
586
587         len = x*x+y*y+z*z;
588         if (len != 0.0f)
589                 len = 1.0f / sqrt(len);
590         x *= len;
591         y *= len;
592         z *= len;
593
594         angle *= M_PI / 180.0;
595         c = cos(angle);
596         s = sin(angle);
597
598         out->m[0][0]=x * x + c * (1 - x * x);
599         out->m[0][1]=x * y * (1 - c) + z * s;
600         out->m[0][2]=z * x * (1 - c) - y * s;
601         out->m[0][3]=0.0f;
602         out->m[1][0]=x * y * (1 - c) - z * s;
603         out->m[1][1]=y * y + c * (1 - y * y);
604         out->m[1][2]=y * z * (1 - c) + x * s;
605         out->m[1][3]=0.0f;
606         out->m[2][0]=z * x * (1 - c) + y * s;
607         out->m[2][1]=y * z * (1 - c) - x * s;
608         out->m[2][2]=z * z + c * (1 - z * z);
609         out->m[2][3]=0.0f;
610 }
611
612 void Matrix3x4_CreateScale (matrix3x4_t *out, float x)
613 {
614         out->m[0][0]=x;
615         out->m[0][1]=0.0f;
616         out->m[0][2]=0.0f;
617         out->m[0][3]=0.0f;
618         out->m[1][0]=0.0f;
619         out->m[1][1]=x;
620         out->m[1][2]=0.0f;
621         out->m[1][3]=0.0f;
622         out->m[2][0]=0.0f;
623         out->m[2][1]=0.0f;
624         out->m[2][2]=x;
625         out->m[2][3]=0.0f;
626 }
627
628 void Matrix3x4_CreateScale3 (matrix3x4_t *out, float x, float y, float z)
629 {
630         out->m[0][0]=x;
631         out->m[0][1]=0.0f;
632         out->m[0][2]=0.0f;
633         out->m[0][3]=0.0f;
634         out->m[1][0]=0.0f;
635         out->m[1][1]=y;
636         out->m[1][2]=0.0f;
637         out->m[1][3]=0.0f;
638         out->m[2][0]=0.0f;
639         out->m[2][1]=0.0f;
640         out->m[2][2]=z;
641         out->m[2][3]=0.0f;
642 }
643
644 void Matrix3x4_CreateFromQuakeEntity(matrix3x4_t *out, float x, float y, float z, float pitch, float yaw, float roll, float scale)
645 {
646         double angle, sr, sp, sy, cr, cp, cy;
647
648         angle = yaw * (M_PI*2 / 360);
649         sy = sin(angle);
650         cy = cos(angle);
651         angle = pitch * (M_PI*2 / 360);
652         sp = sin(angle);
653         cp = cos(angle);
654         angle = roll * (M_PI*2 / 360);
655         sr = sin(angle);
656         cr = cos(angle);
657         out->m[0][0] = (cp*cy) * scale;
658         out->m[0][1] = (sr*sp*cy+cr*-sy) * scale;
659         out->m[0][2] = (cr*sp*cy+-sr*-sy) * scale;
660         out->m[0][3] = x;
661         out->m[1][0] = (cp*sy) * scale;
662         out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
663         out->m[1][2] = (cr*sp*sy+-sr*cy) * scale;
664         out->m[1][3] = y;
665         out->m[2][0] = (-sp) * scale;
666         out->m[2][1] = (sr*cp) * scale;
667         out->m[2][2] = (cr*cp) * scale;
668         out->m[2][3] = z;
669 }
670
671 void Matrix3x4_ToVectors(const matrix3x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
672 {
673         vx[0] = in->m[0][0];
674         vx[1] = in->m[1][0];
675         vx[2] = in->m[2][0];
676         vy[0] = in->m[0][1];
677         vy[1] = in->m[1][1];
678         vy[2] = in->m[2][1];
679         vz[0] = in->m[0][2];
680         vz[1] = in->m[1][2];
681         vz[2] = in->m[2][2];
682         t[0] = in->m[0][3];
683         t[1] = in->m[1][3];
684         t[2] = in->m[2][3];
685 }
686
687 void Matrix3x4_FromVectors(matrix3x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
688 {
689         out->m[0][0] = vx[0];
690         out->m[0][1] = vy[0];
691         out->m[0][2] = vz[0];
692         out->m[0][3] = t[0];
693         out->m[1][0] = vx[1];
694         out->m[1][1] = vy[1];
695         out->m[1][2] = vz[1];
696         out->m[1][3] = t[1];
697         out->m[2][0] = vx[2];
698         out->m[2][1] = vy[2];
699         out->m[2][2] = vz[2];
700         out->m[2][3] = t[2];
701 }
702
703 void Matrix3x4_Transform (const matrix3x4_t *in, const float v[3], float out[3])
704 {
705         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
706         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
707         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
708 }
709
710 void Matrix3x4_SimpleUntransform (const matrix3x4_t *in, const float v[3], float out[3])
711 {
712         float t[3];
713         t[0] = v[0] - in->m[0][3];
714         t[1] = v[1] - in->m[1][3];
715         t[2] = v[2] - in->m[2][3];
716         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
717         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
718         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
719 }
720
721 // FIXME: optimize
722 void Matrix3x4_ConcatTranslate (matrix3x4_t *out, float x, float y, float z)
723 {
724         matrix3x4_t base, temp;
725         base = *out;
726         Matrix3x4_CreateTranslate(&temp, x, y, z);
727         Matrix3x4_Concat(out, &base, &temp);
728 }
729
730 // FIXME: optimize
731 void Matrix3x4_ConcatRotate (matrix3x4_t *out, float angle, float x, float y, float z)
732 {
733         matrix3x4_t base, temp;
734         base = *out;
735         Matrix3x4_CreateRotate(&temp, angle, x, y, z);
736         Matrix3x4_Concat(out, &base, &temp);
737 }
738
739 // FIXME: optimize
740 void Matrix3x4_ConcatScale (matrix3x4_t *out, float x)
741 {
742         matrix3x4_t base, temp;
743         base = *out;
744         Matrix3x4_CreateScale(&temp, x);
745         Matrix3x4_Concat(out, &base, &temp);
746 }
747
748 // FIXME: optimize
749 void Matrix3x4_ConcatScale3 (matrix3x4_t *out, float x, float y, float z)
750 {
751         matrix3x4_t base, temp;
752         base = *out;
753         Matrix3x4_CreateScale3(&temp, x, y, z);
754         Matrix3x4_Concat(out, &base, &temp);
755 }
756
757 void Matrix3x4_Print (const matrix3x4_t *in)
758 {
759         Con_Printf("%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n"
760         , in->m[0][0], in->m[0][1], in->m[0][2], in->m[0][3]
761         , in->m[1][0], in->m[1][1], in->m[1][2], in->m[1][3]
762         , in->m[2][0], in->m[2][1], in->m[2][2], in->m[2][3]);
763 }
764