thanks to Tomaz for finding a severe bug in CopyTranslateOnly (it was copying the...
[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[1][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[2][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 void Matrix4x4_Transform3x3 (const matrix4x4_t *in, const float v[3], float out[3])
368 {
369         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2];
370         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2];
371         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2];
372 }
373
374 /*
375 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
376 {
377         float t[3];
378         t[0] = v[0] - in->m[0][3];
379         t[1] = v[1] - in->m[1][3];
380         t[2] = v[2] - in->m[2][3];
381         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
382         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
383         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
384 }
385 */
386
387 // FIXME: optimize
388 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, float x, float y, float z)
389 {
390         matrix4x4_t base, temp;
391         base = *out;
392         Matrix4x4_CreateTranslate(&temp, x, y, z);
393         Matrix4x4_Concat(out, &base, &temp);
394 }
395
396 // FIXME: optimize
397 void Matrix4x4_ConcatRotate (matrix4x4_t *out, float angle, float x, float y, float z)
398 {
399         matrix4x4_t base, temp;
400         base = *out;
401         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
402         Matrix4x4_Concat(out, &base, &temp);
403 }
404
405 // FIXME: optimize
406 void Matrix4x4_ConcatScale (matrix4x4_t *out, float x)
407 {
408         matrix4x4_t base, temp;
409         base = *out;
410         Matrix4x4_CreateScale(&temp, x);
411         Matrix4x4_Concat(out, &base, &temp);
412 }
413
414 // FIXME: optimize
415 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, float x, float y, float z)
416 {
417         matrix4x4_t base, temp;
418         base = *out;
419         Matrix4x4_CreateScale3(&temp, x, y, z);
420         Matrix4x4_Concat(out, &base, &temp);
421 }
422
423 void Matrix4x4_Print (const matrix4x4_t *in)
424 {
425         Con_Printf("%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n"
426         , in->m[0][0], in->m[0][1], in->m[0][2], in->m[0][3]
427         , in->m[1][0], in->m[1][1], in->m[1][2], in->m[1][3]
428         , in->m[2][0], in->m[2][1], in->m[2][2], in->m[2][3]
429         , in->m[3][0], in->m[3][1], in->m[3][2], in->m[3][3]);
430 }
431
432
433
434
435
436
437
438 void Matrix3x4_Copy (matrix3x4_t *out, const matrix3x4_t *in)
439 {
440         *out = *in;
441 }
442
443 void Matrix3x4_CopyRotateOnly (matrix3x4_t *out, const matrix3x4_t *in)
444 {
445         out->m[0][0] = in->m[0][0];
446         out->m[0][1] = in->m[0][1];
447         out->m[0][2] = in->m[0][2];
448         out->m[0][3] = 0.0f;
449         out->m[1][0] = in->m[1][0];
450         out->m[1][1] = in->m[1][1];
451         out->m[1][2] = in->m[1][2];
452         out->m[1][3] = 0.0f;
453         out->m[2][0] = in->m[2][0];
454         out->m[2][1] = in->m[2][1];
455         out->m[2][2] = in->m[2][2];
456         out->m[2][3] = 0.0f;
457 }
458
459 void Matrix3x4_CopyTranslateOnly (matrix3x4_t *out, const matrix3x4_t *in)
460 {
461         out->m[0][0] = 0.0f;
462         out->m[0][1] = 0.0f;
463         out->m[0][2] = 0.0f;
464         out->m[0][3] = in->m[0][3];
465         out->m[1][0] = 0.0f;
466         out->m[1][1] = 0.0f;
467         out->m[1][2] = 0.0f;
468         out->m[1][3] = in->m[1][3];
469         out->m[2][0] = 0.0f;
470         out->m[2][1] = 0.0f;
471         out->m[2][2] = 0.0f;
472         out->m[2][3] = in->m[2][3];
473 }
474
475 void Matrix3x4_FromMatrix4x4 (matrix3x4_t *out, const matrix4x4_t *in)
476 {
477         out->m[0][0] = in->m[0][0];
478         out->m[0][1] = in->m[0][1];
479         out->m[0][2] = in->m[0][2];
480         out->m[0][3] = in->m[0][3];
481         out->m[1][0] = in->m[1][0];
482         out->m[1][1] = in->m[1][1];
483         out->m[1][2] = in->m[1][2];
484         out->m[1][3] = in->m[1][3];
485         out->m[2][0] = in->m[2][0];
486         out->m[2][1] = in->m[2][1];
487         out->m[2][2] = in->m[2][2];
488         out->m[2][3] = in->m[2][3];
489 }
490
491 void Matrix3x4_Concat (matrix3x4_t *out, const matrix3x4_t *in1, const matrix3x4_t *in2)
492 {
493         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];
494         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];
495         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];
496         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];
497         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];
498         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];
499         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];
500         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];
501         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];
502         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];
503         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];
504         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];
505 }
506
507 void Matrix3x4_Transpose3x3 (matrix3x4_t *out, const matrix3x4_t *in1)
508 {
509         out->m[0][0] = in1->m[0][0];
510         out->m[0][1] = in1->m[1][0];
511         out->m[0][2] = in1->m[2][0];
512         out->m[0][3] = 0.0f;
513         out->m[1][0] = in1->m[0][1];
514         out->m[1][1] = in1->m[1][1];
515         out->m[1][2] = in1->m[2][1];
516         out->m[1][3] = 0.0f;
517         out->m[2][0] = in1->m[0][2];
518         out->m[2][1] = in1->m[1][2];
519         out->m[2][2] = in1->m[2][2];
520         out->m[2][3] = 0.0f;
521 }
522
523 void Matrix3x4_Invert_Simple (matrix3x4_t *out, const matrix3x4_t *in1)
524 {
525         // we only support uniform scaling, so assume the first row is enough
526         // (note the lack of sqrt here, because we're trying to undo the scaling,
527         // this means multiplying by the inverse scale twice - squaring it, which
528         // makes the sqrt a waste of time)
529 #if 1
530         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]);
531 #else
532         double scale = 3.0 / sqrt
533                  (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
534                 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
535                 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
536         scale *= scale;
537 #endif
538
539         // invert the rotation by transposing and multiplying by the squared
540         // recipricol of the input matrix scale as described above
541         out->m[0][0] = in1->m[0][0] * scale;
542         out->m[0][1] = in1->m[1][0] * scale;
543         out->m[0][2] = in1->m[2][0] * scale;
544         out->m[1][0] = in1->m[0][1] * scale;
545         out->m[1][1] = in1->m[1][1] * scale;
546         out->m[1][2] = in1->m[2][1] * scale;
547         out->m[2][0] = in1->m[0][2] * scale;
548         out->m[2][1] = in1->m[1][2] * scale;
549         out->m[2][2] = in1->m[2][2] * scale;
550
551         // invert the translate
552         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]);
553         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]);
554         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]);
555 }
556
557
558 void Matrix3x4_CreateIdentity (matrix3x4_t *out)
559 {
560         out->m[0][0]=1.0f;
561         out->m[0][1]=0.0f;
562         out->m[0][2]=0.0f;
563         out->m[0][3]=0.0f;
564         out->m[1][0]=0.0f;
565         out->m[1][1]=1.0f;
566         out->m[1][2]=0.0f;
567         out->m[1][3]=0.0f;
568         out->m[2][0]=0.0f;
569         out->m[2][1]=0.0f;
570         out->m[2][2]=1.0f;
571         out->m[2][3]=0.0f;
572 }
573
574 void Matrix3x4_CreateTranslate (matrix3x4_t *out, float x, float y, float z)
575 {
576         out->m[0][0]=1.0f;
577         out->m[0][1]=0.0f;
578         out->m[0][2]=0.0f;
579         out->m[0][3]=x;
580         out->m[1][0]=0.0f;
581         out->m[1][1]=1.0f;
582         out->m[1][2]=0.0f;
583         out->m[1][3]=y;
584         out->m[2][0]=0.0f;
585         out->m[2][1]=0.0f;
586         out->m[2][2]=1.0f;
587         out->m[2][3]=z;
588 }
589
590 void Matrix3x4_CreateRotate (matrix3x4_t *out, float angle, float x, float y, float z)
591 {
592         float len, c, s;
593
594         len = x*x+y*y+z*z;
595         if (len != 0.0f)
596                 len = 1.0f / sqrt(len);
597         x *= len;
598         y *= len;
599         z *= len;
600
601         angle *= M_PI / 180.0;
602         c = cos(angle);
603         s = sin(angle);
604
605         out->m[0][0]=x * x + c * (1 - x * x);
606         out->m[0][1]=x * y * (1 - c) + z * s;
607         out->m[0][2]=z * x * (1 - c) - y * s;
608         out->m[0][3]=0.0f;
609         out->m[1][0]=x * y * (1 - c) - z * s;
610         out->m[1][1]=y * y + c * (1 - y * y);
611         out->m[1][2]=y * z * (1 - c) + x * s;
612         out->m[1][3]=0.0f;
613         out->m[2][0]=z * x * (1 - c) + y * s;
614         out->m[2][1]=y * z * (1 - c) - x * s;
615         out->m[2][2]=z * z + c * (1 - z * z);
616         out->m[2][3]=0.0f;
617 }
618
619 void Matrix3x4_CreateScale (matrix3x4_t *out, float x)
620 {
621         out->m[0][0]=x;
622         out->m[0][1]=0.0f;
623         out->m[0][2]=0.0f;
624         out->m[0][3]=0.0f;
625         out->m[1][0]=0.0f;
626         out->m[1][1]=x;
627         out->m[1][2]=0.0f;
628         out->m[1][3]=0.0f;
629         out->m[2][0]=0.0f;
630         out->m[2][1]=0.0f;
631         out->m[2][2]=x;
632         out->m[2][3]=0.0f;
633 }
634
635 void Matrix3x4_CreateScale3 (matrix3x4_t *out, float x, float y, float z)
636 {
637         out->m[0][0]=x;
638         out->m[0][1]=0.0f;
639         out->m[0][2]=0.0f;
640         out->m[0][3]=0.0f;
641         out->m[1][0]=0.0f;
642         out->m[1][1]=y;
643         out->m[1][2]=0.0f;
644         out->m[1][3]=0.0f;
645         out->m[2][0]=0.0f;
646         out->m[2][1]=0.0f;
647         out->m[2][2]=z;
648         out->m[2][3]=0.0f;
649 }
650
651 void Matrix3x4_CreateFromQuakeEntity(matrix3x4_t *out, float x, float y, float z, float pitch, float yaw, float roll, float scale)
652 {
653         double angle, sr, sp, sy, cr, cp, cy;
654
655         angle = yaw * (M_PI*2 / 360);
656         sy = sin(angle);
657         cy = cos(angle);
658         angle = pitch * (M_PI*2 / 360);
659         sp = sin(angle);
660         cp = cos(angle);
661         angle = roll * (M_PI*2 / 360);
662         sr = sin(angle);
663         cr = cos(angle);
664         out->m[0][0] = (cp*cy) * scale;
665         out->m[0][1] = (sr*sp*cy+cr*-sy) * scale;
666         out->m[0][2] = (cr*sp*cy+-sr*-sy) * scale;
667         out->m[0][3] = x;
668         out->m[1][0] = (cp*sy) * scale;
669         out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
670         out->m[1][2] = (cr*sp*sy+-sr*cy) * scale;
671         out->m[1][3] = y;
672         out->m[2][0] = (-sp) * scale;
673         out->m[2][1] = (sr*cp) * scale;
674         out->m[2][2] = (cr*cp) * scale;
675         out->m[2][3] = z;
676 }
677
678 void Matrix3x4_ToVectors(const matrix3x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
679 {
680         vx[0] = in->m[0][0];
681         vx[1] = in->m[1][0];
682         vx[2] = in->m[2][0];
683         vy[0] = in->m[0][1];
684         vy[1] = in->m[1][1];
685         vy[2] = in->m[2][1];
686         vz[0] = in->m[0][2];
687         vz[1] = in->m[1][2];
688         vz[2] = in->m[2][2];
689         t[0] = in->m[0][3];
690         t[1] = in->m[1][3];
691         t[2] = in->m[2][3];
692 }
693
694 void Matrix3x4_FromVectors(matrix3x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
695 {
696         out->m[0][0] = vx[0];
697         out->m[0][1] = vy[0];
698         out->m[0][2] = vz[0];
699         out->m[0][3] = t[0];
700         out->m[1][0] = vx[1];
701         out->m[1][1] = vy[1];
702         out->m[1][2] = vz[1];
703         out->m[1][3] = t[1];
704         out->m[2][0] = vx[2];
705         out->m[2][1] = vy[2];
706         out->m[2][2] = vz[2];
707         out->m[2][3] = t[2];
708 }
709
710 void Matrix3x4_Transform (const matrix3x4_t *in, const float v[3], float out[3])
711 {
712         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
713         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
714         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
715 }
716
717 /*
718 void Matrix3x4_SimpleUntransform (const matrix3x4_t *in, const float v[3], float out[3])
719 {
720         float t[3];
721         t[0] = v[0] - in->m[0][3];
722         t[1] = v[1] - in->m[1][3];
723         t[2] = v[2] - in->m[2][3];
724         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
725         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
726         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
727 }
728 */
729
730 void Matrix3x4_Transform3x3 (const matrix3x4_t *in, const float v[3], float out[3])
731 {
732         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
733         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
734         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
735 }
736
737
738 // FIXME: optimize
739 void Matrix3x4_ConcatTranslate (matrix3x4_t *out, float x, float y, float z)
740 {
741         matrix3x4_t base, temp;
742         base = *out;
743         Matrix3x4_CreateTranslate(&temp, x, y, z);
744         Matrix3x4_Concat(out, &base, &temp);
745 }
746
747 // FIXME: optimize
748 void Matrix3x4_ConcatRotate (matrix3x4_t *out, float angle, float x, float y, float z)
749 {
750         matrix3x4_t base, temp;
751         base = *out;
752         Matrix3x4_CreateRotate(&temp, angle, x, y, z);
753         Matrix3x4_Concat(out, &base, &temp);
754 }
755
756 // FIXME: optimize
757 void Matrix3x4_ConcatScale (matrix3x4_t *out, float x)
758 {
759         matrix3x4_t base, temp;
760         base = *out;
761         Matrix3x4_CreateScale(&temp, x);
762         Matrix3x4_Concat(out, &base, &temp);
763 }
764
765 // FIXME: optimize
766 void Matrix3x4_ConcatScale3 (matrix3x4_t *out, float x, float y, float z)
767 {
768         matrix3x4_t base, temp;
769         base = *out;
770         Matrix3x4_CreateScale3(&temp, x, y, z);
771         Matrix3x4_Concat(out, &base, &temp);
772 }
773
774 void Matrix3x4_Print (const matrix3x4_t *in)
775 {
776         Con_Printf("%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n"
777         , in->m[0][0], in->m[0][1], in->m[0][2], in->m[0][3]
778         , in->m[1][0], in->m[1][1], in->m[1][2], in->m[1][3]
779         , in->m[2][0], in->m[2][1], in->m[2][2], in->m[2][3]);
780 }
781