vertex arrays renamed to varray_ and exposed to rest of engine
[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         Matrix4x4_Copy(out, &base);
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         Matrix4x4_Copy(out, &base);
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         Matrix4x4_Copy(out, &base);
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         Matrix4x4_Copy(out, &base);
412         Matrix4x4_CreateScale3(&temp, x, y, z);
413         Matrix4x4_Concat(out, &base, &temp);
414 }
415
416
417
418
419
420
421
422
423 void Matrix3x4_Copy (matrix3x4_t *out, const matrix3x4_t *in)
424 {
425         *out = *in;
426 }
427
428 void Matrix3x4_CopyRotateOnly (matrix3x4_t *out, const matrix3x4_t *in)
429 {
430         out->m[0][0] = in->m[0][0];
431         out->m[0][1] = in->m[0][1];
432         out->m[0][2] = in->m[0][2];
433         out->m[0][3] = 0.0f;
434         out->m[1][0] = in->m[1][0];
435         out->m[1][1] = in->m[1][1];
436         out->m[1][2] = in->m[1][2];
437         out->m[1][3] = 0.0f;
438         out->m[2][0] = in->m[2][0];
439         out->m[2][1] = in->m[2][1];
440         out->m[2][2] = in->m[2][2];
441         out->m[2][3] = 0.0f;
442 }
443
444 void Matrix3x4_CopyTranslateOnly (matrix3x4_t *out, const matrix3x4_t *in)
445 {
446         out->m[0][0] = 0.0f;
447         out->m[0][1] = 0.0f;
448         out->m[0][2] = 0.0f;
449         out->m[0][3] = in->m[0][3];
450         out->m[1][0] = 0.0f;
451         out->m[1][1] = 0.0f;
452         out->m[1][2] = 0.0f;
453         out->m[1][3] = in->m[0][3];
454         out->m[2][0] = 0.0f;
455         out->m[2][1] = 0.0f;
456         out->m[2][2] = 0.0f;
457         out->m[2][3] = in->m[0][3];
458 }
459
460 void Matrix3x4_FromMatrix4x4 (matrix3x4_t *out, const matrix4x4_t *in)
461 {
462         out->m[0][0] = in->m[0][0];
463         out->m[0][1] = in->m[0][1];
464         out->m[0][2] = in->m[0][2];
465         out->m[0][3] = in->m[0][3];
466         out->m[1][0] = in->m[1][0];
467         out->m[1][1] = in->m[1][1];
468         out->m[1][2] = in->m[1][2];
469         out->m[1][3] = in->m[1][3];
470         out->m[2][0] = in->m[2][0];
471         out->m[2][1] = in->m[2][1];
472         out->m[2][2] = in->m[2][2];
473         out->m[2][3] = in->m[2][3];
474 }
475
476 void Matrix3x4_Concat (matrix3x4_t *out, const matrix3x4_t *in1, const matrix3x4_t *in2)
477 {
478         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];
479         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];
480         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];
481         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];
482         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];
483         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];
484         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];
485         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];
486         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];
487         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];
488         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];
489         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];
490 }
491
492 void Matrix3x4_Transpose3x3 (matrix3x4_t *out, const matrix3x4_t *in1)
493 {
494         out->m[0][0] = in1->m[0][0];
495         out->m[0][1] = in1->m[1][0];
496         out->m[0][2] = in1->m[2][0];
497         out->m[0][3] = 0.0f;
498         out->m[1][0] = in1->m[0][1];
499         out->m[1][1] = in1->m[1][1];
500         out->m[1][2] = in1->m[2][1];
501         out->m[1][3] = 0.0f;
502         out->m[2][0] = in1->m[0][2];
503         out->m[2][1] = in1->m[1][2];
504         out->m[2][2] = in1->m[2][2];
505         out->m[2][3] = 0.0f;
506 }
507
508 void Matrix3x4_Invert_Simple (matrix3x4_t *out, const matrix3x4_t *in1)
509 {
510         // we only support uniform scaling, so assume the first row is enough
511         // (note the lack of sqrt here, because we're trying to undo the scaling,
512         // this means multiplying by the inverse scale twice - squaring it, which
513         // makes the sqrt a waste of time)
514 #if 1
515         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]);
516 #else
517         double scale = 3.0 / sqrt
518                  (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
519                 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
520                 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
521         scale *= scale;
522 #endif
523
524         // invert the rotation by transposing and multiplying by the squared
525         // recipricol of the input matrix scale as described above
526         out->m[0][0] = in1->m[0][0] * scale;
527         out->m[0][1] = in1->m[1][0] * scale;
528         out->m[0][2] = in1->m[2][0] * scale;
529         out->m[1][0] = in1->m[0][1] * scale;
530         out->m[1][1] = in1->m[1][1] * scale;
531         out->m[1][2] = in1->m[2][1] * scale;
532         out->m[2][0] = in1->m[0][2] * scale;
533         out->m[2][1] = in1->m[1][2] * scale;
534         out->m[2][2] = in1->m[2][2] * scale;
535
536         // invert the translate
537         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]);
538         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]);
539         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]);
540 }
541
542
543 void Matrix3x4_CreateIdentity (matrix3x4_t *out)
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]=0.0f;
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]=0.0f;
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]=0.0f;
557 }
558
559 void Matrix3x4_CreateTranslate (matrix3x4_t *out, float x, float y, float z)
560 {
561         out->m[0][0]=1.0f;
562         out->m[0][1]=0.0f;
563         out->m[0][2]=0.0f;
564         out->m[0][3]=x;
565         out->m[1][0]=0.0f;
566         out->m[1][1]=1.0f;
567         out->m[1][2]=0.0f;
568         out->m[1][3]=y;
569         out->m[2][0]=0.0f;
570         out->m[2][1]=0.0f;
571         out->m[2][2]=1.0f;
572         out->m[2][3]=z;
573 }
574
575 void Matrix3x4_CreateRotate (matrix3x4_t *out, float angle, float x, float y, float z)
576 {
577         float len, c, s;
578
579         len = x*x+y*y+z*z;
580         if (len != 0.0f)
581                 len = 1.0f / sqrt(len);
582         x *= len;
583         y *= len;
584         z *= len;
585
586         angle *= M_PI / 180.0;
587         c = cos(angle);
588         s = sin(angle);
589
590         out->m[0][0]=x * x + c * (1 - x * x);
591         out->m[0][1]=x * y * (1 - c) + z * s;
592         out->m[0][2]=z * x * (1 - c) - y * s;
593         out->m[0][3]=0.0f;
594         out->m[1][0]=x * y * (1 - c) - z * s;
595         out->m[1][1]=y * y + c * (1 - y * y);
596         out->m[1][2]=y * z * (1 - c) + x * s;
597         out->m[1][3]=0.0f;
598         out->m[2][0]=z * x * (1 - c) + y * s;
599         out->m[2][1]=y * z * (1 - c) - x * s;
600         out->m[2][2]=z * z + c * (1 - z * z);
601         out->m[2][3]=0.0f;
602 }
603
604 void Matrix3x4_CreateScale (matrix3x4_t *out, float x)
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]=x;
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]=x;
617         out->m[2][3]=0.0f;
618 }
619
620 void Matrix3x4_CreateScale3 (matrix3x4_t *out, float x, float y, float z)
621 {
622         out->m[0][0]=x;
623         out->m[0][1]=0.0f;
624         out->m[0][2]=0.0f;
625         out->m[0][3]=0.0f;
626         out->m[1][0]=0.0f;
627         out->m[1][1]=y;
628         out->m[1][2]=0.0f;
629         out->m[1][3]=0.0f;
630         out->m[2][0]=0.0f;
631         out->m[2][1]=0.0f;
632         out->m[2][2]=z;
633         out->m[2][3]=0.0f;
634 }
635
636 void Matrix3x4_CreateFromQuakeEntity(matrix3x4_t *out, float x, float y, float z, float pitch, float yaw, float roll, float scale)
637 {
638         double angle, sr, sp, sy, cr, cp, cy;
639
640         angle = yaw * (M_PI*2 / 360);
641         sy = sin(angle);
642         cy = cos(angle);
643         angle = pitch * (M_PI*2 / 360);
644         sp = sin(angle);
645         cp = cos(angle);
646         angle = roll * (M_PI*2 / 360);
647         sr = sin(angle);
648         cr = cos(angle);
649         out->m[0][0] = (cp*cy) * scale;
650         out->m[0][1] = (sr*sp*cy+cr*-sy) * scale;
651         out->m[0][2] = (cr*sp*cy+-sr*-sy) * scale;
652         out->m[0][3] = x;
653         out->m[1][0] = (cp*sy) * scale;
654         out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
655         out->m[1][2] = (cr*sp*sy+-sr*cy) * scale;
656         out->m[1][3] = y;
657         out->m[2][0] = (-sp) * scale;
658         out->m[2][1] = (sr*cp) * scale;
659         out->m[2][2] = (cr*cp) * scale;
660         out->m[2][3] = z;
661 }
662
663 void Matrix3x4_ToVectors(const matrix3x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
664 {
665         vx[0] = in->m[0][0];
666         vx[1] = in->m[1][0];
667         vx[2] = in->m[2][0];
668         vy[0] = in->m[0][1];
669         vy[1] = in->m[1][1];
670         vy[2] = in->m[2][1];
671         vz[0] = in->m[0][2];
672         vz[1] = in->m[1][2];
673         vz[2] = in->m[2][2];
674         t[0] = in->m[0][3];
675         t[1] = in->m[1][3];
676         t[2] = in->m[2][3];
677 }
678
679 void Matrix3x4_FromVectors(matrix3x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
680 {
681         out->m[0][0] = vx[0];
682         out->m[0][1] = vy[0];
683         out->m[0][2] = vz[0];
684         out->m[0][3] = t[0];
685         out->m[1][0] = vx[1];
686         out->m[1][1] = vy[1];
687         out->m[1][2] = vz[1];
688         out->m[1][3] = t[1];
689         out->m[2][0] = vx[2];
690         out->m[2][1] = vy[2];
691         out->m[2][2] = vz[2];
692         out->m[2][3] = t[2];
693 }
694
695 void Matrix3x4_Transform (const matrix3x4_t *in, const float v[3], float out[3])
696 {
697         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
698         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
699         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
700 }
701
702 void Matrix3x4_SimpleUntransform (const matrix3x4_t *in, const float v[3], float out[3])
703 {
704         float t[3];
705         t[0] = v[0] - in->m[0][3];
706         t[1] = v[1] - in->m[1][3];
707         t[2] = v[2] - in->m[2][3];
708         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
709         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
710         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
711 }
712
713 // FIXME: optimize
714 void Matrix3x4_ConcatTranslate (matrix3x4_t *out, float x, float y, float z)
715 {
716         matrix3x4_t base, temp;
717         Matrix3x4_Copy(out, &base);
718         Matrix3x4_CreateTranslate(&temp, x, y, z);
719         Matrix3x4_Concat(out, &base, &temp);
720 }
721
722 // FIXME: optimize
723 void Matrix3x4_ConcatRotate (matrix3x4_t *out, float angle, float x, float y, float z)
724 {
725         matrix3x4_t base, temp;
726         Matrix3x4_Copy(out, &base);
727         Matrix3x4_CreateRotate(&temp, angle, x, y, z);
728         Matrix3x4_Concat(out, &base, &temp);
729 }
730
731 // FIXME: optimize
732 void Matrix3x4_ConcatScale (matrix3x4_t *out, float x)
733 {
734         matrix3x4_t base, temp;
735         Matrix3x4_Copy(out, &base);
736         Matrix3x4_CreateScale(&temp, x);
737         Matrix3x4_Concat(out, &base, &temp);
738 }
739
740 // FIXME: optimize
741 void Matrix3x4_ConcatScale3 (matrix3x4_t *out, float x, float y, float z)
742 {
743         matrix3x4_t base, temp;
744         Matrix3x4_Copy(out, &base);
745         Matrix3x4_CreateScale3(&temp, x, y, z);
746         Matrix3x4_Concat(out, &base, &temp);
747 }