Tomaz optimized Matrix4x4_CreateFromQuakeEntity
[divverent/darkplaces.git] / matrixlib.c
1
2 #include <math.h>
3 #include "matrixlib.h"
4
5 void Matrix4x4_Copy (matrix4x4_t *out, const matrix4x4_t *in)
6 {
7         *out = *in;
8 }
9
10 void Matrix4x4_CopyRotateOnly (matrix4x4_t *out, const matrix4x4_t *in)
11 {
12         out->m[0][0] = in->m[0][0];
13         out->m[0][1] = in->m[0][1];
14         out->m[0][2] = in->m[0][2];
15         out->m[0][3] = 0.0f;
16         out->m[1][0] = in->m[1][0];
17         out->m[1][1] = in->m[1][1];
18         out->m[1][2] = in->m[1][2];
19         out->m[1][3] = 0.0f;
20         out->m[2][0] = in->m[2][0];
21         out->m[2][1] = in->m[2][1];
22         out->m[2][2] = in->m[2][2];
23         out->m[2][3] = 0.0f;
24         out->m[3][0] = 0.0f;
25         out->m[3][1] = 0.0f;
26         out->m[3][2] = 0.0f;
27         out->m[3][3] = 1.0f;
28 }
29
30 void Matrix4x4_CopyTranslateOnly (matrix4x4_t *out, const matrix4x4_t *in)
31 {
32         out->m[0][0] = 0.0f;
33         out->m[0][1] = 0.0f;
34         out->m[0][2] = 0.0f;
35         out->m[0][3] = in->m[0][3];
36         out->m[1][0] = 0.0f;
37         out->m[1][1] = 0.0f;
38         out->m[1][2] = 0.0f;
39         out->m[1][3] = in->m[1][3];
40         out->m[2][0] = 0.0f;
41         out->m[2][1] = 0.0f;
42         out->m[2][2] = 0.0f;
43         out->m[2][3] = in->m[2][3];
44         out->m[3][0] = 0.0f;
45         out->m[3][1] = 0.0f;
46         out->m[3][2] = 0.0f;
47         out->m[3][3] = 1.0f;
48 }
49
50 void Matrix4x4_Concat (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2)
51 {
52         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];
53         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];
54         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];
55         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];
56         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];
57         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];
58         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];
59         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];
60         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];
61         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];
62         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];
63         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];
64         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];
65         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];
66         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];
67         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];
68 }
69
70 void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1)
71 {
72         out->m[0][0] = in1->m[0][0];
73         out->m[0][1] = in1->m[1][0];
74         out->m[0][2] = in1->m[2][0];
75         out->m[0][3] = in1->m[3][0];
76         out->m[1][0] = in1->m[0][1];
77         out->m[1][1] = in1->m[1][1];
78         out->m[1][2] = in1->m[2][1];
79         out->m[1][3] = in1->m[3][1];
80         out->m[2][0] = in1->m[0][2];
81         out->m[2][1] = in1->m[1][2];
82         out->m[2][2] = in1->m[2][2];
83         out->m[2][3] = in1->m[3][2];
84         out->m[3][0] = in1->m[0][3];
85         out->m[3][1] = in1->m[1][3];
86         out->m[3][2] = in1->m[2][3];
87         out->m[3][3] = in1->m[3][3];
88 }
89
90 void Matrix4x4_Transpose3x3 (matrix4x4_t *out, const matrix4x4_t *in1)
91 {
92         out->m[0][0] = in1->m[0][0];
93         out->m[0][1] = in1->m[1][0];
94         out->m[0][2] = in1->m[2][0];
95         out->m[1][0] = in1->m[0][1];
96         out->m[1][1] = in1->m[1][1];
97         out->m[1][2] = in1->m[2][1];
98         out->m[2][0] = in1->m[0][2];
99         out->m[2][1] = in1->m[1][2];
100         out->m[2][2] = in1->m[2][2];
101
102         out->m[0][3] = in1->m[0][3];
103         out->m[1][3] = in1->m[1][3];
104         out->m[2][3] = in1->m[2][3];
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_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1)
112 {
113         // we only support uniform scaling, so assume the first row is enough
114         // (note the lack of sqrt here, because we're trying to undo the scaling,
115         // this means multiplying by the inverse scale twice - squaring it, which
116         // makes the sqrt a waste of time)
117 #if 1
118         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]);
119 #else
120         double scale = 3.0 / sqrt
121                  (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
122                 + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
123                 + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
124         scale *= scale;
125 #endif
126
127         // invert the rotation by transposing and multiplying by the squared
128         // recipricol of the input matrix scale as described above
129         out->m[0][0] = (float)(in1->m[0][0] * scale);
130         out->m[0][1] = (float)(in1->m[1][0] * scale);
131         out->m[0][2] = (float)(in1->m[2][0] * scale);
132         out->m[1][0] = (float)(in1->m[0][1] * scale);
133         out->m[1][1] = (float)(in1->m[1][1] * scale);
134         out->m[1][2] = (float)(in1->m[2][1] * scale);
135         out->m[2][0] = (float)(in1->m[0][2] * scale);
136         out->m[2][1] = (float)(in1->m[1][2] * scale);
137         out->m[2][2] = (float)(in1->m[2][2] * scale);
138
139         // invert the translate
140         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]);
141         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]);
142         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]);
143
144         // don't know if there's anything worth doing here
145         out->m[3][0] = 0;
146         out->m[3][1] = 0;
147         out->m[3][2] = 0;
148         out->m[3][3] = 1;
149 }
150
151 void Matrix4x4_CreateIdentity (matrix4x4_t *out)
152 {
153         out->m[0][0]=1.0f;
154         out->m[0][1]=0.0f;
155         out->m[0][2]=0.0f;
156         out->m[0][3]=0.0f;
157         out->m[1][0]=0.0f;
158         out->m[1][1]=1.0f;
159         out->m[1][2]=0.0f;
160         out->m[1][3]=0.0f;
161         out->m[2][0]=0.0f;
162         out->m[2][1]=0.0f;
163         out->m[2][2]=1.0f;
164         out->m[2][3]=0.0f;
165         out->m[3][0]=0.0f;
166         out->m[3][1]=0.0f;
167         out->m[3][2]=0.0f;
168         out->m[3][3]=1.0f;
169 }
170
171 void Matrix4x4_CreateTranslate (matrix4x4_t *out, float x, float y, float z)
172 {
173         out->m[0][0]=1.0f;
174         out->m[0][1]=0.0f;
175         out->m[0][2]=0.0f;
176         out->m[0][3]=x;
177         out->m[1][0]=0.0f;
178         out->m[1][1]=1.0f;
179         out->m[1][2]=0.0f;
180         out->m[1][3]=y;
181         out->m[2][0]=0.0f;
182         out->m[2][1]=0.0f;
183         out->m[2][2]=1.0f;
184         out->m[2][3]=z;
185         out->m[3][0]=0.0f;
186         out->m[3][1]=0.0f;
187         out->m[3][2]=0.0f;
188         out->m[3][3]=1.0f;
189 }
190
191 void Matrix4x4_CreateRotate (matrix4x4_t *out, float angle, float x, float y, float z)
192 {
193         float len, c, s;
194
195         len = x*x+y*y+z*z;
196         if (len != 0.0f)
197                 len = 1.0f / (float)sqrt(len);
198         x *= len;
199         y *= len;
200         z *= len;
201
202         angle *= (float)(-M_PI / 180.0);
203         c = (float)cos(angle);
204         s = (float)sin(angle);
205
206         out->m[0][0]=x * x + c * (1 - x * x);
207         out->m[0][1]=x * y * (1 - c) + z * s;
208         out->m[0][2]=z * x * (1 - c) - y * s;
209         out->m[0][3]=0.0f;
210         out->m[1][0]=x * y * (1 - c) - z * s;
211         out->m[1][1]=y * y + c * (1 - y * y);
212         out->m[1][2]=y * z * (1 - c) + x * s;
213         out->m[1][3]=0.0f;
214         out->m[2][0]=z * x * (1 - c) + y * s;
215         out->m[2][1]=y * z * (1 - c) - x * s;
216         out->m[2][2]=z * z + c * (1 - z * z);
217         out->m[2][3]=0.0f;
218         out->m[3][0]=0.0f;
219         out->m[3][1]=0.0f;
220         out->m[3][2]=0.0f;
221         out->m[3][3]=1.0f;
222 }
223
224 void Matrix4x4_CreateScale (matrix4x4_t *out, float x)
225 {
226         out->m[0][0]=x;
227         out->m[0][1]=0.0f;
228         out->m[0][2]=0.0f;
229         out->m[0][3]=0.0f;
230         out->m[1][0]=0.0f;
231         out->m[1][1]=x;
232         out->m[1][2]=0.0f;
233         out->m[1][3]=0.0f;
234         out->m[2][0]=0.0f;
235         out->m[2][1]=0.0f;
236         out->m[2][2]=x;
237         out->m[2][3]=0.0f;
238         out->m[3][0]=0.0f;
239         out->m[3][1]=0.0f;
240         out->m[3][2]=0.0f;
241         out->m[3][3]=1.0f;
242 }
243
244 void Matrix4x4_CreateScale3 (matrix4x4_t *out, float x, float y, float z)
245 {
246         out->m[0][0]=x;
247         out->m[0][1]=0.0f;
248         out->m[0][2]=0.0f;
249         out->m[0][3]=0.0f;
250         out->m[1][0]=0.0f;
251         out->m[1][1]=y;
252         out->m[1][2]=0.0f;
253         out->m[1][3]=0.0f;
254         out->m[2][0]=0.0f;
255         out->m[2][1]=0.0f;
256         out->m[2][2]=z;
257         out->m[2][3]=0.0f;
258         out->m[3][0]=0.0f;
259         out->m[3][1]=0.0f;
260         out->m[3][2]=0.0f;
261         out->m[3][3]=1.0f;
262 }
263
264 void Matrix4x4_CreateFromQuakeEntity(matrix4x4_t *out, float x, float y, float z, float pitch, float yaw, float roll, float scale)
265 {
266         double angle, sr, sp, sy, cr, cp, cy;
267
268         if (roll)
269         {
270                 angle = yaw * (M_PI*2 / 360);
271                 sy = sin(angle);
272                 cy = cos(angle);
273                 angle = pitch * (M_PI*2 / 360);
274                 sp = sin(angle);
275                 cp = cos(angle);
276                 angle = roll * (M_PI*2 / 360);
277                 sr = sin(angle);
278                 cr = cos(angle);
279                 out->m[0][0] = (float)((cp*cy) * scale);
280                 out->m[0][1] = (float)((sr*sp*cy+cr*-sy) * scale);
281                 out->m[0][2] = (float)((cr*sp*cy+-sr*-sy) * scale);
282                 out->m[0][3] = x;
283                 out->m[1][0] = (float)((cp*sy) * scale);
284                 out->m[1][1] = (float)((sr*sp*sy+cr*cy) * scale);
285                 out->m[1][2] = (float)((cr*sp*sy+-sr*cy) * scale);
286                 out->m[1][3] = y;
287                 out->m[2][0] = (float)((-sp) * scale);
288                 out->m[2][1] = (float)((sr*cp) * scale);
289                 out->m[2][2] = (float)((cr*cp) * scale);
290                 out->m[2][3] = z;
291                 out->m[3][0] = 0;
292                 out->m[3][1] = 0;
293                 out->m[3][2] = 0;
294                 out->m[3][3] = 1;
295         }
296         else if (pitch)
297         {
298                 angle = yaw * (M_PI*2 / 360);
299                 sy = sin(angle);
300                 cy = cos(angle);
301                 angle = pitch * (M_PI*2 / 360);
302                 sp = sin(angle);
303                 cp = cos(angle);
304                 out->m[0][0] = (float)((cp*cy) * scale);
305                 out->m[0][1] = (float)((-sy) * scale);
306                 out->m[0][2] = (float)((sp*cy) * scale);
307                 out->m[0][3] = x;
308                 out->m[1][0] = (float)((cp*sy) * scale);
309                 out->m[1][1] = (float)((cy) * scale);
310                 out->m[1][2] = (float)((sp*sy) * scale);
311                 out->m[1][3] = y;
312                 out->m[2][0] = (float)((-sp) * scale);
313                 out->m[2][1] = 0;
314                 out->m[2][2] = (float)((cp) * scale);
315                 out->m[2][3] = z;
316                 out->m[3][0] = 0;
317                 out->m[3][1] = 0;
318                 out->m[3][2] = 0;
319                 out->m[3][3] = 1;
320         }
321         else if (yaw)
322         {
323                 angle = yaw * (M_PI*2 / 360);
324                 sy = sin(angle);
325                 cy = cos(angle);
326                 out->m[0][0] = (float)((cy) * scale);
327                 out->m[0][1] = (float)((-sy) * scale);
328                 out->m[0][2] = 0;
329                 out->m[0][3] = x;
330                 out->m[1][0] = (float)((sy) * scale);
331                 out->m[1][1] = (float)((cy) * scale);
332                 out->m[1][2] = 0;
333                 out->m[1][3] = y;
334                 out->m[2][0] = 0;
335                 out->m[2][1] = 0;
336                 out->m[2][2] = scale;
337                 out->m[2][3] = z;
338                 out->m[3][0] = 0;
339                 out->m[3][1] = 0;
340                 out->m[3][2] = 0;
341                 out->m[3][3] = 1;
342         }
343         else
344         {
345                 out->m[0][0] = scale;
346                 out->m[0][1] = 0;
347                 out->m[0][2] = 0;
348                 out->m[0][3] = x;
349                 out->m[1][0] = 0;
350                 out->m[1][1] = scale;
351                 out->m[1][2] = 0;
352                 out->m[1][3] = y;
353                 out->m[2][0] = 0;
354                 out->m[2][1] = 0;
355                 out->m[2][2] = scale;
356                 out->m[2][3] = z;
357                 out->m[3][0] = 0;
358                 out->m[3][1] = 0;
359                 out->m[3][2] = 0;
360                 out->m[3][3] = 1;
361         }
362 }
363
364 void Matrix4x4_ToVectors(const matrix4x4_t *in, float vx[3], float vy[3], float vz[3], float t[3])
365 {
366         vx[0] = in->m[0][0];
367         vx[1] = in->m[1][0];
368         vx[2] = in->m[2][0];
369         vy[0] = in->m[0][1];
370         vy[1] = in->m[1][1];
371         vy[2] = in->m[2][1];
372         vz[0] = in->m[0][2];
373         vz[1] = in->m[1][2];
374         vz[2] = in->m[2][2];
375         t[0] = in->m[0][3];
376         t[1] = in->m[1][3];
377         t[2] = in->m[2][3];
378 }
379
380 void Matrix4x4_FromVectors(matrix4x4_t *out, const float vx[3], const float vy[3], const float vz[3], const float t[3])
381 {
382         out->m[0][0] = vx[0];
383         out->m[0][1] = vy[0];
384         out->m[0][2] = vz[0];
385         out->m[0][3] = t[0];
386         out->m[1][0] = vx[1];
387         out->m[1][1] = vy[1];
388         out->m[1][2] = vz[1];
389         out->m[1][3] = t[1];
390         out->m[2][0] = vx[2];
391         out->m[2][1] = vy[2];
392         out->m[2][2] = vz[2];
393         out->m[2][3] = t[2];
394         out->m[3][0] = 0.0f;
395         out->m[3][1] = 0.0f;
396         out->m[3][2] = 0.0f;
397         out->m[3][3] = 1.0f;
398 }
399
400 void Matrix4x4_Blend (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2, float blend)
401 {
402         float iblend = 1 - blend;
403         out->m[0][0] = in1->m[0][0] * iblend + in2->m[0][0] * blend;
404         out->m[0][1] = in1->m[0][1] * iblend + in2->m[0][1] * blend;
405         out->m[0][2] = in1->m[0][2] * iblend + in2->m[0][2] * blend;
406         out->m[0][3] = in1->m[0][3] * iblend + in2->m[0][3] * blend;
407         out->m[1][0] = in1->m[1][0] * iblend + in2->m[1][0] * blend;
408         out->m[1][1] = in1->m[1][1] * iblend + in2->m[1][1] * blend;
409         out->m[1][2] = in1->m[1][2] * iblend + in2->m[1][2] * blend;
410         out->m[1][3] = in1->m[1][3] * iblend + in2->m[1][3] * blend;
411         out->m[2][0] = in1->m[2][0] * iblend + in2->m[2][0] * blend;
412         out->m[2][1] = in1->m[2][1] * iblend + in2->m[2][1] * blend;
413         out->m[2][2] = in1->m[2][2] * iblend + in2->m[2][2] * blend;
414         out->m[2][3] = in1->m[2][3] * iblend + in2->m[2][3] * blend;
415         out->m[3][0] = in1->m[3][0] * iblend + in2->m[3][0] * blend;
416         out->m[3][1] = in1->m[3][1] * iblend + in2->m[3][1] * blend;
417         out->m[3][2] = in1->m[3][2] * iblend + in2->m[3][2] * blend;
418         out->m[3][3] = in1->m[3][3] * iblend + in2->m[3][3] * blend;
419 }
420
421
422 void Matrix4x4_Transform (const matrix4x4_t *in, const float v[3], float out[3])
423 {
424         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
425         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
426         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
427 }
428
429 void Matrix4x4_Transform4 (const matrix4x4_t *in, const float v[4], float out[4])
430 {
431         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];
432         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];
433         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];
434         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];
435 }
436
437 void Matrix4x4_Transform3x3 (const matrix4x4_t *in, const float v[3], float out[3])
438 {
439         out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2];
440         out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2];
441         out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2];
442 }
443
444 /*
445 void Matrix4x4_SimpleUntransform (const matrix4x4_t *in, const float v[3], float out[3])
446 {
447         float t[3];
448         t[0] = v[0] - in->m[0][3];
449         t[1] = v[1] - in->m[1][3];
450         t[2] = v[2] - in->m[2][3];
451         out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
452         out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
453         out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
454 }
455 */
456
457 // FIXME: optimize
458 void Matrix4x4_ConcatTranslate (matrix4x4_t *out, float x, float y, float z)
459 {
460         matrix4x4_t base, temp;
461         base = *out;
462         Matrix4x4_CreateTranslate(&temp, x, y, z);
463         Matrix4x4_Concat(out, &base, &temp);
464 }
465
466 // FIXME: optimize
467 void Matrix4x4_ConcatRotate (matrix4x4_t *out, float angle, float x, float y, float z)
468 {
469         matrix4x4_t base, temp;
470         base = *out;
471         Matrix4x4_CreateRotate(&temp, angle, x, y, z);
472         Matrix4x4_Concat(out, &base, &temp);
473 }
474
475 // FIXME: optimize
476 void Matrix4x4_ConcatScale (matrix4x4_t *out, float x)
477 {
478         matrix4x4_t base, temp;
479         base = *out;
480         Matrix4x4_CreateScale(&temp, x);
481         Matrix4x4_Concat(out, &base, &temp);
482 }
483
484 // FIXME: optimize
485 void Matrix4x4_ConcatScale3 (matrix4x4_t *out, float x, float y, float z)
486 {
487         matrix4x4_t base, temp;
488         base = *out;
489         Matrix4x4_CreateScale3(&temp, x, y, z);
490         Matrix4x4_Concat(out, &base, &temp);
491 }
492
493 void Matrix4x4_OriginFromMatrix (const matrix4x4_t *in, float *out)
494 {
495         out[0] = in->m[0][3];
496         out[1] = in->m[1][3];
497         out[2] = in->m[2][3];
498 }
499
500 float Matrix4x4_ScaleFromMatrix (const matrix4x4_t *in)
501 {
502         // we only support uniform scaling, so assume the first row is enough
503         return (float)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]);
504 }
505