* moved zeroradiant (1.6) into trunk
[xonotic/netradiant.git] / libs / mathlib / m4x4.c
1 /*
2 Copyright (C) 1999-2007 id Software, Inc. and contributors.
3 For a list of contributors, see the accompanying CONTRIBUTORS file.
4
5 This file is part of GtkRadiant.
6
7 GtkRadiant is free software; you can redistribute it and/or modify
8 it under the terms of the GNU General Public License as published by
9 the Free Software Foundation; either version 2 of the License, or
10 (at your option) any later version.
11
12 GtkRadiant is distributed in the hope that it will be useful,
13 but WITHOUT ANY WARRANTY; without even the implied warranty of
14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15 GNU General Public License for more details.
16
17 You should have received a copy of the GNU General Public License
18 along with GtkRadiant; if not, write to the Free Software
19 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
20 */
21
22 #include "mathlib.h"
23 #include "memory.h"
24
25 void m4x4_identity(m4x4_t matrix)
26 {
27   matrix[1] = matrix[2] = matrix[3] =
28   matrix[4] = matrix[6] = matrix[7] =
29   matrix[8] = matrix[9] = matrix[11] =
30   matrix[12] = matrix[13] = matrix[14] = 0;
31
32   matrix[0] = matrix[5] = matrix[10] = matrix[15] = 1;
33 }
34
35 void m4x4_translation_for_vec3(m4x4_t matrix, const vec3_t translation)
36 {
37   matrix[1] = matrix[2] = matrix[3] =
38   matrix[4] = matrix[6] = matrix[7] =
39   matrix[8] = matrix[9] = matrix[11] = 0;
40
41   matrix[0] = matrix[5] = matrix[10] = matrix[15] = 1;
42
43   matrix[12] = translation[0];
44   matrix[13] = translation[1];
45   matrix[14] = translation[2];
46 }
47
48 void m4x4_rotation_for_vec3(m4x4_t matrix, const vec3_t euler, eulerOrder_t order)
49 {
50   double cx, sx, cy, sy, cz, sz;
51     
52   cx = cos(DEG2RAD(euler[0]));
53   sx = sin(DEG2RAD(euler[0]));
54   cy = cos(DEG2RAD(euler[1]));
55   sy = sin(DEG2RAD(euler[1]));
56   cz = cos(DEG2RAD(euler[2]));
57   sz = sin(DEG2RAD(euler[2]));
58
59   switch(order)
60   {
61   case eXYZ:
62
63 #if 1
64
65     {
66       matrix[0]  = (vec_t)(cy*cz);
67       matrix[1]  = (vec_t)(cy*sz);
68       matrix[2]  = (vec_t)-sy;
69       matrix[4]  = (vec_t)(sx*sy*cz + cx*-sz);
70       matrix[5]  = (vec_t)(sx*sy*sz + cx*cz);
71       matrix[6]  = (vec_t)(sx*cy);
72       matrix[8]  = (vec_t)(cx*sy*cz + sx*sz);
73       matrix[9]  = (vec_t)(cx*sy*sz + -sx*cz);
74       matrix[10] = (vec_t)(cx*cy);
75     }
76
77     matrix[12]  =  matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0;
78     matrix[15] =  1;
79
80 #else
81
82     m4x4_identity(matrix);
83     matrix[5] =(vec_t) cx; matrix[6] =(vec_t) sx;
84     matrix[9] =(vec_t)-sx; matrix[10]=(vec_t) cx;
85
86     {
87       m4x4_t temp;
88       m4x4_identity(temp);
89       temp[0] =(vec_t) cy; temp[2] =(vec_t)-sy;
90       temp[8] =(vec_t) sy; temp[10]=(vec_t) cy;
91       m4x4_premultiply_by_m4x4(matrix, temp);
92       m4x4_identity(temp);
93       temp[0] =(vec_t) cz; temp[1] =(vec_t) sz;
94       temp[4] =(vec_t)-sz; temp[5] =(vec_t) cz;
95       m4x4_premultiply_by_m4x4(matrix, temp);
96     }
97 #endif
98
99     break;
100
101   case eYZX:
102     m4x4_identity(matrix);
103     matrix[0] =(vec_t) cy; matrix[2] =(vec_t)-sy;
104     matrix[8] =(vec_t) sy; matrix[10]=(vec_t) cy;
105
106     {
107       m4x4_t temp;
108       m4x4_identity(temp);
109       temp[5] =(vec_t) cx; temp[6] =(vec_t) sx;
110       temp[9] =(vec_t)-sx; temp[10]=(vec_t) cx;
111       m4x4_premultiply_by_m4x4(matrix, temp);
112       m4x4_identity(temp);
113       temp[0] =(vec_t) cz; temp[1] =(vec_t) sz;
114       temp[4] =(vec_t)-sz; temp[5] =(vec_t) cz;
115       m4x4_premultiply_by_m4x4(matrix, temp);
116     }
117     break;
118
119   case eZXY:
120     m4x4_identity(matrix);
121     matrix[0] =(vec_t) cz; matrix[1] =(vec_t) sz;
122     matrix[4] =(vec_t)-sz; matrix[5] =(vec_t) cz;
123
124     {
125       m4x4_t temp;
126       m4x4_identity(temp);
127       temp[5] =(vec_t) cx; temp[6] =(vec_t) sx;
128       temp[9] =(vec_t)-sx; temp[10]=(vec_t) cx;
129       m4x4_premultiply_by_m4x4(matrix, temp);
130       m4x4_identity(temp);
131       temp[0] =(vec_t) cy; temp[2] =(vec_t)-sy;
132       temp[8] =(vec_t) sy; temp[10]=(vec_t) cy;
133       m4x4_premultiply_by_m4x4(matrix, temp);
134     }
135     break;
136
137   case eXZY:
138     m4x4_identity(matrix);
139     matrix[5] =(vec_t) cx; matrix[6] =(vec_t) sx;
140     matrix[9] =(vec_t)-sx; matrix[10]=(vec_t) cx;
141
142     {
143       m4x4_t temp;
144       m4x4_identity(temp);
145       temp[0] =(vec_t) cz; temp[1] =(vec_t) sz;
146       temp[4] =(vec_t)-sz; temp[5] =(vec_t) cz;
147       m4x4_premultiply_by_m4x4(matrix, temp);
148       m4x4_identity(temp);
149       temp[0] =(vec_t) cy; temp[2] =(vec_t)-sy;
150       temp[8] =(vec_t) sy; temp[10]=(vec_t) cy;
151       m4x4_premultiply_by_m4x4(matrix, temp);
152     }
153     break;
154
155   case eYXZ:
156
157 /* transposed
158 |  cy.cz + sx.sy.-sz + -cx.sy.0   0.cz + cx.-sz + sx.0   sy.cz + -sx.cy.-sz + cx.cy.0 |
159 |  cy.sz + sx.sy.cz + -cx.sy.0    0.sz + cx.cz + sx.0    sy.sz + -sx.cy.cz + cx.cy.0  |
160 |  cy.0 + sx.sy.0 + -cx.sy.1      0.0 + cx.0 + sx.1      sy.0 + -sx.cy.0 + cx.cy.1    |
161 */
162
163 #if 1
164
165   {
166     matrix[0]  = (vec_t)(cy*cz + sx*sy*-sz);
167     matrix[1]  = (vec_t)(cy*sz + sx*sy*cz);
168     matrix[2]  = (vec_t)(-cx*sy);
169     matrix[4]  = (vec_t)(cx*-sz);
170     matrix[5]  = (vec_t)(cx*cz);
171     matrix[6]  = (vec_t)(sx);
172     matrix[8]  = (vec_t)(sy*cz + -sx*cy*-sz);
173     matrix[9]  = (vec_t)(sy*sz + -sx*cy*cz);
174     matrix[10] = (vec_t)(cx*cy);
175   }
176
177   matrix[12]  =  matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0;
178   matrix[15] =  1;
179
180 #else
181
182   m4x4_identity(matrix);
183   matrix[0] =(vec_t) cy; matrix[2] =(vec_t)-sy;
184   matrix[8] =(vec_t) sy; matrix[10]=(vec_t) cy;
185
186   {
187     m4x4_t temp;
188     m4x4_identity(temp);
189     temp[5] =(vec_t) cx; temp[6] =(vec_t) sx;
190     temp[9] =(vec_t)-sx; temp[10]=(vec_t) cx;
191     m4x4_premultiply_by_m4x4(matrix, temp);
192     m4x4_identity(temp);
193     temp[0] =(vec_t) cz; temp[1] =(vec_t) sz;
194     temp[4] =(vec_t)-sz; temp[5] =(vec_t) cz;
195     m4x4_premultiply_by_m4x4(matrix, temp);
196   }
197 #endif
198   break;
199
200   case eZYX:
201 #if 1
202
203   {
204     matrix[0]  = (vec_t)(cy*cz);
205     matrix[4]  = (vec_t)(cy*-sz);
206     matrix[8]  = (vec_t)sy;
207     matrix[1]  = (vec_t)(sx*sy*cz + cx*sz);
208     matrix[5]  = (vec_t)(sx*sy*-sz + cx*cz);
209     matrix[9]  = (vec_t)(-sx*cy);
210     matrix[2]  = (vec_t)(cx*-sy*cz + sx*sz);
211     matrix[6]  = (vec_t)(cx*-sy*-sz + sx*cz);
212     matrix[10] = (vec_t)(cx*cy);
213   }
214
215   matrix[12]  =  matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0;
216   matrix[15] =  1;
217
218 #else
219
220   m4x4_identity(matrix);
221   matrix[0] =(vec_t) cz; matrix[1] =(vec_t) sz;
222   matrix[4] =(vec_t)-sz; matrix[5] =(vec_t) cz;
223   {
224     m4x4_t temp;
225     m4x4_identity(temp);
226     temp[0] =(vec_t) cy; temp[2] =(vec_t)-sy;
227     temp[8] =(vec_t) sy; temp[10]=(vec_t) cy;
228     m4x4_premultiply_by_m4x4(matrix, temp);
229     m4x4_identity(temp);
230     temp[5] =(vec_t) cx; temp[6] =(vec_t) sx;
231     temp[9] =(vec_t)-sx; temp[10]=(vec_t) cx;
232     m4x4_premultiply_by_m4x4(matrix, temp);
233   }
234
235 #endif
236   break;
237
238   }
239
240 }
241
242 void m4x4_scale_for_vec3(m4x4_t matrix, const vec3_t scale)
243 {
244   matrix[1] = matrix[2] = matrix[3] =
245   matrix[4] = matrix[6] = matrix[7] =
246   matrix[8] = matrix[9] = matrix[11] =
247   matrix[12] = matrix[13] = matrix[14] = 0;
248
249   matrix[15] = 1;
250
251   matrix[0] = scale[0];
252   matrix[5] = scale[1];
253   matrix[10] = scale[2];
254 }
255
256 void m4x4_rotation_for_quat(m4x4_t matrix, const vec4_t rotation)
257 {
258   float xx,xy,xz,xw,yy,yz,yw,zz,zw;
259
260   xx      = rotation[0] * rotation[0];
261   xy      = rotation[0] * rotation[1];
262   xz      = rotation[0] * rotation[2];
263   xw      = rotation[0] * rotation[3];
264
265   yy      = rotation[1] * rotation[1];
266   yz      = rotation[1] * rotation[2];
267   yw      = rotation[1] * rotation[3];
268
269   zz      = rotation[2] * rotation[2];
270   zw      = rotation[2] * rotation[3];
271
272   matrix[0]  = 1 - 2 * ( yy + zz );
273   matrix[4]  =     2 * ( xy - zw );
274   matrix[8]  =     2 * ( xz + yw );
275
276   matrix[1]  =     2 * ( xy + zw );
277   matrix[5]  = 1 - 2 * ( xx + zz );
278   matrix[9]  =     2 * ( yz - xw );
279
280   matrix[2]  =     2 * ( xz - yw );
281   matrix[6]  =     2 * ( yz + xw );
282   matrix[10] = 1 - 2 * ( xx + yy );
283
284   matrix[3]  = matrix[7] = matrix[11] = matrix[12] = matrix[13] = matrix[14] = 0;
285   matrix[15] = 1;
286 }
287
288 void m4x4_rotation_for_axisangle(m4x4_t matrix, const vec3_t axis, vec_t angle)
289 {
290   vec4_t rotation;
291   angle *= 0.5;
292
293   rotation[3] = (float)sin((float)(angle));
294
295   rotation[0]    = axis[0] * rotation[3];
296   rotation[1]    = axis[1] * rotation[3];
297   rotation[2]    = axis[2] * rotation[3];
298   rotation[3]    = (float)cos((float)(angle));
299
300   m4x4_rotation_for_quat(matrix, rotation);
301 }
302
303 void m4x4_translate_by_vec3(m4x4_t matrix, const vec3_t translation)
304 {
305   m4x4_t temp;
306   m4x4_translation_for_vec3(temp, translation);
307   m4x4_multiply_by_m4x4(matrix, temp);
308 }
309
310 void m4x4_rotate_by_vec3(m4x4_t matrix, const vec3_t euler, eulerOrder_t order)
311 {
312   m4x4_t temp;
313   m4x4_rotation_for_vec3(temp, euler, order);
314   m4x4_multiply_by_m4x4(matrix, temp);
315 }
316
317 void m4x4_scale_by_vec3(m4x4_t matrix, const vec3_t scale)
318 {
319   m4x4_t temp;
320   m4x4_scale_for_vec3(temp, scale);
321   m4x4_multiply_by_m4x4(matrix, temp);
322 }
323
324 void m4x4_rotate_by_quat(m4x4_t matrix, const vec4_t rotation)
325 {
326   m4x4_t temp;
327   m4x4_rotation_for_quat(temp, rotation);
328   m4x4_multiply_by_m4x4(matrix, temp);
329 }
330
331 void m4x4_rotate_by_axisangle(m4x4_t matrix, const vec3_t axis, vec_t angle)
332 {
333   m4x4_t temp;
334   m4x4_rotation_for_axisangle(temp, axis, angle);
335   m4x4_multiply_by_m4x4(matrix, temp);
336 }
337
338 void m4x4_transform_by_vec3(m4x4_t matrix, const vec3_t translation, const vec3_t euler, eulerOrder_t order, const vec3_t scale)
339 {
340   m4x4_translate_by_vec3(matrix, translation);
341   m4x4_rotate_by_vec3(matrix, euler, order);
342   m4x4_scale_by_vec3(matrix, scale);
343 }
344
345 void m4x4_pivoted_rotate_by_vec3(m4x4_t matrix, const vec3_t euler, eulerOrder_t order, const vec3_t pivotpoint)
346 {
347   vec3_t vec3_temp;
348   VectorNegative(pivotpoint, vec3_temp);
349
350   m4x4_translate_by_vec3(matrix, pivotpoint);
351   m4x4_rotate_by_vec3(matrix, euler, order);
352   m4x4_translate_by_vec3(matrix, vec3_temp);
353 }
354
355 void m4x4_pivoted_scale_by_vec3(m4x4_t matrix, const vec3_t scale, const vec3_t pivotpoint)
356 {
357   vec3_t vec3_temp;
358   VectorNegative(pivotpoint, vec3_temp);
359
360   m4x4_translate_by_vec3(matrix, pivotpoint);
361   m4x4_scale_by_vec3(matrix, scale);
362   m4x4_translate_by_vec3(matrix, vec3_temp);
363 }
364
365 void m4x4_pivoted_transform_by_vec3(m4x4_t matrix, const vec3_t translation, const vec3_t euler, eulerOrder_t order, const vec3_t scale, const vec3_t pivotpoint)
366 {
367   vec3_t vec3_temp;
368
369   VectorAdd(pivotpoint, translation, vec3_temp);
370   m4x4_translate_by_vec3(matrix, vec3_temp);
371   m4x4_rotate_by_vec3(matrix, euler, order);
372   m4x4_scale_by_vec3(matrix, scale);
373   VectorNegative(pivotpoint, vec3_temp);
374   m4x4_translate_by_vec3(matrix, vec3_temp);
375 }
376
377 void m4x4_pivoted_rotate_by_quat(m4x4_t matrix, const vec4_t rotation, const vec3_t pivotpoint)
378 {
379   vec3_t vec3_temp;
380   VectorNegative(pivotpoint, vec3_temp);
381
382   m4x4_translate_by_vec3(matrix, pivotpoint);
383   m4x4_rotate_by_quat(matrix, rotation);
384   m4x4_translate_by_vec3(matrix, vec3_temp);
385 }
386
387 void m4x4_pivoted_rotate_by_axisangle(m4x4_t matrix, const vec3_t axis, vec_t angle, const vec3_t pivotpoint)
388 {
389   vec3_t vec3_temp;
390   VectorNegative(pivotpoint, vec3_temp);
391
392   m4x4_translate_by_vec3(matrix, pivotpoint);
393   m4x4_rotate_by_axisangle(matrix, axis, angle);
394   m4x4_translate_by_vec3(matrix, vec3_temp);
395 }
396
397
398 /*
399 A = A.B
400
401 A0 = B0 * A0 + B1 * A4 + B2 * A8 + B3 * A12
402 A4 = B4 * A0 + B5 * A4 + B6 * A8 + B7 * A12
403 A8 = B8 * A0 + B9 * A4 + B10* A8 + B11* A12
404 A12= B12* A0 + B13* A4 + B14* A8 + B15* A12
405
406 A1 = B0 * A1 + B1 * A5 + B2 * A9 + B3 * A13
407 A5 = B4 * A1 + B5 * A5 + B6 * A9 + B7 * A13
408 A9 = B8 * A1 + B9 * A5 + B10* A9 + B11* A13
409 A13= B12* A1 + B13* A5 + B14* A9 + B15* A13
410
411 A2 = B0 * A2 + B1 * A6 + B2 * A10+ B3 * A14
412 A6 = B4 * A2 + B5 * A6 + B6 * A10+ B7 * A14
413 A10= B8 * A2 + B9 * A6 + B10* A10+ B11* A14
414 A14= B12* A2 + B13* A6 + B14* A10+ B15* A14
415
416 A3 = B0 * A3 + B1 * A7 + B2 * A11+ B3 * A15
417 A7 = B4 * A3 + B5 * A7 + B6 * A11+ B7 * A15
418 A11= B8 * A3 + B9 * A7 + B10* A11+ B11* A15
419 A15= B12* A3 + B13* A7 + B14* A11+ B15* A15
420 */
421
422 void m4x4_multiply_by_m4x4(m4x4_t dst, const m4x4_t src)
423 {
424         vec_t dst0, dst1, dst2, dst3;
425
426 #if 1
427
428   dst0 = src[0] * dst[0] + src[1] * dst[4] + src[2] * dst[8] + src[3] * dst[12];
429   dst1 = src[4] * dst[0] + src[5] * dst[4] + src[6] * dst[8] + src[7] * dst[12];
430   dst2 = src[8] * dst[0] + src[9] * dst[4] + src[10]* dst[8] + src[11]* dst[12];
431   dst3 = src[12]* dst[0] + src[13]* dst[4] + src[14]* dst[8] + src[15]* dst[12];
432   dst[0] = dst0; dst[4] = dst1; dst[8] = dst2; dst[12]= dst3;
433
434   dst0 = src[0] * dst[1] + src[1] * dst[5] + src[2] * dst[9] + src[3] * dst[13];
435   dst1 = src[4] * dst[1] + src[5] * dst[5] + src[6] * dst[9] + src[7] * dst[13];
436   dst2 = src[8] * dst[1] + src[9] * dst[5] + src[10]* dst[9] + src[11]* dst[13];
437   dst3 = src[12]* dst[1] + src[13]* dst[5] + src[14]* dst[9] + src[15]* dst[13];
438   dst[1] = dst0; dst[5] = dst1; dst[9] = dst2; dst[13]= dst3;
439
440   dst0 = src[0] * dst[2] + src[1] * dst[6] + src[2] * dst[10]+ src[3] * dst[14];
441   dst1 = src[4] * dst[2] + src[5] * dst[6] + src[6] * dst[10]+ src[7] * dst[14];
442   dst2 = src[8] * dst[2] + src[9] * dst[6] + src[10]* dst[10]+ src[11]* dst[14];
443   dst3 = src[12]* dst[2] + src[13]* dst[6] + src[14]* dst[10]+ src[15]* dst[14];
444   dst[2] = dst0; dst[6] = dst1; dst[10]= dst2; dst[14]= dst3;
445
446   dst0 = src[0] * dst[3] + src[1] * dst[7] + src[2] * dst[11]+ src[3] * dst[15];
447   dst1 = src[4] * dst[3] + src[5] * dst[7] + src[6] * dst[11]+ src[7] * dst[15];
448   dst2 = src[8] * dst[3] + src[9] * dst[7] + src[10]* dst[11]+ src[11]* dst[15];
449   dst3 = src[12]* dst[3] + src[13]* dst[7] + src[14]* dst[11]+ src[15]* dst[15];
450   dst[3] = dst0; dst[7] = dst1; dst[11]= dst2; dst[15]= dst3;
451
452 #else
453
454   vec_t * p = dst;
455         for(int i=0;i<4;i++)
456         {
457                 dst1 =  src[0]  * p[0];
458                 dst1 += src[1]  * p[4];
459                 dst1 += src[2]  * p[8];
460                 dst1 += src[3]  * p[12];
461                 dst2 =  src[4]  * p[0];
462                 dst2 += src[5]  * p[4];
463                 dst2 += src[6]  * p[8];
464                 dst2 += src[7]  * p[12];
465                 dst3 =  src[8]  * p[0];
466                 dst3 += src[9]  * p[4];
467                 dst3 += src[10] * p[8];
468                 dst3 += src[11] * p[12];
469                 dst4 =  src[12] * p[0];
470                 dst4 += src[13] * p[4];
471                 dst4 += src[14] * p[8];
472                 dst4 += src[15] * p[12];
473
474                 p[0] = dst1;
475                 p[4] = dst2;
476                 p[8] = dst3;
477                 p[12] = dst4;
478     p++;
479         }
480
481 #endif
482 }
483
484 /*
485 A = B.A
486
487 A0 = A0 * B0 + A1 * B4 + A2 * B8 + A3 * B12
488 A1 = A0 * B1 + A1 * B5 + A2 * B9 + A3 * B13
489 A2 = A0 * B2 + A1 * B6 + A2 * B10+ A3 * B14
490 A3 = A0 * B3 + A1 * B7 + A2 * B11+ A3 * B15
491
492 A4 = A4 * B0 + A5 * B4 + A6 * B8 + A7 * B12
493 A5 = A4 * B1 + A5 * B5 + A6 * B9 + A7 * B13
494 A6 = A4 * B2 + A5 * B6 + A6 * B10+ A7 * B14
495 A7 = A4 * B3 + A5 * B7 + A6 * B11+ A7 * B15
496
497 A8 = A8 * B0 + A9 * B4 + A10* B8 + A11* B12
498 A9 = A8 * B1 + A9 * B5 + A10* B9 + A11* B13
499 A10= A8 * B2 + A9 * B6 + A10* B10+ A11* B14
500 A11= A8 * B3 + A9 * B7 + A10* B11+ A11* B15
501
502 A12= A12* B0 + A13* B4 + A14* B8 + A15* B12
503 A13= A12* B1 + A13* B5 + A14* B9 + A15* B13
504 A14= A12* B2 + A13* B6 + A14* B10+ A15* B14
505 A15= A12* B3 + A13* B7 + A14* B11+ A15* B15
506 */
507
508 void m4x4_premultiply_by_m4x4(m4x4_t dst, const m4x4_t src)
509 {
510         vec_t dst0, dst1, dst2, dst3;
511
512 #if 1
513
514   dst0 = dst[0] * src[0] + dst[1] * src[4] + dst[2] * src[8] + dst[3] * src[12];
515   dst1 = dst[0] * src[1] + dst[1] * src[5] + dst[2] * src[9] + dst[3] * src[13];
516   dst2 = dst[0] * src[2] + dst[1] * src[6] + dst[2] * src[10]+ dst[3] * src[14];
517   dst3 = dst[0] * src[3] + dst[1] * src[7] + dst[2] * src[11]+ dst[3] * src[15];
518   dst[0] = dst0; dst[1] = dst1; dst[2] = dst2; dst[3]= dst3;
519
520   dst0 = dst[4] * src[0] + dst[5] * src[4] + dst[6] * src[8] + dst[7] * src[12];
521   dst1 = dst[4] * src[1] + dst[5] * src[5] + dst[6] * src[9] + dst[7] * src[13];
522   dst2 = dst[4] * src[2] + dst[5] * src[6] + dst[6] * src[10]+ dst[7] * src[14];
523   dst3 = dst[4] * src[3] + dst[5] * src[7] + dst[6] * src[11]+ dst[7] * src[15];
524   dst[4] = dst0; dst[5] = dst1; dst[6] = dst2; dst[7]= dst3;
525
526   dst0 = dst[8] * src[0] + dst[9] * src[4] + dst[10]* src[8] + dst[11]* src[12];
527   dst1 = dst[8] * src[1] + dst[9] * src[5] + dst[10]* src[9] + dst[11]* src[13];
528   dst2 = dst[8] * src[2] + dst[9] * src[6] + dst[10]* src[10]+ dst[11]* src[14];
529   dst3 = dst[8] * src[3] + dst[9] * src[7] + dst[10]* src[11]+ dst[11]* src[15];
530   dst[8] = dst0; dst[9] = dst1; dst[10] = dst2; dst[11]= dst3;
531
532   dst0 = dst[12]* src[0] + dst[13]* src[4] + dst[14]* src[8] + dst[15]* src[12];
533   dst1 = dst[12]* src[1] + dst[13]* src[5] + dst[14]* src[9] + dst[15]* src[13];
534   dst2 = dst[12]* src[2] + dst[13]* src[6] + dst[14]* src[10]+ dst[15]* src[14];
535   dst3 = dst[12]* src[3] + dst[13]* src[7] + dst[14]* src[11]+ dst[15]* src[15];
536   dst[12] = dst0; dst[13] = dst1; dst[14] = dst2; dst[15]= dst3;
537
538 #else
539
540   vec_t* p = dst;
541         for(int i=0;i<4;i++)
542         {
543                 dst1 =  src[0]  * p[0];
544                 dst2 =  src[1]  * p[0];
545                 dst3 =  src[2]  * p[0];
546                 dst4 =  src[3]  * p[0];
547                 dst1 += src[4]  * p[1];
548                 dst2 += src[5]  * p[1];
549                 dst3 += src[6]  * p[1];
550                 dst4 += src[7]  * p[1];
551                 dst1 += src[8]  * p[2];
552                 dst2 += src[9]  * p[2];
553                 dst4 += src[11] * p[2];
554                 dst3 += src[10] * p[2];
555                 dst1 += src[12] * p[3];
556                 dst2 += src[13] * p[3];
557                 dst3 += src[14] * p[3];
558                 dst4 += src[15] * p[3];
559
560                 *p++ = dst1;
561                 *p++ = dst2;
562                 *p++ = dst3;
563                 *p++ = dst4;
564         }
565
566 #endif
567 }
568
569 void m4x4_transform_point(const m4x4_t matrix, vec3_t point)
570 {
571   float out1, out2, out3;
572
573         out1 =  matrix[0]  * point[0];
574         out2 =  matrix[1]  * point[0];
575         out3 =  matrix[2]  * point[0];
576         out1 += matrix[4]  * point[1];
577         out2 += matrix[5]  * point[1];
578         out3 += matrix[6]  * point[1];
579         out1 += matrix[8]  * point[2];
580         out2 += matrix[9]  * point[2];
581         out3 += matrix[10] * point[2];
582   out1 += matrix[12];
583   out2 += matrix[13];
584   out3 += matrix[14];
585
586         point[0] = out1;
587         point[1] = out2;
588         point[2] = out3;
589 }
590
591 void m4x4_transform_normal(const m4x4_t matrix, vec3_t normal)
592 {
593   float out1, out2, out3;
594
595         out1 =  matrix[0]  * normal[0];
596         out2 =  matrix[1]  * normal[0];
597         out3 =  matrix[2]  * normal[0];
598         out1 += matrix[4]  * normal[1];
599         out2 += matrix[5]  * normal[1];
600         out3 += matrix[6]  * normal[1];
601         out1 += matrix[8]  * normal[2];
602         out2 += matrix[9]  * normal[2];
603         out3 += matrix[10] * normal[2];
604
605         normal[0] = out1;
606         normal[1] = out2;
607         normal[2] = out3;
608 }
609
610 void m4x4_transform_vec4(const m4x4_t matrix, vec4_t vector)
611 {
612   float out1, out2, out3, out4;
613
614         out1 =  matrix[0]  * vector[0];
615         out2 =  matrix[1]  * vector[0];
616         out3 =  matrix[2]  * vector[0];
617         out4 =  matrix[3]  * vector[0];
618         out1 += matrix[4]  * vector[1];
619         out2 += matrix[5]  * vector[1];
620         out3 += matrix[6]  * vector[1];
621         out4 += matrix[7]  * vector[1];
622         out1 += matrix[8]  * vector[2];
623         out2 += matrix[9]  * vector[2];
624         out3 += matrix[10] * vector[2];
625         out4 += matrix[11] * vector[2];
626   out1 += matrix[12] * vector[3];
627   out2 += matrix[13] * vector[3];
628   out3 += matrix[14] * vector[3];
629   out4 += matrix[15] * vector[3];
630
631         vector[0] = out1;
632         vector[1] = out2;
633         vector[2] = out3;
634   vector[3] = out4;
635 }
636
637 void m4x4_transpose(m4x4_t matrix)
638 {
639         int i, j;
640         float temp, *p1, *p2;
641
642   for (i=1; i<4; i++) {
643     for (j=0; j<i; j++) {
644       p1 = matrix+(j*4+i);
645       p2 = matrix+(i*4+j);
646       temp = *p1;
647       *p1=*p2;
648       *p2=temp;
649     }
650   }
651 }
652
653 void m4x4_orthogonal_invert(m4x4_t matrix)
654 {
655   float temp;
656
657   temp = -matrix[3];
658   matrix[3] = matrix[12];
659   matrix[12] = temp;
660
661   temp = -matrix[7];
662   matrix[7] = matrix[13];
663   matrix[13] = temp;
664
665   temp = -matrix[11];
666   matrix[11] = matrix[14];
667   matrix[14] = temp;
668
669   /*
670   temp = matrix[1];
671   matrix[1] = matrix[4];
672   matrix[4] = temp;
673
674   temp = matrix[2];
675   matrix[2] = matrix[8];
676   matrix[8] = temp;
677
678   temp = matrix[6];
679   matrix[6] = matrix[9];
680   matrix[9] = temp;
681
682   matrix[3] = -matrix[3];
683   matrix[7] = -matrix[7];
684   matrix[11] = -matrix[11];
685   */
686 }
687
688 float m3_det( m3x3_t mat )
689 {
690   float det;
691   
692   det = mat[0] * ( mat[4]*mat[8] - mat[7]*mat[5] )
693     - mat[1] * ( mat[3]*mat[8] - mat[6]*mat[5] )
694     + mat[2] * ( mat[3]*mat[7] - mat[6]*mat[4] );
695   
696   return( det );
697 }
698
699 /*
700 void m3_inverse( m3x3_t mr, m3x3_t ma )
701 {
702   float det = m3_det( ma );
703   
704   if ( fabs( det ) < 0.0005 )
705   {
706     m3_identity( ma );
707     return;
708   }
709   
710   mr[0] =    ma[4]*ma[8] - ma[5]*ma[7]   / det;
711   mr[1] = -( ma[1]*ma[8] - ma[7]*ma[2] ) / det;
712   mr[2] =    ma[1]*ma[5] - ma[4]*ma[2]   / det;
713   
714   mr[3] = -( ma[3]*ma[8] - ma[5]*ma[6] ) / det;
715   mr[4] =    ma[0]*ma[8] - ma[6]*ma[2]   / det;
716   mr[5] = -( ma[0]*ma[5] - ma[3]*ma[2] ) / det;
717   
718   mr[6] =    ma[3]*ma[7] - ma[6]*ma[4]   / det;
719   mr[7] = -( ma[0]*ma[7] - ma[6]*ma[1] ) / det;
720   mr[8] =    ma[0]*ma[4] - ma[1]*ma[3]   / det;
721 }
722 */
723
724 void m4_submat( m4x4_t mr, m3x3_t mb, int i, int j )
725 {
726   int ti, tj, idst, jdst;
727   
728   for ( ti = 0; ti < 4; ti++ )
729   {
730     if ( ti < i )
731       idst = ti;
732     else
733       if ( ti > i )
734         idst = ti-1;
735       
736       for ( tj = 0; tj < 4; tj++ )
737       {
738         if ( tj < j )
739           jdst = tj;
740         else
741           if ( tj > j )
742             jdst = tj-1;
743           
744           if ( ti != i && tj != j )
745             mb[idst*3 + jdst] = mr[ti*4 + tj ];
746       }
747   }
748 }
749
750 float m4_det( m4x4_t mr )
751 {
752   float  det, result = 0, i = 1;
753   m3x3_t msub3;
754   int     n;
755   
756   for ( n = 0; n < 4; n++, i *= -1 )
757   {
758     m4_submat( mr, msub3, 0, n );
759     
760     det     = m3_det( msub3 );
761     result += mr[n] * det * i;
762   }
763   
764   return result;
765 }
766
767 int m4x4_invert(m4x4_t matrix)
768 {
769   float  mdet = m4_det( matrix );
770   m3x3_t mtemp;
771   int     i, j, sign;
772   m4x4_t m4x4_temp;
773   
774   if ( fabs( mdet ) < 0.0000000001 )    //% 0.0005
775     return 1;
776
777   memcpy(m4x4_temp, matrix, sizeof(m4x4_t));
778   
779   for ( i = 0; i < 4; i++ )
780     for ( j = 0; j < 4; j++ )
781     {
782       sign = 1 - ( (i +j) % 2 ) * 2;
783       
784       m4_submat( m4x4_temp, mtemp, i, j );
785       
786       matrix[i+j*4] = ( m3_det( mtemp ) * sign ) / mdet;
787     }
788     
789   return 0;
790 }