2 Copyright (C) 1999-2007 id Software, Inc. and contributors.
3 For a list of contributors, see the accompanying CONTRIBUTORS file.
5 This file is part of GtkRadiant.
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.
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.
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
25 void m4x4_identity( m4x4_t matrix ){
26 matrix[1] = matrix[2] = matrix[3] =
27 matrix[4] = matrix[6] = matrix[7] =
28 matrix[8] = matrix[9] = matrix[11] =
29 matrix[12] = matrix[13] = matrix[14] = 0;
31 matrix[0] = matrix[5] = matrix[10] = matrix[15] = 1;
34 void m4x4_translation_for_vec3( m4x4_t matrix, const vec3_t translation ){
35 matrix[1] = matrix[2] = matrix[3] =
36 matrix[4] = matrix[6] = matrix[7] =
37 matrix[8] = matrix[9] = matrix[11] = 0;
39 matrix[0] = matrix[5] = matrix[10] = matrix[15] = 1;
41 matrix[12] = translation[0];
42 matrix[13] = translation[1];
43 matrix[14] = translation[2];
46 void m4x4_rotation_for_vec3( m4x4_t matrix, const vec3_t euler, eulerOrder_t order ){
47 double cx, sx, cy, sy, cz, sz;
49 cx = cos( DEG2RAD( euler[0] ) );
50 sx = sin( DEG2RAD( euler[0] ) );
51 cy = cos( DEG2RAD( euler[1] ) );
52 sy = sin( DEG2RAD( euler[1] ) );
53 cz = cos( DEG2RAD( euler[2] ) );
54 sz = sin( DEG2RAD( euler[2] ) );
63 matrix[0] = (vec_t)( cy * cz );
64 matrix[1] = (vec_t)( cy * sz );
65 matrix[2] = (vec_t)-sy;
66 matrix[4] = (vec_t)( sx * sy * cz + cx * -sz );
67 matrix[5] = (vec_t)( sx * sy * sz + cx * cz );
68 matrix[6] = (vec_t)( sx * cy );
69 matrix[8] = (vec_t)( cx * sy * cz + sx * sz );
70 matrix[9] = (vec_t)( cx * sy * sz + -sx * cz );
71 matrix[10] = (vec_t)( cx * cy );
74 matrix[12] = matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0;
79 m4x4_identity( matrix );
80 matrix[5] = (vec_t) cx; matrix[6] = (vec_t) sx;
81 matrix[9] = (vec_t)-sx; matrix[10] = (vec_t) cx;
85 m4x4_identity( temp );
86 temp[0] = (vec_t) cy; temp[2] = (vec_t)-sy;
87 temp[8] = (vec_t) sy; temp[10] = (vec_t) cy;
88 m4x4_premultiply_by_m4x4( matrix, temp );
89 m4x4_identity( temp );
90 temp[0] = (vec_t) cz; temp[1] = (vec_t) sz;
91 temp[4] = (vec_t)-sz; temp[5] = (vec_t) cz;
92 m4x4_premultiply_by_m4x4( matrix, temp );
99 m4x4_identity( matrix );
100 matrix[0] = (vec_t) cy; matrix[2] = (vec_t)-sy;
101 matrix[8] = (vec_t) sy; matrix[10] = (vec_t) cy;
105 m4x4_identity( temp );
106 temp[5] = (vec_t) cx; temp[6] = (vec_t) sx;
107 temp[9] = (vec_t)-sx; temp[10] = (vec_t) cx;
108 m4x4_premultiply_by_m4x4( matrix, temp );
109 m4x4_identity( temp );
110 temp[0] = (vec_t) cz; temp[1] = (vec_t) sz;
111 temp[4] = (vec_t)-sz; temp[5] = (vec_t) cz;
112 m4x4_premultiply_by_m4x4( matrix, temp );
117 m4x4_identity( matrix );
118 matrix[0] = (vec_t) cz; matrix[1] = (vec_t) sz;
119 matrix[4] = (vec_t)-sz; matrix[5] = (vec_t) cz;
123 m4x4_identity( temp );
124 temp[5] = (vec_t) cx; temp[6] = (vec_t) sx;
125 temp[9] = (vec_t)-sx; temp[10] = (vec_t) cx;
126 m4x4_premultiply_by_m4x4( matrix, temp );
127 m4x4_identity( temp );
128 temp[0] = (vec_t) cy; temp[2] = (vec_t)-sy;
129 temp[8] = (vec_t) sy; temp[10] = (vec_t) cy;
130 m4x4_premultiply_by_m4x4( matrix, temp );
135 m4x4_identity( matrix );
136 matrix[5] = (vec_t) cx; matrix[6] = (vec_t) sx;
137 matrix[9] = (vec_t)-sx; matrix[10] = (vec_t) cx;
141 m4x4_identity( temp );
142 temp[0] = (vec_t) cz; temp[1] = (vec_t) sz;
143 temp[4] = (vec_t)-sz; temp[5] = (vec_t) cz;
144 m4x4_premultiply_by_m4x4( matrix, temp );
145 m4x4_identity( temp );
146 temp[0] = (vec_t) cy; temp[2] = (vec_t)-sy;
147 temp[8] = (vec_t) sy; temp[10] = (vec_t) cy;
148 m4x4_premultiply_by_m4x4( matrix, temp );
155 | cy.cz + sx.sy.-sz + -cx.sy.0 0.cz + cx.-sz + sx.0 sy.cz + -sx.cy.-sz + cx.cy.0 |
156 | cy.sz + sx.sy.cz + -cx.sy.0 0.sz + cx.cz + sx.0 sy.sz + -sx.cy.cz + cx.cy.0 |
157 | cy.0 + sx.sy.0 + -cx.sy.1 0.0 + cx.0 + sx.1 sy.0 + -sx.cy.0 + cx.cy.1 |
163 matrix[0] = (vec_t)( cy * cz + sx * sy * -sz );
164 matrix[1] = (vec_t)( cy * sz + sx * sy * cz );
165 matrix[2] = (vec_t)( -cx * sy );
166 matrix[4] = (vec_t)( cx * -sz );
167 matrix[5] = (vec_t)( cx * cz );
168 matrix[6] = (vec_t)( sx );
169 matrix[8] = (vec_t)( sy * cz + -sx * cy * -sz );
170 matrix[9] = (vec_t)( sy * sz + -sx * cy * cz );
171 matrix[10] = (vec_t)( cx * cy );
174 matrix[12] = matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0;
179 m4x4_identity( matrix );
180 matrix[0] = (vec_t) cy; matrix[2] = (vec_t)-sy;
181 matrix[8] = (vec_t) sy; matrix[10] = (vec_t) cy;
185 m4x4_identity( temp );
186 temp[5] = (vec_t) cx; temp[6] = (vec_t) sx;
187 temp[9] = (vec_t)-sx; temp[10] = (vec_t) cx;
188 m4x4_premultiply_by_m4x4( matrix, temp );
189 m4x4_identity( temp );
190 temp[0] = (vec_t) cz; temp[1] = (vec_t) sz;
191 temp[4] = (vec_t)-sz; temp[5] = (vec_t) cz;
192 m4x4_premultiply_by_m4x4( matrix, temp );
201 matrix[0] = (vec_t)( cy * cz );
202 matrix[4] = (vec_t)( cy * -sz );
203 matrix[8] = (vec_t)sy;
204 matrix[1] = (vec_t)( sx * sy * cz + cx * sz );
205 matrix[5] = (vec_t)( sx * sy * -sz + cx * cz );
206 matrix[9] = (vec_t)( -sx * cy );
207 matrix[2] = (vec_t)( cx * -sy * cz + sx * sz );
208 matrix[6] = (vec_t)( cx * -sy * -sz + sx * cz );
209 matrix[10] = (vec_t)( cx * cy );
212 matrix[12] = matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0;
217 m4x4_identity( matrix );
218 matrix[0] = (vec_t) cz; matrix[1] = (vec_t) sz;
219 matrix[4] = (vec_t)-sz; matrix[5] = (vec_t) cz;
222 m4x4_identity( temp );
223 temp[0] = (vec_t) cy; temp[2] = (vec_t)-sy;
224 temp[8] = (vec_t) sy; temp[10] = (vec_t) cy;
225 m4x4_premultiply_by_m4x4( matrix, temp );
226 m4x4_identity( temp );
227 temp[5] = (vec_t) cx; temp[6] = (vec_t) sx;
228 temp[9] = (vec_t)-sx; temp[10] = (vec_t) cx;
229 m4x4_premultiply_by_m4x4( matrix, temp );
239 void m4x4_scale_for_vec3( m4x4_t matrix, const vec3_t scale ){
240 matrix[1] = matrix[2] = matrix[3] =
241 matrix[4] = matrix[6] = matrix[7] =
242 matrix[8] = matrix[9] = matrix[11] =
243 matrix[12] = matrix[13] = matrix[14] = 0;
247 matrix[0] = scale[0];
248 matrix[5] = scale[1];
249 matrix[10] = scale[2];
252 void m4x4_rotation_for_quat( m4x4_t matrix, const vec4_t rotation ){
253 float xx,xy,xz,xw,yy,yz,yw,zz,zw;
255 xx = rotation[0] * rotation[0];
256 xy = rotation[0] * rotation[1];
257 xz = rotation[0] * rotation[2];
258 xw = rotation[0] * rotation[3];
260 yy = rotation[1] * rotation[1];
261 yz = rotation[1] * rotation[2];
262 yw = rotation[1] * rotation[3];
264 zz = rotation[2] * rotation[2];
265 zw = rotation[2] * rotation[3];
267 matrix[0] = 1 - 2 * ( yy + zz );
268 matrix[4] = 2 * ( xy - zw );
269 matrix[8] = 2 * ( xz + yw );
271 matrix[1] = 2 * ( xy + zw );
272 matrix[5] = 1 - 2 * ( xx + zz );
273 matrix[9] = 2 * ( yz - xw );
275 matrix[2] = 2 * ( xz - yw );
276 matrix[6] = 2 * ( yz + xw );
277 matrix[10] = 1 - 2 * ( xx + yy );
279 matrix[3] = matrix[7] = matrix[11] = matrix[12] = matrix[13] = matrix[14] = 0;
283 void m4x4_rotation_for_axisangle( m4x4_t matrix, const vec3_t axis, vec_t angle ){
287 rotation[3] = (float)sin( (float)( angle ) );
289 rotation[0] = axis[0] * rotation[3];
290 rotation[1] = axis[1] * rotation[3];
291 rotation[2] = axis[2] * rotation[3];
292 rotation[3] = (float)cos( (float)( angle ) );
294 m4x4_rotation_for_quat( matrix, rotation );
297 void m4x4_translate_by_vec3( m4x4_t matrix, const vec3_t translation ){
299 m4x4_translation_for_vec3( temp, translation );
300 m4x4_multiply_by_m4x4( matrix, temp );
303 void m4x4_rotate_by_vec3( m4x4_t matrix, const vec3_t euler, eulerOrder_t order ){
305 m4x4_rotation_for_vec3( temp, euler, order );
306 m4x4_multiply_by_m4x4( matrix, temp );
309 void m4x4_scale_by_vec3( m4x4_t matrix, const vec3_t scale ){
311 m4x4_scale_for_vec3( temp, scale );
312 m4x4_multiply_by_m4x4( matrix, temp );
315 void m4x4_rotate_by_quat( m4x4_t matrix, const vec4_t rotation ){
317 m4x4_rotation_for_quat( temp, rotation );
318 m4x4_multiply_by_m4x4( matrix, temp );
321 void m4x4_rotate_by_axisangle( m4x4_t matrix, const vec3_t axis, vec_t angle ){
323 m4x4_rotation_for_axisangle( temp, axis, angle );
324 m4x4_multiply_by_m4x4( matrix, temp );
327 void m4x4_transform_by_vec3( m4x4_t matrix, const vec3_t translation, const vec3_t euler, eulerOrder_t order, const vec3_t scale ){
328 m4x4_translate_by_vec3( matrix, translation );
329 m4x4_rotate_by_vec3( matrix, euler, order );
330 m4x4_scale_by_vec3( matrix, scale );
333 void m4x4_pivoted_rotate_by_vec3( m4x4_t matrix, const vec3_t euler, eulerOrder_t order, const vec3_t pivotpoint ){
335 VectorNegative( pivotpoint, vec3_temp );
337 m4x4_translate_by_vec3( matrix, pivotpoint );
338 m4x4_rotate_by_vec3( matrix, euler, order );
339 m4x4_translate_by_vec3( matrix, vec3_temp );
342 void m4x4_pivoted_scale_by_vec3( m4x4_t matrix, const vec3_t scale, const vec3_t pivotpoint ){
344 VectorNegative( pivotpoint, vec3_temp );
346 m4x4_translate_by_vec3( matrix, pivotpoint );
347 m4x4_scale_by_vec3( matrix, scale );
348 m4x4_translate_by_vec3( matrix, vec3_temp );
351 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 ){
354 VectorAdd( pivotpoint, translation, vec3_temp );
355 m4x4_translate_by_vec3( matrix, vec3_temp );
356 m4x4_rotate_by_vec3( matrix, euler, order );
357 m4x4_scale_by_vec3( matrix, scale );
358 VectorNegative( pivotpoint, vec3_temp );
359 m4x4_translate_by_vec3( matrix, vec3_temp );
362 void m4x4_pivoted_rotate_by_quat( m4x4_t matrix, const vec4_t rotation, const vec3_t pivotpoint ){
364 VectorNegative( pivotpoint, vec3_temp );
366 m4x4_translate_by_vec3( matrix, pivotpoint );
367 m4x4_rotate_by_quat( matrix, rotation );
368 m4x4_translate_by_vec3( matrix, vec3_temp );
371 void m4x4_pivoted_rotate_by_axisangle( m4x4_t matrix, const vec3_t axis, vec_t angle, const vec3_t pivotpoint ){
373 VectorNegative( pivotpoint, vec3_temp );
375 m4x4_translate_by_vec3( matrix, pivotpoint );
376 m4x4_rotate_by_axisangle( matrix, axis, angle );
377 m4x4_translate_by_vec3( matrix, vec3_temp );
384 A0 = B0 * A0 + B1 * A4 + B2 * A8 + B3 * A12
385 A4 = B4 * A0 + B5 * A4 + B6 * A8 + B7 * A12
386 A8 = B8 * A0 + B9 * A4 + B10* A8 + B11* A12
387 A12= B12* A0 + B13* A4 + B14* A8 + B15* A12
389 A1 = B0 * A1 + B1 * A5 + B2 * A9 + B3 * A13
390 A5 = B4 * A1 + B5 * A5 + B6 * A9 + B7 * A13
391 A9 = B8 * A1 + B9 * A5 + B10* A9 + B11* A13
392 A13= B12* A1 + B13* A5 + B14* A9 + B15* A13
394 A2 = B0 * A2 + B1 * A6 + B2 * A10+ B3 * A14
395 A6 = B4 * A2 + B5 * A6 + B6 * A10+ B7 * A14
396 A10= B8 * A2 + B9 * A6 + B10* A10+ B11* A14
397 A14= B12* A2 + B13* A6 + B14* A10+ B15* A14
399 A3 = B0 * A3 + B1 * A7 + B2 * A11+ B3 * A15
400 A7 = B4 * A3 + B5 * A7 + B6 * A11+ B7 * A15
401 A11= B8 * A3 + B9 * A7 + B10* A11+ B11* A15
402 A15= B12* A3 + B13* A7 + B14* A11+ B15* A15
405 void m4x4_multiply_by_m4x4( m4x4_t dst, const m4x4_t src ){
406 vec_t dst0, dst1, dst2, dst3;
410 dst0 = src[0] * dst[0] + src[1] * dst[4] + src[2] * dst[8] + src[3] * dst[12];
411 dst1 = src[4] * dst[0] + src[5] * dst[4] + src[6] * dst[8] + src[7] * dst[12];
412 dst2 = src[8] * dst[0] + src[9] * dst[4] + src[10] * dst[8] + src[11] * dst[12];
413 dst3 = src[12] * dst[0] + src[13] * dst[4] + src[14] * dst[8] + src[15] * dst[12];
414 dst[0] = dst0; dst[4] = dst1; dst[8] = dst2; dst[12] = dst3;
416 dst0 = src[0] * dst[1] + src[1] * dst[5] + src[2] * dst[9] + src[3] * dst[13];
417 dst1 = src[4] * dst[1] + src[5] * dst[5] + src[6] * dst[9] + src[7] * dst[13];
418 dst2 = src[8] * dst[1] + src[9] * dst[5] + src[10] * dst[9] + src[11] * dst[13];
419 dst3 = src[12] * dst[1] + src[13] * dst[5] + src[14] * dst[9] + src[15] * dst[13];
420 dst[1] = dst0; dst[5] = dst1; dst[9] = dst2; dst[13] = dst3;
422 dst0 = src[0] * dst[2] + src[1] * dst[6] + src[2] * dst[10] + src[3] * dst[14];
423 dst1 = src[4] * dst[2] + src[5] * dst[6] + src[6] * dst[10] + src[7] * dst[14];
424 dst2 = src[8] * dst[2] + src[9] * dst[6] + src[10] * dst[10] + src[11] * dst[14];
425 dst3 = src[12] * dst[2] + src[13] * dst[6] + src[14] * dst[10] + src[15] * dst[14];
426 dst[2] = dst0; dst[6] = dst1; dst[10] = dst2; dst[14] = dst3;
428 dst0 = src[0] * dst[3] + src[1] * dst[7] + src[2] * dst[11] + src[3] * dst[15];
429 dst1 = src[4] * dst[3] + src[5] * dst[7] + src[6] * dst[11] + src[7] * dst[15];
430 dst2 = src[8] * dst[3] + src[9] * dst[7] + src[10] * dst[11] + src[11] * dst[15];
431 dst3 = src[12] * dst[3] + src[13] * dst[7] + src[14] * dst[11] + src[15] * dst[15];
432 dst[3] = dst0; dst[7] = dst1; dst[11] = dst2; dst[15] = dst3;
437 for ( int i = 0; i < 4; i++ )
439 dst1 = src[0] * p[0];
440 dst1 += src[1] * p[4];
441 dst1 += src[2] * p[8];
442 dst1 += src[3] * p[12];
443 dst2 = src[4] * p[0];
444 dst2 += src[5] * p[4];
445 dst2 += src[6] * p[8];
446 dst2 += src[7] * p[12];
447 dst3 = src[8] * p[0];
448 dst3 += src[9] * p[4];
449 dst3 += src[10] * p[8];
450 dst3 += src[11] * p[12];
451 dst4 = src[12] * p[0];
452 dst4 += src[13] * p[4];
453 dst4 += src[14] * p[8];
454 dst4 += src[15] * p[12];
469 A0 = A0 * B0 + A1 * B4 + A2 * B8 + A3 * B12
470 A1 = A0 * B1 + A1 * B5 + A2 * B9 + A3 * B13
471 A2 = A0 * B2 + A1 * B6 + A2 * B10+ A3 * B14
472 A3 = A0 * B3 + A1 * B7 + A2 * B11+ A3 * B15
474 A4 = A4 * B0 + A5 * B4 + A6 * B8 + A7 * B12
475 A5 = A4 * B1 + A5 * B5 + A6 * B9 + A7 * B13
476 A6 = A4 * B2 + A5 * B6 + A6 * B10+ A7 * B14
477 A7 = A4 * B3 + A5 * B7 + A6 * B11+ A7 * B15
479 A8 = A8 * B0 + A9 * B4 + A10* B8 + A11* B12
480 A9 = A8 * B1 + A9 * B5 + A10* B9 + A11* B13
481 A10= A8 * B2 + A9 * B6 + A10* B10+ A11* B14
482 A11= A8 * B3 + A9 * B7 + A10* B11+ A11* B15
484 A12= A12* B0 + A13* B4 + A14* B8 + A15* B12
485 A13= A12* B1 + A13* B5 + A14* B9 + A15* B13
486 A14= A12* B2 + A13* B6 + A14* B10+ A15* B14
487 A15= A12* B3 + A13* B7 + A14* B11+ A15* B15
490 void m4x4_premultiply_by_m4x4( m4x4_t dst, const m4x4_t src ){
491 vec_t dst0, dst1, dst2, dst3;
495 dst0 = dst[0] * src[0] + dst[1] * src[4] + dst[2] * src[8] + dst[3] * src[12];
496 dst1 = dst[0] * src[1] + dst[1] * src[5] + dst[2] * src[9] + dst[3] * src[13];
497 dst2 = dst[0] * src[2] + dst[1] * src[6] + dst[2] * src[10] + dst[3] * src[14];
498 dst3 = dst[0] * src[3] + dst[1] * src[7] + dst[2] * src[11] + dst[3] * src[15];
499 dst[0] = dst0; dst[1] = dst1; dst[2] = dst2; dst[3] = dst3;
501 dst0 = dst[4] * src[0] + dst[5] * src[4] + dst[6] * src[8] + dst[7] * src[12];
502 dst1 = dst[4] * src[1] + dst[5] * src[5] + dst[6] * src[9] + dst[7] * src[13];
503 dst2 = dst[4] * src[2] + dst[5] * src[6] + dst[6] * src[10] + dst[7] * src[14];
504 dst3 = dst[4] * src[3] + dst[5] * src[7] + dst[6] * src[11] + dst[7] * src[15];
505 dst[4] = dst0; dst[5] = dst1; dst[6] = dst2; dst[7] = dst3;
507 dst0 = dst[8] * src[0] + dst[9] * src[4] + dst[10] * src[8] + dst[11] * src[12];
508 dst1 = dst[8] * src[1] + dst[9] * src[5] + dst[10] * src[9] + dst[11] * src[13];
509 dst2 = dst[8] * src[2] + dst[9] * src[6] + dst[10] * src[10] + dst[11] * src[14];
510 dst3 = dst[8] * src[3] + dst[9] * src[7] + dst[10] * src[11] + dst[11] * src[15];
511 dst[8] = dst0; dst[9] = dst1; dst[10] = dst2; dst[11] = dst3;
513 dst0 = dst[12] * src[0] + dst[13] * src[4] + dst[14] * src[8] + dst[15] * src[12];
514 dst1 = dst[12] * src[1] + dst[13] * src[5] + dst[14] * src[9] + dst[15] * src[13];
515 dst2 = dst[12] * src[2] + dst[13] * src[6] + dst[14] * src[10] + dst[15] * src[14];
516 dst3 = dst[12] * src[3] + dst[13] * src[7] + dst[14] * src[11] + dst[15] * src[15];
517 dst[12] = dst0; dst[13] = dst1; dst[14] = dst2; dst[15] = dst3;
522 for ( int i = 0; i < 4; i++ )
524 dst1 = src[0] * p[0];
525 dst2 = src[1] * p[0];
526 dst3 = src[2] * p[0];
527 dst4 = src[3] * p[0];
528 dst1 += src[4] * p[1];
529 dst2 += src[5] * p[1];
530 dst3 += src[6] * p[1];
531 dst4 += src[7] * p[1];
532 dst1 += src[8] * p[2];
533 dst2 += src[9] * p[2];
534 dst4 += src[11] * p[2];
535 dst3 += src[10] * p[2];
536 dst1 += src[12] * p[3];
537 dst2 += src[13] * p[3];
538 dst3 += src[14] * p[3];
539 dst4 += src[15] * p[3];
550 void m4x4_transform_point( const m4x4_t matrix, vec3_t point ){
551 float out1, out2, out3;
553 out1 = matrix[0] * point[0];
554 out2 = matrix[1] * point[0];
555 out3 = matrix[2] * point[0];
556 out1 += matrix[4] * point[1];
557 out2 += matrix[5] * point[1];
558 out3 += matrix[6] * point[1];
559 out1 += matrix[8] * point[2];
560 out2 += matrix[9] * point[2];
561 out3 += matrix[10] * point[2];
571 void m4x4_transform_normal( const m4x4_t matrix, vec3_t normal ){
572 float out1, out2, out3;
574 out1 = matrix[0] * normal[0];
575 out2 = matrix[1] * normal[0];
576 out3 = matrix[2] * normal[0];
577 out1 += matrix[4] * normal[1];
578 out2 += matrix[5] * normal[1];
579 out3 += matrix[6] * normal[1];
580 out1 += matrix[8] * normal[2];
581 out2 += matrix[9] * normal[2];
582 out3 += matrix[10] * normal[2];
589 void m4x4_transform_vec4( const m4x4_t matrix, vec4_t vector ){
590 float out1, out2, out3, out4;
592 out1 = matrix[0] * vector[0];
593 out2 = matrix[1] * vector[0];
594 out3 = matrix[2] * vector[0];
595 out4 = matrix[3] * vector[0];
596 out1 += matrix[4] * vector[1];
597 out2 += matrix[5] * vector[1];
598 out3 += matrix[6] * vector[1];
599 out4 += matrix[7] * vector[1];
600 out1 += matrix[8] * vector[2];
601 out2 += matrix[9] * vector[2];
602 out3 += matrix[10] * vector[2];
603 out4 += matrix[11] * vector[2];
604 out1 += matrix[12] * vector[3];
605 out2 += matrix[13] * vector[3];
606 out3 += matrix[14] * vector[3];
607 out4 += matrix[15] * vector[3];
615 void m4x4_transpose( m4x4_t matrix ){
617 float temp, *p1, *p2;
619 for ( i = 1; i < 4; i++ ) {
620 for ( j = 0; j < i; j++ ) {
621 p1 = matrix + ( j * 4 + i );
622 p2 = matrix + ( i * 4 + j );
630 void m4x4_orthogonal_invert( m4x4_t matrix ){
634 matrix[3] = matrix[12];
638 matrix[7] = matrix[13];
642 matrix[11] = matrix[14];
647 matrix[1] = matrix[4];
651 matrix[2] = matrix[8];
655 matrix[6] = matrix[9];
658 matrix[3] = -matrix[3];
659 matrix[7] = -matrix[7];
660 matrix[11] = -matrix[11];
664 float m3_det( m3x3_t mat ){
667 det = mat[0] * ( mat[4] * mat[8] - mat[7] * mat[5] )
668 - mat[1] * ( mat[3] * mat[8] - mat[6] * mat[5] )
669 + mat[2] * ( mat[3] * mat[7] - mat[6] * mat[4] );
675 void m3_inverse( m3x3_t mr, m3x3_t ma )
677 float det = m3_det( ma );
679 if ( fabs( det ) < 0.0005 )
685 mr[0] = ma[4]*ma[8] - ma[5]*ma[7] / det;
686 mr[1] = -( ma[1]*ma[8] - ma[7]*ma[2] ) / det;
687 mr[2] = ma[1]*ma[5] - ma[4]*ma[2] / det;
689 mr[3] = -( ma[3]*ma[8] - ma[5]*ma[6] ) / det;
690 mr[4] = ma[0]*ma[8] - ma[6]*ma[2] / det;
691 mr[5] = -( ma[0]*ma[5] - ma[3]*ma[2] ) / det;
693 mr[6] = ma[3]*ma[7] - ma[6]*ma[4] / det;
694 mr[7] = -( ma[0]*ma[7] - ma[6]*ma[1] ) / det;
695 mr[8] = ma[0]*ma[4] - ma[1]*ma[3] / det;
699 void m4_submat( m4x4_t mr, m3x3_t mb, int i, int j ){
700 int ti, tj, idst, jdst;
703 for ( ti = 0; ti < 4; ti++ )
713 for ( tj = 0; tj < 4; tj++ )
723 if ( ti != i && tj != j ) {
724 mb[idst * 3 + jdst] = mr[ti * 4 + tj ];
730 float m4_det( m4x4_t mr ){
731 float det, result = 0, i = 1;
735 for ( n = 0; n < 4; n++, i *= -1 )
737 m4_submat( mr, msub3, 0, n );
739 det = m3_det( msub3 );
740 result += mr[n] * det * i;
746 int m4x4_invert( m4x4_t matrix ){
747 float mdet = m4_det( matrix );
752 if ( fabs( mdet ) < 0.0000000001 ) { //% 0.0005
756 memcpy( m4x4_temp, matrix, sizeof( m4x4_t ) );
758 for ( i = 0; i < 4; i++ )
759 for ( j = 0; j < 4; j++ )
761 sign = 1 - ( ( i + j ) % 2 ) * 2;
763 m4_submat( m4x4_temp, mtemp, i, j );
765 matrix[i + j * 4] = ( m3_det( mtemp ) * sign ) / mdet;