implemented MATRIX4X4_OPENGLORIENTATION #define option, this is on by default and...
[divverent/darkplaces.git] / matrixlib.c
1
2 #include <math.h>
3 #include "matrixlib.h"
4
5 const matrix4x4_t identitymatrix =
6 {
7         {
8                 {1, 0, 0, 0},
9                 {0, 1, 0, 0},
10                 {0, 0, 1, 0},
11                 {0, 0, 0, 1}
12         }
13 };
14
15 void Matrix4x4_Copy (matrix4x4_t *out, const matrix4x4_t *in)
16 {
17         *out = *in;
18 }
19
20 void Matrix4x4_CopyRotateOnly (matrix4x4_t *out, const matrix4x4_t *in)
21 {
22         out->m[0][0] = in->m[0][0];
23         out->m[0][1] = in->m[0][1];
24         out->m[0][2] = in->m[0][2];
25         out->m[0][3] = 0.0f;
26         out->m[1][0] = in->m[1][0];
27         out->m[1][1] = in->m[1][1];
28         out->m[1][2] = in->m[1][2];
29         out->m[1][3] = 0.0f;
30         out->m[2][0] = in->m[2][0];
31         out->m[2][1] = in->m[2][1];
32         out->m[2][2] = in->m[2][2];
33         out->m[2][3] = 0.0f;
34         out->m[3][0] = 0.0f;
35         out->m[3][1] = 0.0f;
36         out->m[3][2] = 0.0f;
37         out->m[3][3] = 1.0f;
38 }
39
40 void Matrix4x4_CopyTranslateOnly (matrix4x4_t *out, const matrix4x4_t *in)
41 {
42 #ifdef MATRIX4x4_OPENGLORIENTATION
43         out->m[0][0] = 1.0f;
44         out->m[1][0] = 0.0f;
45         out->m[2][0] = 0.0f;
46         out->m[3][0] = in->m[0][3];
47         out->m[0][1] = 0.0f;
48         out->m[1][1] = 1.0f;
49         out->m[2][1] = 0.0f;
50         out->m[3][1] = in->m[1][3];
51         out->m[0][2] = 0.0f;
52         out->m[1][2] = 0.0f;
53         out->m[2][2] = 1.0f;
54         out->m[3][2] = in->m[2][3];
55         out->m[0][3] = 0.0f;
56         out->m[1][3] = 0.0f;
57         out->m[2][3] = 0.0f;
58         out->m[3][3] = 1.0f;
59 #else
60         out->m[0][0] = 1.0f;
61         out->m[0][1] = 0.0f;
62         out->m[0][2] = 0.0f;
63         out->m[0][3] = in->m[0][3];
64         out->m[1][0] = 0.0f;
65         out->m[1][1] = 1.0f;
66         out->m[1][2] = 0.0f;
67         out->m[1][3] = in->m[1][3];
68         out->m[2][0] = 0.0f;
69         out->m[2][1] = 0.0f;
70         out->m[2][2] = 1.0f;
71         out->m[2][3] = in->m[2][3];
72         out->m[3][0] = 0.0f;
73         out->m[3][1] = 0.0f;
74         out->m[3][2] = 0.0f;
75         out->m[3][3] = 1.0f;
76 #endif
77 }
78
79 void Matrix4x4_Concat (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2)
80 {
81 #ifdef MATRIX4x4_OPENGLORIENTATION
82         out->m[0][0] = in1->m[0][0] * in2->m[0][0] + in1->m[1][0] * in2->m[0][1] + in1->m[2][0] * in2->m[0][2] + in1->m[3][0] * in2->m[0][3];
83         out->m[1][0] = in1->m[0][0] * in2->m[1][0] + in1->m[1][0] * in2->m[1][1] + in1->m[2][0] * in2->m[1][2] + in1->m[3][0] * in2->m[1][3];
84         out->m[2][0] = in1->m[0][0] * in2->m[2][0] + in1->m[1][0] * in2->m[2][1] + in1->m[2][0] * in2->m[2][2] + in1->m[3][0] * in2->m[2][3];
85         out->m[3][0] = in1->m[0][0] * in2->m[3][0] + in1->m[1][0] * in2->m[3][1] + in1->m[2][0] * in2->m[3][2] + in1->m[3][0] * in2->m[3][3];
86         out->m[0][1] = in1->m[0][1] * in2->m[0][0] + in1->m[1][1] * in2->m[0][1] + in1->m[2][1] * in2->m[0][2] + in1->m[3][1] * in2->m[0][3];
87         out->m[1][1] = in1->m[0][1] * in2->m[1][0] + in1->m[1][1] * in2->m[1][1] + in1->m[2][1] * in2->m[1][2] + in1->m[3][1] * in2->m[1][3];
88         out->m[2][1] = in1->m[0][1] * in2->m[2][0] + in1->m[1][1] * in2->m[2][1] + in1->m[2][1] * in2->m[2][2] + in1->m[3][1] * in2->m[2][3];
89         out->m[3][1] = in1->m[0][1] * in2->m[3][0] + in1->m[1][1] * in2->m[3][1] + in1->m[2][1] * in2->m[3][2] + in1->m[3][1] * in2->m[3][3];
90         out->m[0][2] = in1->m[0][2] * in2->m[0][0] + in1->m[1][2] * in2->m[0][1] + in1->m[2][2] * in2->m[0][2] + in1->m[3][2] * in2->m[0][3];
91         out->m[1][2] = in1->m[0][2] * in2->m[1][0] + in1->m[1][2] * in2->m[1][1] + in1->m[2][2] * in2->m[1][2] + in1->m[3][2] * in2->m[1][3];
92         out->m[2][2] = in1->m[0][2] * in2->m[2][0] + in1->m[1][2] * in2->m[2][1] + in1->m[2][2] * in2->m[2][2] + in1->m[3][2] * in2->m[2][3];
93         out->m[3][2] = in1->m[0][2] * in2->m[3][0] + in1->m[1][2] * in2->m[3][1] + in1->m[2][2] * in2->m[3][2] + in1->m[3][2] * in2->m[3][3];
94         out->m[0][3] = in1->m[0][3] * in2->m[0][0] + in1->m[1][3] * in2->m[0][1] + in1->m[2][3] * in2->m[0][2] + in1->m[3][3] * in2->m[0][3];
95         out->m[1][3] = in1->m[0][3] * in2->m[1][0] + in1->m[1][3] * in2->m[1][1] + in1->m[2][3] * in2->m[1][2] + in1->m[3][3] * in2->m[1][3];
96         out->m[2][3] = in1->m[0][3] * in2->m[2][0] + in1->m[1][3] * in2->m[2][1] + in1->m[2][3] * in2->m[2][2] + in1->m[3][3] * in2->m[2][3];
97         out->m[3][3] = in1->m[0][3] * in2->m[3][0] + in1->m[1][3] * in2->m[3][1] + in1->m[2][3] * in2->m[3][2] + in1->m[3][3] * in2->m[3][3];
98 #else
99         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];
100         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];
101         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];
102         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];
103         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];
104         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];
105         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];
106         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];
107         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];
108         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];
109         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];
110         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];
111         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];
112         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];
113         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];
114         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];
115 #endif
116 }
117
118 void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1)
119 {
120         out->m[0][0] = in1->m[0][0];
121         out->m[0][1] = in1->m[1][0];
122         out->m[0][2] = in1->m[2][0];
123         out->m[0][3] = in1->m[3][0];
124         out->m[1][0] = in1->m[0][1];
125         out->m[1][1] = in1->m[1][1];
126         out->m[1][2] = in1->m[2][1];
127         out->m[1][3] = in1->m[3][1];
128         out->m[2][0] = in1->m[0][2];
129         out->m[2][1] = in1->m[1][2];
130         out->m[2][2] = in1->m[2][2];
131         out->m[2][3] = in1->m[3][2];
132         out->m[3][0] = in1->m[0][3];
133         out->m[3][1] = in1->m[1][3];
134         out->m[3][2] = in1->m[2][3];
135         out->m[3][3] = in1->m[3][3];
136 }
137
138 void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1)
139 {
140         // we only support uniform scaling, so assume the first row is enough
141         // (note the lack of sqrt here, because we're trying to undo the scaling,
142         // this means multiplying by the inverse scale twice - squaring it, which
143         // makes the sqrt a waste of time)
144 #if 1
145         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]);
146 #else
147         double scale = 3.0 / sqrt
148                  (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
149                 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
150                 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
151         scale *= scale;
152 #endif
153
154         // invert the rotation by transposing and multiplying by the squared
155         // recipricol of the input matrix scale as described above
156         out->m[0][0] = in1->m[0][0] * scale;
157         out->m[0][1] = in1->m[1][0] * scale;
158         out->m[0][2] = in1->m[2][0] * scale;
159         out->m[1][0] = in1->m[0][1] * scale;
160         out->m[1][1] = in1->m[1][1] * scale;
161         out->m[1][2] = in1->m[2][1] * scale;
162         out->m[2][0] = in1->m[0][2] * scale;
163         out->m[2][1] = in1->m[1][2] * scale;
164         out->m[2][2] = in1->m[2][2] * scale;
165
166 #ifdef MATRIX4x4_OPENGLORIENTATION
167         // invert the translate
168         out->m[3][0] = -(in1->m[3][0] * out->m[0][0] + in1->m[3][1] * out->m[1][0] + in1->m[3][2] * out->m[2][0]);
169         out->m[3][1] = -(in1->m[3][0] * out->m[0][1] + in1->m[3][1] * out->m[1][1] + in1->m[3][2] * out->m[2][1]);
170         out->m[3][2] = -(in1->m[3][0] * out->m[0][2] + in1->m[3][1] * out->m[1][2] + in1->m[3][2] * out->m[2][2]);
171
172         // don't know if there's anything worth doing here
173         out->m[0][3] = 0;
174         out->m[1][3] = 0;
175         out->m[2][3] = 0;
176         out->m[3][3] = 1;
177 #else
178         // invert the translate
179         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]);
180         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]);
181         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]);
182
183         // don't know if there's anything worth doing here
184         out->m[3][0] = 0;
185         out->m[3][1] = 0;
186         out->m[3][2] = 0;
187         out->m[3][3] = 1;
188 #endif
189 }
190
191 void Matrix4x4_Normalize (matrix4x4_t *out, matrix4x4_t *in1)
192 {
193         // scale rotation matrix vectors to a length of 1
194         // note: this is only designed to undo uniform scaling
195         double scale = 1.0 / sqrt(in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]);
196         *out = *in1;
197         Matrix4x4_Scale(out, scale, 1);
198 }
199
200 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
201 {
202         out->m[0][0]=1.0f;
203         out->m[0][1]=0.0f;
204         out->m[0][2]=0.0f;
205         out->m[0][3]=0.0f;
206         out->m[1][0]=0.0f;
207         out->m[1][1]=1.0f;
208         out->m[1][2]=0.0f;
209         out->m[1][3]=0.0f;
210         out->m[2][0]=0.0f;
211         out->m[2][1]=0.0f;
212         out->m[2][2]=1.0f;
213         out->m[2][3]=0.0f;
214         out->m[3][0]=0.0f;
215         out->m[3][1]=0.0f;
216         out->m[3][2]=0.0f;
217         out->m[3][3]=1.0f;
218 }
219
220 void Matrix4x4_CreateTranslate (matrix4x4_t *out, double x, double y, double z)
221 {
222 #ifdef MATRIX4x4_OPENGLORIENTATION
223         out->m[0][0]=1.0f;
224         out->m[1][0]=0.0f;
225         out->m[2][0]=0.0f;
226         out->m[3][0]=x;
227         out->m[0][1]=0.0f;
228         out->m[1][1]=1.0f;
229         out->m[2][1]=0.0f;
230         out->m[3][1]=y;
231         out->m[0][2]=0.0f;
232         out->m[1][2]=0.0f;
233         out->m[2][2]=1.0f;
234         out->m[3][2]=z;
235         out->m[0][3]=0.0f;
236         out->m[1][3]=0.0f;
237         out->m[2][3]=0.0f;
238         out->m[3][3]=1.0f;
239 #else
240         out->m[0][0]=1.0f;
241         out->m[0][1]=0.0f;
242         out->m[0][2]=0.0f;
243         out->m[0][3]=x;
244         out->m[1][0]=0.0f;
245         out->m[1][1]=1.0f;
246         out->m[1][2]=0.0f;
247         out->m[1][3]=y;
248         out->m[2][0]=0.0f;
249         out->m[2][1]=0.0f;
250         out->m[2][2]=1.0f;
251         out->m[2][3]=z;
252         out->m[3][0]=0.0f;
253         out->m[3][1]=0.0f;
254         out->m[3][2]=0.0f;
255         out->m[3][3]=1.0f;
256 #endif
257 }
258
259 void Matrix4x4_CreateRotate (matrix4x4_t *out, double angle, double x, double y, double z)
260 {
261         double len, c, s;
262
263         len = x*x+y*y+z*z;
264         if (len != 0.0f)
265                 len = 1.0f / sqrt(len);
266         x *= len;
267         y *= len;
268         z *= len;
269
270         angle *= (-M_PI / 180.0);
271         c = cos(angle);
272         s = sin(angle);
273
274 #ifdef MATRIX4x4_OPENGLORIENTATION
275         out->m[0][0]=x * x + c * (1 - x * x);
276         out->m[1][0]=x * y * (1 - c) + z * s;
277         out->m[2][0]=z * x * (1 - c) - y * s;
278         out->m[3][0]=0.0f;
279         out->m[0][1]=x * y * (1 - c) - z * s;
280         out->m[1][1]=y * y + c * (1 - y * y);
281         out->m[2][1]=y * z * (1 - c) + x * s;
282         out->m[3][1]=0.0f;
283         out->m[0][2]=z * x * (1 - c) + y * s;
284         out->m[1][2]=y * z * (1 - c) - x * s;
285         out->m[2][2]=z * z + c * (1 - z * z);
286         out->m[3][2]=0.0f;
287         out->m[0][3]=0.0f;
288         out->m[1][3]=0.0f;
289         out->m[2][3]=0.0f;
290         out->m[3][3]=1.0f;
291 #else
292         out->m[0][0]=x * x + c * (1 - x * x);
293         out->m[0][1]=x * y * (1 - c) + z * s;
294         out->m[0][2]=z * x * (1 - c) - y * s;
295         out->m[0][3]=0.0f;
296         out->m[1][0]=x * y * (1 - c) - z * s;
297         out->m[1][1]=y * y + c * (1 - y * y);
298         out->m[1][2]=y * z * (1 - c) + x * s;
299         out->m[1][3]=0.0f;
300         out->m[2][0]=z * x * (1 - c) + y * s;
301         out->m[2][1]=y * z * (1 - c) - x * s;
302         out->m[2][2]=z * z + c * (1 - z * z);
303         out->m[2][3]=0.0f;
304         out->m[3][0]=0.0f;
305         out->m[3][1]=0.0f;
306         out->m[3][2]=0.0f;
307         out->m[3][3]=1.0f;
308 #endif
309 }
310
311 void Matrix4x4_CreateScale (matrix4x4_t *out, double x)
312 {
313         out->m[0][0]=x;
314         out->m[0][1]=0.0f;
315         out->m[0][2]=0.0f;
316         out->m[0][3]=0.0f;
317         out->m[1][0]=0.0f;
318         out->m[1][1]=x;
319         out->m[1][2]=0.0f;
320         out->m[1][3]=0.0f;
321         out->m[2][0]=0.0f;
322         out->m[2][1]=0.0f;
323         out->m[2][2]=x;
324         out->m[2][3]=0.0f;
325         out->m[3][0]=0.0f;
326         out->m[3][1]=0.0f;
327         out->m[3][2]=0.0f;
328         out->m[3][3]=1.0f;
329 }
330
331 void Matrix4x4_CreateScale3 (matrix4x4_t *out, double x, double y, double z)
332 {
333         out->m[0][0]=x;
334         out->m[0][1]=0.0f;
335         out->m[0][2]=0.0f;
336         out->m[0][3]=0.0f;
337         out->m[1][0]=0.0f;
338         out->m[1][1]=y;
339         out->m[1][2]=0.0f;
340         out->m[1][3]=0.0f;
341         out->m[2][0]=0.0f;
342         out->m[2][1]=0.0f;
343         out->m[2][2]=z;
344         out->m[2][3]=0.0f;
345         out->m[3][0]=0.0f;
346         out->m[3][1]=0.0f;
347         out->m[3][2]=0.0f;
348         out->m[3][3]=1.0f;
349 }
350
351 void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, double x, double y, double z, double pitch, double yaw, double roll, double scale)
352 {
353         double angle, sr, sp, sy, cr, cp, cy;
354
355         if (roll)
356         {
357                 angle = yaw * (M_PI*2 / 360);
358                 sy = sin(angle);
359                 cy = cos(angle);
360                 angle = pitch * (M_PI*2 / 360);
361                 sp = sin(angle);
362                 cp = cos(angle);
363                 angle = roll * (M_PI*2 / 360);
364                 sr = sin(angle);
365                 cr = cos(angle);
366 #ifdef MATRIX4x4_OPENGLORIENTATION
367                 out->m[0][0] = (cp*cy) * scale;
368                 out->m[1][0] = (sr*sp*cy+cr*-sy) * scale;
369                 out->m[2][0] = (cr*sp*cy+-sr*-sy) * scale;
370                 out->m[3][0] = x;
371                 out->m[0][1] = (cp*sy) * scale;
372                 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
373                 out->m[2][1] = (cr*sp*sy+-sr*cy) * scale;
374                 out->m[3][1] = y;
375                 out->m[0][2] = (-sp) * scale;
376                 out->m[1][2] = (sr*cp) * scale;
377                 out->m[2][2] = (cr*cp) * scale;
378                 out->m[3][2] = z;
379                 out->m[0][3] = 0;
380                 out->m[1][3] = 0;
381                 out->m[2][3] = 0;
382                 out->m[3][3] = 1;
383 #else
384                 out->m[0][0] = (cp*cy) * scale;
385                 out->m[0][1] = (sr*sp*cy+cr*-sy) * scale;
386                 out->m[0][2] = (cr*sp*cy+-sr*-sy) * scale;
387                 out->m[0][3] = x;
388                 out->m[1][0] = (cp*sy) * scale;
389                 out->m[1][1] = (sr*sp*sy+cr*cy) * scale;
390                 out->m[1][2] = (cr*sp*sy+-sr*cy) * scale;
391                 out->m[1][3] = y;
392                 out->m[2][0] = (-sp) * scale;
393                 out->m[2][1] = (sr*cp) * scale;
394                 out->m[2][2] = (cr*cp) * scale;
395                 out->m[2][3] = z;
396                 out->m[3][0] = 0;
397                 out->m[3][1] = 0;
398                 out->m[3][2] = 0;
399                 out->m[3][3] = 1;
400 #endif
401         }
402         else if (pitch)
403         {
404                 angle = yaw * (M_PI*2 / 360);
405                 sy = sin(angle);
406                 cy = cos(angle);
407                 angle = pitch * (M_PI*2 / 360);
408                 sp = sin(angle);
409                 cp = cos(angle);
410 #ifdef MATRIX4x4_OPENGLORIENTATION
411                 out->m[0][0] = (cp*cy) * scale;
412                 out->m[1][0] = (-sy) * scale;
413                 out->m[2][0] = (sp*cy) * scale;
414                 out->m[3][0] = x;
415                 out->m[0][1] = (cp*sy) * scale;
416                 out->m[1][1] = (cy) * scale;
417                 out->m[2][1] = (sp*sy) * scale;
418                 out->m[3][1] = y;
419                 out->m[0][2] = (-sp) * scale;
420                 out->m[1][2] = 0;
421                 out->m[2][2] = (cp) * scale;
422                 out->m[3][2] = z;
423                 out->m[0][3] = 0;
424                 out->m[1][3] = 0;
425                 out->m[2][3] = 0;
426                 out->m[3][3] = 1;
427 #else
428                 out->m[0][0] = (cp*cy) * scale;
429                 out->m[0][1] = (-sy) * scale;
430                 out->m[0][2] = (sp*cy) * scale;
431                 out->m[0][3] = x;
432                 out->m[1][0] = (cp*sy) * scale;
433                 out->m[1][1] = (cy) * scale;
434                 out->m[1][2] = (sp*sy) * scale;
435                 out->m[1][3] = y;
436                 out->m[2][0] = (-sp) * scale;
437                 out->m[2][1] = 0;
438                 out->m[2][2] = (cp) * scale;
439                 out->m[2][3] = z;
440                 out->m[3][0] = 0;
441                 out->m[3][1] = 0;
442                 out->m[3][2] = 0;
443                 out->m[3][3] = 1;
444 #endif
445         }
446         else if (yaw)
447         {
448                 angle = yaw * (M_PI*2 / 360);
449                 sy = sin(angle);
450                 cy = cos(angle);
451 #ifdef MATRIX4x4_OPENGLORIENTATION
452                 out->m[0][0] = (cy) * scale;
453                 out->m[1][0] = (-sy) * scale;
454                 out->m[2][0] = 0;
455                 out->m[3][0] = x;
456                 out->m[0][1] = (sy) * scale;
457                 out->m[1][1] = (cy) * scale;
458                 out->m[2][1] = 0;
459                 out->m[3][1] = y;
460                 out->m[0][2] = 0;
461                 out->m[1][2] = 0;
462                 out->m[2][2] = scale;
463                 out->m[3][2] = z;
464                 out->m[0][3] = 0;
465                 out->m[1][3] = 0;
466                 out->m[2][3] = 0;
467                 out->m[3][3] = 1;
468 #else
469                 out->m[0][0] = (cy) * scale;
470                 out->m[0][1] = (-sy) * scale;
471                 out->m[0][2] = 0;
472                 out->m[0][3] = x;
473                 out->m[1][0] = (sy) * scale;
474                 out->m[1][1] = (cy) * scale;
475                 out->m[1][2] = 0;
476                 out->m[1][3] = y;
477                 out->m[2][0] = 0;
478                 out->m[2][1] = 0;
479                 out->m[2][2] = scale;
480                 out->m[2][3] = z;
481                 out->m[3][0] = 0;
482                 out->m[3][1] = 0;
483                 out->m[3][2] = 0;
484                 out->m[3][3] = 1;
485 #endif
486         }
487         else
488         {
489 #ifdef MATRIX4x4_OPENGLORIENTATION
490                 out->m[0][0] = scale;
491                 out->m[1][0] = 0;
492                 out->m[2][0] = 0;
493                 out->m[3][0] = x;
494                 out->m[0][1] = 0;
495                 out->m[1][1] = scale;
496                 out->m[2][1] = 0;
497                 out->m[3][1] = y;
498                 out->m[0][2] = 0;
499                 out->m[1][2] = 0;
500                 out->m[2][2] = scale;
501                 out->m[3][2] = z;
502                 out->m[0][3] = 0;
503                 out->m[1][3] = 0;
504                 out->m[2][3] = 0;
505                 out->m[3][3] = 1;
506 #else
507                 out->m[0][0] = scale;
508                 out->m[0][1] = 0;
509                 out->m[0][2] = 0;
510                 out->m[0][3] = x;
511                 out->m[1][0] = 0;
512                 out->m[1][1] = scale;
513                 out->m[1][2] = 0;
514                 out->m[1][3] = y;
515                 out->m[2][0] = 0;
516                 out->m[2][1] = 0;
517                 out->m[2][2] = scale;
518                 out->m[2][3] = z;
519                 out->m[3][0] = 0;
520                 out->m[3][1] = 0;
521                 out->m[3][2] = 0;
522                 out->m[3][3] = 1;
523 #endif
524         }
525 }
526
527 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
528 {
529 #ifdef MATRIX4x4_OPENGLORIENTATION
530         vx[0] = in->m[0][0];
531         vx[1] = in->m[0][1];
532         vx[2] = in->m[0][2];
533         vy[0] = in->m[1][0];
534         vy[1] = in->m[1][1];
535         vy[2] = in->m[1][2];
536         vz[0] = in->m[2][0];
537         vz[1] = in->m[2][1];
538         vz[2] = in->m[2][2];
539         t [0] = in->m[3][0];
540         t [1] = in->m[3][1];
541         t [2] = in->m[3][2];
542 #else
543         vx[0] = in->m[0][0];
544         vx[1] = in->m[1][0];
545         vx[2] = in->m[2][0];
546         vy[0] = in->m[0][1];
547         vy[1] = in->m[1][1];
548         vy[2] = in->m[2][1];
549         vz[0] = in->m[0][2];
550         vz[1] = in->m[1][2];
551         vz[2] = in->m[2][2];
552         t [0] = in->m[0][3];
553         t [1] = in->m[1][3];
554         t [2] = in->m[2][3];
555 #endif
556 }
557
558 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
559 {
560 #ifdef MATRIX4x4_OPENGLORIENTATION
561         out->m[0][0] = vx[0];
562         out->m[1][0] = vy[0];
563         out->m[2][0] = vz[0];
564         out->m[3][0] = t[0];
565         out->m[0][1] = vx[1];
566         out->m[1][1] = vy[1];
567         out->m[2][1] = vz[1];
568         out->m[3][1] = t[1];
569         out->m[0][2] = vx[2];
570         out->m[1][2] = vy[2];
571         out->m[2][2] = vz[2];
572         out->m[3][2] = t[2];
573         out->m[0][3] = 0.0f;
574         out->m[1][3] = 0.0f;
575         out->m[2][3] = 0.0f;
576         out->m[3][3] = 1.0f;
577 #else
578         out->m[0][0] = vx[0];
579         out->m[0][1] = vy[0];
580         out->m[0][2] = vz[0];
581         out->m[0][3] = t[0];
582         out->m[1][0] = vx[1];
583         out->m[1][1] = vy[1];
584         out->m[1][2] = vz[1];
585         out->m[1][3] = t[1];
586         out->m[2][0] = vx[2];
587         out->m[2][1] = vy[2];
588         out->m[2][2] = vz[2];
589         out->m[2][3] = t[2];
590         out->m[3][0] = 0.0f;
591         out->m[3][1] = 0.0f;
592         out->m[3][2] = 0.0f;
593         out->m[3][3] = 1.0f;
594 #endif
595 }
596
597 void Matrix4x4_ToArrayDoubleGL(const matrix4x4_t *in, double out[16])
598 {
599 #ifdef MATRIX4x4_OPENGLORIENTATION
600         out[ 0] = in->m[0][0];
601         out[ 1] = in->m[0][1];
602         out[ 2] = in->m[0][2];
603         out[ 3] = in->m[0][3];
604         out[ 4] = in->m[1][0];
605         out[ 5] = in->m[1][1];
606         out[ 6] = in->m[1][2];
607         out[ 7] = in->m[1][3];
608         out[ 8] = in->m[2][0];
609         out[ 9] = in->m[2][1];
610         out[10] = in->m[2][2];
611         out[11] = in->m[2][3];
612         out[12] = in->m[3][0];
613         out[13] = in->m[3][1];
614         out[14] = in->m[3][2];
615         out[15] = in->m[3][3];
616 #else
617         out[ 0] = in->m[0][0];
618         out[ 1] = in->m[1][0];
619         out[ 2] = in->m[2][0];
620         out[ 3] = in->m[3][0];
621         out[ 4] = in->m[0][1];
622         out[ 5] = in->m[1][1];
623         out[ 6] = in->m[2][1];
624         out[ 7] = in->m[3][1];
625         out[ 8] = in->m[0][2];
626         out[ 9] = in->m[1][2];
627         out[10] = in->m[2][2];
628         out[11] = in->m[3][2];
629         out[12] = in->m[0][3];
630         out[13] = in->m[1][3];
631         out[14] = in->m[2][3];
632         out[15] = in->m[3][3];
633 #endif
634 }
635
636 void Matrix4x4_FromArrayDoubleGL (matrix4x4_t *out, const double in[16])
637 {
638 #ifdef MATRIX4x4_OPENGLORIENTATION
639         out->m[0][0] = in[0];
640         out->m[0][1] = in[1];
641         out->m[0][2] = in[2];
642         out->m[0][3] = in[3];
643         out->m[1][0] = in[4];
644         out->m[1][1] = in[5];
645         out->m[1][2] = in[6];
646         out->m[1][3] = in[7];
647         out->m[2][0] = in[8];
648         out->m[2][1] = in[9];
649         out->m[2][2] = in[10];
650         out->m[2][3] = in[11];
651         out->m[3][0] = in[12];
652         out->m[3][1] = in[13];
653         out->m[3][2] = in[14];
654         out->m[3][3] = in[15];
655 #else
656         out->m[0][0] = in[0];
657         out->m[1][0] = in[1];
658         out->m[2][0] = in[2];
659         out->m[3][0] = in[3];
660         out->m[0][1] = in[4];
661         out->m[1][1] = in[5];
662         out->m[2][1] = in[6];
663         out->m[3][1] = in[7];
664         out->m[0][2] = in[8];
665         out->m[1][2] = in[9];
666         out->m[2][2] = in[10];
667         out->m[3][2] = in[11];
668         out->m[0][3] = in[12];
669         out->m[1][3] = in[13];
670         out->m[2][3] = in[14];
671         out->m[3][3] = in[15];
672 #endif
673 }
674
675 void Matrix4x4_ToArrayDoubleD3D(const matrix4x4_t *in, double out[16])
676 {
677 #ifdef MATRIX4x4_OPENGLORIENTATION
678         out[ 0] = in->m[0][0];
679         out[ 1] = in->m[1][0];
680         out[ 2] = in->m[2][0];
681         out[ 3] = in->m[3][0];
682         out[ 4] = in->m[0][1];
683         out[ 5] = in->m[1][1];
684         out[ 6] = in->m[2][1];
685         out[ 7] = in->m[3][1];
686         out[ 8] = in->m[0][2];
687         out[ 9] = in->m[1][2];
688         out[10] = in->m[2][2];
689         out[11] = in->m[3][2];
690         out[12] = in->m[0][3];
691         out[13] = in->m[1][3];
692         out[14] = in->m[2][3];
693         out[15] = in->m[3][3];
694 #else
695         out[ 0] = in->m[0][0];
696         out[ 1] = in->m[0][1];
697         out[ 2] = in->m[0][2];
698         out[ 3] = in->m[0][3];
699         out[ 4] = in->m[1][0];
700         out[ 5] = in->m[1][1];
701         out[ 6] = in->m[1][2];
702         out[ 7] = in->m[1][3];
703         out[ 8] = in->m[2][0];
704         out[ 9] = in->m[2][1];
705         out[10] = in->m[2][2];
706         out[11] = in->m[2][3];
707         out[12] = in->m[3][0];
708         out[13] = in->m[3][1];
709         out[14] = in->m[3][2];
710         out[15] = in->m[3][3];
711 #endif
712 }
713
714 void Matrix4x4_FromArrayDoubleD3D (matrix4x4_t *out, const double in[16])
715 {
716 #ifdef MATRIX4x4_OPENGLORIENTATION
717         out->m[0][0] = in[0];
718         out->m[1][0] = in[1];
719         out->m[2][0] = in[2];
720         out->m[3][0] = in[3];
721         out->m[0][1] = in[4];
722         out->m[1][1] = in[5];
723         out->m[2][1] = in[6];
724         out->m[3][1] = in[7];
725         out->m[0][2] = in[8];
726         out->m[1][2] = in[9];
727         out->m[2][2] = in[10];
728         out->m[3][2] = in[11];
729         out->m[0][3] = in[12];
730         out->m[1][3] = in[13];
731         out->m[2][3] = in[14];
732         out->m[3][3] = in[15];
733 #else
734         out->m[0][0] = in[0];
735         out->m[0][1] = in[1];
736         out->m[0][2] = in[2];
737         out->m[0][3] = in[3];
738         out->m[1][0] = in[4];
739         out->m[1][1] = in[5];
740         out->m[1][2] = in[6];
741         out->m[1][3] = in[7];
742         out->m[2][0] = in[8];
743         out->m[2][1] = in[9];
744         out->m[2][2] = in[10];
745         out->m[2][3] = in[11];
746         out->m[3][0] = in[12];
747         out->m[3][1] = in[13];
748         out->m[3][2] = in[14];
749         out->m[3][3] = in[15];
750 #endif
751 }
752
753 void Matrix4x4_ToArray12FloatGL(const matrix4x4_t *in, float out[12])
754 {
755 #ifdef MATRIX4x4_OPENGLORIENTATION
756         out[ 0] = in->m[0][0];
757         out[ 1] = in->m[0][1];
758         out[ 2] = in->m[0][2];
759         out[ 3] = in->m[1][0];
760         out[ 4] = in->m[1][1];
761         out[ 5] = in->m[1][2];
762         out[ 6] = in->m[2][0];
763         out[ 7] = in->m[2][1];
764         out[ 8] = in->m[2][2];
765         out[ 9] = in->m[3][0];
766         out[10] = in->m[3][1];
767         out[11] = in->m[3][2];
768 #else
769         out[ 0] = in->m[0][0];
770         out[ 1] = in->m[1][0];
771         out[ 2] = in->m[2][0];
772         out[ 3] = in->m[0][1];
773         out[ 4] = in->m[1][1];
774         out[ 5] = in->m[2][1];
775         out[ 6] = in->m[0][2];
776         out[ 7] = in->m[1][2];
777         out[ 8] = in->m[2][2];
778         out[ 9] = in->m[0][3];
779         out[10] = in->m[1][3];
780         out[11] = in->m[2][3];
781 #endif
782 }
783
784 void Matrix4x4_FromArray12FloatGL(matrix4x4_t *out, const float in[12])
785 {
786 #ifdef MATRIX4x4_OPENGLORIENTATION
787         out->m[0][0] = in[0];
788         out->m[0][1] = in[1];
789         out->m[0][2] = in[2];
790         out->m[0][3] = 0;
791         out->m[1][0] = in[3];
792         out->m[1][1] = in[4];
793         out->m[1][2] = in[5];
794         out->m[1][3] = 0;
795         out->m[2][0] = in[6];
796         out->m[2][1] = in[7];
797         out->m[2][2] = in[8];
798         out->m[2][3] = 0;
799         out->m[3][0] = in[9];
800         out->m[3][1] = in[10];
801         out->m[3][2] = in[11];
802         out->m[3][3] = 1;
803 #else
804         out->m[0][0] = in[0];
805         out->m[1][0] = in[1];
806         out->m[2][0] = in[2];
807         out->m[3][0] = 0;
808         out->m[0][1] = in[3];
809         out->m[1][1] = in[4];
810         out->m[2][1] = in[5];
811         out->m[3][1] = 0;
812         out->m[0][2] = in[6];
813         out->m[1][2] = in[7];
814         out->m[2][2] = in[8];
815         out->m[3][2] = 0;
816         out->m[0][3] = in[9];
817         out->m[1][3] = in[10];
818         out->m[2][3] = in[11];
819         out->m[3][3] = 1;
820 #endif
821 }
822
823 void Matrix4x4_ToArray12FloatD3D(const matrix4x4_t *in, float out[12])
824 {
825 #ifdef MATRIX4x4_OPENGLORIENTATION
826         out[ 0] = in->m[0][0];
827         out[ 1] = in->m[1][0];
828         out[ 2] = in->m[2][0];
829         out[ 3] = in->m[3][0];
830         out[ 4] = in->m[0][1];
831         out[ 5] = in->m[1][1];
832         out[ 6] = in->m[2][1];
833         out[ 7] = in->m[3][1];
834         out[ 8] = in->m[0][2];
835         out[ 9] = in->m[1][2];
836         out[10] = in->m[2][2];
837         out[11] = in->m[3][2];
838 #else
839         out[ 0] = in->m[0][0];
840         out[ 1] = in->m[0][1];
841         out[ 2] = in->m[0][2];
842         out[ 3] = in->m[0][3];
843         out[ 4] = in->m[1][0];
844         out[ 5] = in->m[1][1];
845         out[ 6] = in->m[1][2];
846         out[ 7] = in->m[1][3];
847         out[ 8] = in->m[2][0];
848         out[ 9] = in->m[2][1];
849         out[10] = in->m[2][2];
850         out[11] = in->m[2][3];
851 #endif
852 }
853
854 void Matrix4x4_FromArray12FloatD3D(matrix4x4_t *out, const float in[12])
855 {
856 #ifdef MATRIX4x4_OPENGLORIENTATION
857         out->m[0][0] = in[0];
858         out->m[1][0] = in[1];
859         out->m[2][0] = in[2];
860         out->m[3][0] = in[3];
861         out->m[0][1] = in[4];
862         out->m[1][1] = in[5];
863         out->m[2][1] = in[6];
864         out->m[3][1] = in[7];
865         out->m[0][2] = in[8];
866         out->m[1][2] = in[9];
867         out->m[2][2] = in[10];
868         out->m[3][2] = in[11];
869         out->m[0][3] = 0;
870         out->m[1][3] = 0;
871         out->m[2][3] = 0;
872         out->m[3][3] = 1;
873 #else
874         out->m[0][0] = in[0];
875         out->m[0][1] = in[1];
876         out->m[0][2] = in[2];
877         out->m[0][3] = in[3];
878         out->m[1][0] = in[4];
879         out->m[1][1] = in[5];
880         out->m[1][2] = in[6];
881         out->m[1][3] = in[7];
882         out->m[2][0] = in[8];
883         out->m[2][1] = in[9];
884         out->m[2][2] = in[10];
885         out->m[2][3] = in[11];
886         out->m[3][0] = 0;
887         out->m[3][1] = 0;
888         out->m[3][2] = 0;
889         out->m[3][3] = 1;
890 #endif
891 }
892
893 void Matrix4x4_FromOriginQuat(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z, double w)
894 {
895 #ifdef MATRIX4x4_OPENGLORIENTATION
896         m->m[0][0]=1-2*(y*y+z*z);m->m[1][0]=  2*(x*y-z*w);m->m[2][0]=  2*(x*z+y*w);m->m[3][0]=ox;
897         m->m[0][1]=  2*(x*y+z*w);m->m[1][1]=1-2*(x*x+z*z);m->m[2][1]=  2*(y*z-x*w);m->m[3][1]=oy;
898         m->m[0][2]=  2*(x*z-y*w);m->m[1][2]=  2*(y*z+x*w);m->m[2][2]=1-2*(x*x+y*y);m->m[3][2]=oz;
899         m->m[0][3]=  0          ;m->m[1][3]=  0          ;m->m[2][3]=  0          ;m->m[3][3]=1;
900 #else
901         m->m[0][0]=1-2*(y*y+z*z);m->m[0][1]=  2*(x*y-z*w);m->m[0][2]=  2*(x*z+y*w);m->m[0][3]=ox;
902         m->m[1][0]=  2*(x*y+z*w);m->m[1][1]=1-2*(x*x+z*z);m->m[1][2]=  2*(y*z-x*w);m->m[1][3]=oy;
903         m->m[2][0]=  2*(x*z-y*w);m->m[2][1]=  2*(y*z+x*w);m->m[2][2]=1-2*(x*x+y*y);m->m[2][3]=oz;
904         m->m[3][0]=  0          ;m->m[3][1]=  0          ;m->m[3][2]=  0          ;m->m[3][3]=1;
905 #endif
906 }
907
908 // LordHavoc: I got this code from:
909 //http://www.doom3world.org/phpbb2/viewtopic.php?t=2884
910 void Matrix4x4_FromDoom3Joint(matrix4x4_t *m, double ox, double oy, double oz, double x, double y, double z)
911 {
912         double w = 1.0 - (x*x+y*y+z*z);
913         w = w > 0.0 ? -sqrt(w) : 0.0;
914 #ifdef MATRIX4x4_OPENGLORIENTATION
915         m->m[0][0]=1-2*(y*y+z*z);m->m[1][0]=  2*(x*y-z*w);m->m[2][0]=  2*(x*z+y*w);m->m[3][0]=ox;
916         m->m[0][1]=  2*(x*y+z*w);m->m[1][1]=1-2*(x*x+z*z);m->m[2][1]=  2*(y*z-x*w);m->m[3][1]=oy;
917         m->m[0][2]=  2*(x*z-y*w);m->m[1][2]=  2*(y*z+x*w);m->m[2][2]=1-2*(x*x+y*y);m->m[3][2]=oz;
918         m->m[0][3]=  0          ;m->m[1][3]=  0          ;m->m[2][3]=  0          ;m->m[3][3]=1;
919 #else
920         m->m[0][0]=1-2*(y*y+z*z);m->m[0][1]=  2*(x*y-z*w);m->m[0][2]=  2*(x*z+y*w);m->m[0][3]=ox;
921         m->m[1][0]=  2*(x*y+z*w);m->m[1][1]=1-2*(x*x+z*z);m->m[1][2]=  2*(y*z-x*w);m->m[1][3]=oy;
922         m->m[2][0]=  2*(x*z-y*w);m->m[2][1]=  2*(y*z+x*w);m->m[2][2]=1-2*(x*x+y*y);m->m[2][3]=oz;
923         m->m[3][0]=  0          ;m->m[3][1]=  0          ;m->m[3][2]=  0          ;m->m[3][3]=1;
924 #endif
925 }
926
927 void Matrix4x4_Blend (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2, double blend)
928 {
929         double iblend = 1 - blend;
930         out->m[0][0] = in1->m[0][0] * iblend + in2->m[0][0] * blend;
931         out->m[0][1] = in1->m[0][1] * iblend + in2->m[0][1] * blend;
932         out->m[0][2] = in1->m[0][2] * iblend + in2->m[0][2] * blend;
933         out->m[0][3] = in1->m[0][3] * iblend + in2->m[0][3] * blend;
934         out->m[1][0] = in1->m[1][0] * iblend + in2->m[1][0] * blend;
935         out->m[1][1] = in1->m[1][1] * iblend + in2->m[1][1] * blend;
936         out->m[1][2] = in1->m[1][2] * iblend + in2->m[1][2] * blend;
937         out->m[1][3] = in1->m[1][3] * iblend + in2->m[1][3] * blend;
938         out->m[2][0] = in1->m[2][0] * iblend + in2->m[2][0] * blend;
939         out->m[2][1] = in1->m[2][1] * iblend + in2->m[2][1] * blend;
940         out->m[2][2] = in1->m[2][2] * iblend + in2->m[2][2] * blend;
941         out->m[2][3] = in1->m[2][3] * iblend + in2->m[2][3] * blend;
942         out->m[3][0] = in1->m[3][0] * iblend + in2->m[3][0] * blend;
943         out->m[3][1] = in1->m[3][1] * iblend + in2->m[3][1] * blend;
944         out->m[3][2] = in1->m[3][2] * iblend + in2->m[3][2] * blend;
945         out->m[3][3] = in1->m[3][3] * iblend + in2->m[3][3] * blend;
946 }
947
948
949 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
950 {
951 #ifdef MATRIX4x4_OPENGLORIENTATION
952         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0] + in->m[3][0];
953         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1] + in->m[3][1];
954         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2] + in->m[3][2];
955 #else
956         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
957         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
958         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
959 #endif
960 }
961
962 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
963 {
964 #ifdef MATRIX4x4_OPENGLORIENTATION
965         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0] + v[3] * in->m[3][0];
966         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1] + v[3] * in->m[3][1];
967         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2] + v[3] * in->m[3][2];
968         out[3] = v[0] * in->m[0][3] + v[1] * in->m[1][3] + v[2] * in->m[2][3] + v[3] * in->m[3][3];
969 #else
970         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];
971         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];
972         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];
973         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];
974 #endif
975 }
976
977 void Matrix4x4_Transform3x3 (const matrix4x4_t *in, const float v[3], float out[3])
978 {
979 #ifdef MATRIX4x4_OPENGLORIENTATION
980         out[0] = v[0] * in->m[0][0] + v[1] * in->m[1][0] + v[2] * in->m[2][0];
981         out[1] = v[0] * in->m[0][1] + v[1] * in->m[1][1] + v[2] * in->m[2][1];
982         out[2] = v[0] * in->m[0][2] + v[1] * in->m[1][2] + v[2] * in->m[2][2];
983 #else
984         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2];
985         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2];
986         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2];
987 #endif
988 }
989
990 /*
991 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
992 {
993         double t[3];
994 #ifdef MATRIX4x4_OPENGLORIENTATION
995         t[0] = v[0] - in->m[3][0];
996         t[1] = v[1] - in->m[3][1];
997         t[2] = v[2] - in->m[3][2];
998         out[0] = t[0] * in->m[0][0] + t[1] * in->m[0][1] + t[2] * in->m[0][2];
999         out[1] = t[0] * in->m[1][0] + t[1] * in->m[1][1] + t[2] * in->m[1][2];
1000         out[2] = t[0] * in->m[2][0] + t[1] * in->m[2][1] + t[2] * in->m[2][2];
1001 #else
1002         t[0] = v[0] - in->m[0][3];
1003         t[1] = v[1] - in->m[1][3];
1004         t[2] = v[2] - in->m[2][3];
1005         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
1006         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
1007         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
1008 #endif
1009 }
1010 */
1011
1012 // FIXME: optimize
1013 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, double x, double y, double z)
1014 {
1015         matrix4x4_t base, temp;
1016         base = *out;
1017         Matrix4x4_CreateTranslate(&temp, x, y, z);
1018         Matrix4x4_Concat(out, &base, &temp);
1019 }
1020
1021 // FIXME: optimize
1022 void Matrix4x4_ConcatRotate (matrix4x4_t *out, double angle, double x, double y, double z)
1023 {
1024         matrix4x4_t base, temp;
1025         base = *out;
1026         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
1027         Matrix4x4_Concat(out, &base, &temp);
1028 }
1029
1030 // FIXME: optimize
1031 void Matrix4x4_ConcatScale (matrix4x4_t *out, double x)
1032 {
1033         matrix4x4_t base, temp;
1034         base = *out;
1035         Matrix4x4_CreateScale(&temp, x);
1036         Matrix4x4_Concat(out, &base, &temp);
1037 }
1038
1039 // FIXME: optimize
1040 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, double x, double y, double z)
1041 {
1042         matrix4x4_t base, temp;
1043         base = *out;
1044         Matrix4x4_CreateScale3(&temp, x, y, z);
1045         Matrix4x4_Concat(out, &base, &temp);
1046 }
1047
1048 void Matrix4x4_OriginFromMatrix (const matrix4x4_t *in, float *out)
1049 {
1050 #ifdef MATRIX4x4_OPENGLORIENTATION
1051         out[0] = in->m[3][0];
1052         out[1] = in->m[3][1];
1053         out[2] = in->m[3][2];
1054 #else
1055         out[0] = in->m[0][3];
1056         out[1] = in->m[1][3];
1057         out[2] = in->m[2][3];
1058 #endif
1059 }
1060
1061 double Matrix4x4_ScaleFromMatrix (const matrix4x4_t *in)
1062 {
1063         // we only support uniform scaling, so assume the first row is enough
1064         return sqrt(in->m[0][0] * in->m[0][0] + in->m[0][1] * in->m[0][1] + in->m[0][2] * in->m[0][2]);
1065 }
1066
1067 void Matrix4x4_SetOrigin (matrix4x4_t *out, double x, double y, double z)
1068 {
1069 #ifdef MATRIX4x4_OPENGLORIENTATION
1070         out->m[3][0] = x;
1071         out->m[3][1] = y;
1072         out->m[3][2] = z;
1073 #else
1074         out->m[0][3] = x;
1075         out->m[1][3] = y;
1076         out->m[2][3] = z;
1077 #endif
1078 }
1079
1080 void Matrix4x4_AdjustOrigin (matrix4x4_t *out, double x, double y, double z)
1081 {
1082 #ifdef MATRIX4x4_OPENGLORIENTATION
1083         out->m[3][0] += x;
1084         out->m[3][1] += y;
1085         out->m[3][2] += z;
1086 #else
1087         out->m[0][3] += x;
1088         out->m[1][3] += y;
1089         out->m[2][3] += z;
1090 #endif
1091 }
1092
1093 void Matrix4x4_Scale (matrix4x4_t *out, double rotatescale, double originscale)
1094 {
1095         out->m[0][0] *= rotatescale;
1096         out->m[0][1] *= rotatescale;
1097         out->m[0][2] *= rotatescale;
1098         out->m[1][0] *= rotatescale;
1099         out->m[1][1] *= rotatescale;
1100         out->m[1][2] *= rotatescale;
1101         out->m[2][0] *= rotatescale;
1102         out->m[2][1] *= rotatescale;
1103         out->m[2][2] *= rotatescale;
1104 #ifdef MATRIX4x4_OPENGLORIENTATION
1105         out->m[3][0] *= originscale;
1106         out->m[3][1] *= originscale;
1107         out->m[3][2] *= originscale;
1108 #else
1109         out->m[0][3] *= originscale;
1110         out->m[1][3] *= originscale;
1111         out->m[2][3] *= originscale;
1112 #endif
1113 }