2 Copyright (C) 2001-2006, William Joseph.
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
24 const m4x4_t g_m4x4_identity = {
31 void m4x4_identity( m4x4_t matrix ){
32 matrix[1] = matrix[2] = matrix[3] =
33 matrix[4] = matrix[6] = matrix[7] =
34 matrix[8] = matrix[9] = matrix[11] =
35 matrix[12] = matrix[13] = matrix[14] = 0;
37 matrix[0] = matrix[5] = matrix[10] = matrix[15] = 1;
40 m4x4Handedness_t m4x4_handedness( const m4x4_t matrix ){
42 CrossProduct( matrix + 0, matrix + 4, cross );
43 return ( DotProduct( matrix + 8, cross ) < 0 ) ? eLeftHanded : eRightHanded;
46 void m4x4_assign( m4x4_t matrix, const m4x4_t other ){
47 M4X4_COPY( matrix, other );
50 void m4x4_translation_for_vec3( m4x4_t matrix, const vec3_t translation ){
51 matrix[1] = matrix[2] = matrix[3] =
52 matrix[4] = matrix[6] = matrix[7] =
53 matrix[8] = matrix[9] = matrix[11] = 0;
55 matrix[0] = matrix[5] = matrix[10] = matrix[15] = 1;
57 matrix[12] = translation[0];
58 matrix[13] = translation[1];
59 matrix[14] = translation[2];
63 clockwise rotation around X, Y, Z, facing along axis
65 0 cx sx 0 1 0 -sz cz 0
66 0 -sx cx -sy 0 cy 0 0 1
68 rows of Z by cols of Y
69 cy*cz -sy*cz+sz -sy*sz+cz
72 .. or something like that..
74 final rotation is Z * Y * X
75 cy*cz -sx*-sy*cz+cx*sz cx*-sy*sz+sx*cz
76 -cy*sz sx*sy*sz+cx*cz -cx*-sy*sz+sx*cz
81 | cy.cz + 0.sz + sy.0 cy.-sz + 0 .cz + sy.0 cy.0 + 0 .0 + sy.1 |
82 | sx.sy.cz + cx.sz + -sx.cy.0 sx.sy.-sz + cx.cz + -sx.cy.0 sx.sy.0 + cx.0 + -sx.cy.1 |
83 | -cx.sy.cz + sx.sz + cx.cy.0 -cx.sy.-sz + sx.cz + cx.cy.0 -cx.sy.0 + 0 .0 + cx.cy.1 |
85 void m4x4_rotation_for_vec3( m4x4_t matrix, const vec3_t euler, eulerOrder_t order ){
86 double cx, sx, cy, sy, cz, sz;
88 cx = cos( DEG2RAD( euler[0] ) );
89 sx = sin( DEG2RAD( euler[0] ) );
90 cy = cos( DEG2RAD( euler[1] ) );
91 sy = sin( DEG2RAD( euler[1] ) );
92 cz = cos( DEG2RAD( euler[2] ) );
93 sz = sin( DEG2RAD( euler[2] ) );
102 matrix[0] = (vec_t)( cy * cz );
103 matrix[1] = (vec_t)( cy * sz );
104 matrix[2] = (vec_t)-sy;
105 matrix[4] = (vec_t)( sx * sy * cz + cx * -sz );
106 matrix[5] = (vec_t)( sx * sy * sz + cx * cz );
107 matrix[6] = (vec_t)( sx * cy );
108 matrix[8] = (vec_t)( cx * sy * cz + sx * sz );
109 matrix[9] = (vec_t)( cx * sy * sz + -sx * cz );
110 matrix[10] = (vec_t)( cx * cy );
113 matrix[12] = matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0;
118 m4x4_identity( matrix );
119 matrix[5] = (vec_t) cx; matrix[6] = (vec_t) sx;
120 matrix[9] = (vec_t)-sx; matrix[10] = (vec_t) cx;
124 m4x4_identity( temp );
125 temp[0] = (vec_t) cy; temp[2] = (vec_t)-sy;
126 temp[8] = (vec_t) sy; temp[10] = (vec_t) cy;
127 m4x4_premultiply_by_m4x4( matrix, temp );
128 m4x4_identity( temp );
129 temp[0] = (vec_t) cz; temp[1] = (vec_t) sz;
130 temp[4] = (vec_t)-sz; temp[5] = (vec_t) cz;
131 m4x4_premultiply_by_m4x4( matrix, temp );
138 m4x4_identity( matrix );
139 matrix[0] = (vec_t) cy; matrix[2] = (vec_t)-sy;
140 matrix[8] = (vec_t) sy; matrix[10] = (vec_t) cy;
144 m4x4_identity( temp );
145 temp[5] = (vec_t) cx; temp[6] = (vec_t) sx;
146 temp[9] = (vec_t)-sx; temp[10] = (vec_t) cx;
147 m4x4_premultiply_by_m4x4( matrix, temp );
148 m4x4_identity( temp );
149 temp[0] = (vec_t) cz; temp[1] = (vec_t) sz;
150 temp[4] = (vec_t)-sz; temp[5] = (vec_t) cz;
151 m4x4_premultiply_by_m4x4( matrix, temp );
156 m4x4_identity( matrix );
157 matrix[0] = (vec_t) cz; matrix[1] = (vec_t) sz;
158 matrix[4] = (vec_t)-sz; matrix[5] = (vec_t) cz;
162 m4x4_identity( temp );
163 temp[5] = (vec_t) cx; temp[6] = (vec_t) sx;
164 temp[9] = (vec_t)-sx; temp[10] = (vec_t) cx;
165 m4x4_premultiply_by_m4x4( matrix, temp );
166 m4x4_identity( temp );
167 temp[0] = (vec_t) cy; temp[2] = (vec_t)-sy;
168 temp[8] = (vec_t) sy; temp[10] = (vec_t) cy;
169 m4x4_premultiply_by_m4x4( matrix, temp );
174 m4x4_identity( matrix );
175 matrix[5] = (vec_t) cx; matrix[6] = (vec_t) sx;
176 matrix[9] = (vec_t)-sx; matrix[10] = (vec_t) cx;
180 m4x4_identity( temp );
181 temp[0] = (vec_t) cz; temp[1] = (vec_t) sz;
182 temp[4] = (vec_t)-sz; temp[5] = (vec_t) cz;
183 m4x4_premultiply_by_m4x4( matrix, temp );
184 m4x4_identity( temp );
185 temp[0] = (vec_t) cy; temp[2] = (vec_t)-sy;
186 temp[8] = (vec_t) sy; temp[10] = (vec_t) cy;
187 m4x4_premultiply_by_m4x4( matrix, temp );
194 | cy.cz + sx.sy.-sz + -cx.sy.0 0.cz + cx.-sz + sx.0 sy.cz + -sx.cy.-sz + cx.cy.0 |
195 | cy.sz + sx.sy.cz + -cx.sy.0 0.sz + cx.cz + sx.0 sy.sz + -sx.cy.cz + cx.cy.0 |
196 | cy.0 + sx.sy.0 + -cx.sy.1 0.0 + cx.0 + sx.1 sy.0 + -sx.cy.0 + cx.cy.1 |
202 matrix[0] = (vec_t)( cy * cz + sx * sy * -sz );
203 matrix[1] = (vec_t)( cy * sz + sx * sy * cz );
204 matrix[2] = (vec_t)( -cx * sy );
205 matrix[4] = (vec_t)( cx * -sz );
206 matrix[5] = (vec_t)( cx * cz );
207 matrix[6] = (vec_t)( sx );
208 matrix[8] = (vec_t)( sy * cz + -sx * cy * -sz );
209 matrix[9] = (vec_t)( sy * sz + -sx * cy * cz );
210 matrix[10] = (vec_t)( cx * cy );
213 matrix[12] = matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0;
218 m4x4_identity( matrix );
219 matrix[0] = (vec_t) cy; matrix[2] = (vec_t)-sy;
220 matrix[8] = (vec_t) sy; matrix[10] = (vec_t) cy;
224 m4x4_identity( temp );
225 temp[5] = (vec_t) cx; temp[6] = (vec_t) sx;
226 temp[9] = (vec_t)-sx; temp[10] = (vec_t) cx;
227 m4x4_premultiply_by_m4x4( matrix, temp );
228 m4x4_identity( temp );
229 temp[0] = (vec_t) cz; temp[1] = (vec_t) sz;
230 temp[4] = (vec_t)-sz; temp[5] = (vec_t) cz;
231 m4x4_premultiply_by_m4x4( matrix, temp );
240 matrix[0] = (vec_t)( cy * cz );
241 matrix[4] = (vec_t)( cy * -sz );
242 matrix[8] = (vec_t)sy;
243 matrix[1] = (vec_t)( sx * sy * cz + cx * sz );
244 matrix[5] = (vec_t)( sx * sy * -sz + cx * cz );
245 matrix[9] = (vec_t)( -sx * cy );
246 matrix[2] = (vec_t)( cx * -sy * cz + sx * sz );
247 matrix[6] = (vec_t)( cx * -sy * -sz + sx * cz );
248 matrix[10] = (vec_t)( cx * cy );
251 matrix[12] = matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0;
256 m4x4_identity( matrix );
257 matrix[0] = (vec_t) cz; matrix[1] = (vec_t) sz;
258 matrix[4] = (vec_t)-sz; matrix[5] = (vec_t) cz;
261 m4x4_identity( temp );
262 temp[0] = (vec_t) cy; temp[2] = (vec_t)-sy;
263 temp[8] = (vec_t) sy; temp[10] = (vec_t) cy;
264 m4x4_premultiply_by_m4x4( matrix, temp );
265 m4x4_identity( temp );
266 temp[5] = (vec_t) cx; temp[6] = (vec_t) sx;
267 temp[9] = (vec_t)-sx; temp[10] = (vec_t) cx;
268 m4x4_premultiply_by_m4x4( matrix, temp );
277 void m4x4_scale_for_vec3( m4x4_t matrix, const vec3_t scale ){
278 matrix[1] = matrix[2] = matrix[3] =
279 matrix[4] = matrix[6] = matrix[7] =
280 matrix[8] = matrix[9] = matrix[11] =
281 matrix[12] = matrix[13] = matrix[14] = 0;
285 matrix[0] = scale[0];
286 matrix[5] = scale[1];
287 matrix[10] = scale[2];
290 void m4x4_rotation_for_quat( m4x4_t matrix, const vec4_t quat ){
292 const double xx = quat[0] * quat[0];
293 const double xy = quat[0] * quat[1];
294 const double xz = quat[0] * quat[2];
295 const double xw = quat[0] * quat[3];
297 const double yy = quat[1] * quat[1];
298 const double yz = quat[1] * quat[2];
299 const double yw = quat[1] * quat[3];
301 const double zz = quat[2] * quat[2];
302 const double zw = quat[2] * quat[3];
304 matrix[0] = 1 - 2 * ( yy + zz );
305 matrix[4] = 2 * ( xy - zw );
306 matrix[8] = 2 * ( xz + yw );
308 matrix[1] = 2 * ( xy + zw );
309 matrix[5] = 1 - 2 * ( xx + zz );
310 matrix[9] = 2 * ( yz - xw );
312 matrix[2] = 2 * ( xz - yw );
313 matrix[6] = 2 * ( yz + xw );
314 matrix[10] = 1 - 2 * ( xx + yy );
316 const double x2 = quat[0] + quat[0];
317 const double y2 = quat[1] + quat[1];
318 const double z2 = quat[2] + quat[2];
319 const double xx = quat[0] * x2;
320 const double xy = quat[0] * y2;
321 const double xz = quat[0] * z2;
322 const double yy = quat[1] * y2;
323 const double yz = quat[1] * z2;
324 const double zz = quat[2] * z2;
325 const double wx = quat[3] * x2;
326 const double wy = quat[3] * y2;
327 const double wz = quat[3] * z2;
329 matrix[0] = (vec_t)( 1.0 - ( yy + zz ) );
330 matrix[4] = (vec_t)( xy - wz );
331 matrix[8] = (vec_t)( xz + wy );
333 matrix[1] = (vec_t)( xy + wz );
334 matrix[5] = (vec_t)( 1.0 - ( xx + zz ) );
335 matrix[9] = (vec_t)( yz - wx );
337 matrix[2] = (vec_t)( xz - wy );
338 matrix[6] = (vec_t)( yz + wx );
339 matrix[10] = (vec_t)( 1.0 - ( xx + yy ) );
342 matrix[3] = matrix[7] = matrix[11] = matrix[12] = matrix[13] = matrix[14] = 0;
346 void m4x4_rotation_for_axisangle( m4x4_t matrix, const vec3_t axis, double angle ){
348 quat_for_axisangle( quat, axis, angle );
349 m4x4_rotation_for_quat( matrix, quat );
352 void m4x4_frustum( m4x4_t matrix,
353 vec_t left, vec_t right,
354 vec_t bottom, vec_t top,
355 vec_t nearval, vec_t farval ){
356 matrix[0] = (vec_t)( ( 2 * nearval ) / ( right - left ) );
362 matrix[5] = (vec_t)( ( 2 * nearval ) / ( top - bottom ) );
366 matrix[8] = (vec_t)( ( right + left ) / ( right - left ) );
367 matrix[9] = (vec_t)( ( top + bottom ) / ( top - bottom ) );
368 matrix[10] = (vec_t)( -( farval + nearval ) / ( farval - nearval ) );
373 matrix[14] = (vec_t)( -( 2 * farval * nearval ) / ( farval - nearval ) );
378 void m4x4_get_translation_vec3( const m4x4_t matrix, vec3_t translation ){
379 translation[0] = matrix[12];
380 translation[1] = matrix[13];
381 translation[2] = matrix[14];
384 void m4x4_get_rotation_vec3( const m4x4_t matrix, vec3_t euler, eulerOrder_t order ){
390 a = asin( -matrix[2] );
392 euler[1] = (vec_t)RAD2DEG( a ); /* Calculate Y-axis angle */
394 if ( fabs( ca ) > 0.005 ) { /* Gimbal lock? */
395 /* No, so get Z-axis angle */
396 euler[2] = (vec_t)RAD2DEG( atan2( matrix[1] / ca, matrix[0] / ca ) );
398 /* Get X-axis angle */
399 euler[0] = (vec_t)RAD2DEG( atan2( matrix[6] / ca, matrix[10] / ca ) );
401 else /* Gimbal lock has occurred */
403 /* Set Z-axis angle to zero */
406 /* And calculate X-axis angle */
407 euler[0] = (vec_t)RAD2DEG( atan2( -matrix[9], matrix[5] ) );
411 /* NOT IMPLEMENTED */
414 /* NOT IMPLEMENTED */
417 /* NOT IMPLEMENTED */
420 a = asin( matrix[6] );
422 euler[0] = (vec_t)RAD2DEG( a ); /* Calculate X-axis angle */
424 if ( fabs( ca ) > 0.005 ) { /* Gimbal lock? */
425 /* No, so get Y-axis angle */
426 euler[1] = (vec_t)RAD2DEG( atan2( -matrix[2] / ca, matrix[10] / ca ) );
428 /* Get Z-axis angle */
429 euler[2] = (vec_t)RAD2DEG( atan2( -matrix[4] / ca, matrix[5] / ca ) );
431 else /* Gimbal lock has occurred */
433 /* Set Z-axis angle to zero */
436 /* And calculate Y-axis angle */
437 euler[1] = (vec_t)RAD2DEG( atan2( matrix[8], matrix[0] ) );
441 a = asin( matrix[8] );
443 euler[1] = (vec_t)RAD2DEG( a ); /* Calculate Y-axis angle */
445 if ( fabs( ca ) > 0.005 ) { /* Gimbal lock? */
446 /* No, so get X-axis angle */
447 euler[0] = (vec_t)RAD2DEG( atan2( -matrix[9] / ca, matrix[10] / ca ) );
449 /* Get Z-axis angle */
450 euler[2] = (vec_t)RAD2DEG( atan2( -matrix[4] / ca, matrix[0] / ca ) );
452 else /* Gimbal lock has occurred */
454 /* Set X-axis angle to zero */
457 /* And calculate Z-axis angle */
458 euler[2] = (vec_t)RAD2DEG( atan2( matrix[1], matrix[5] ) );
463 /* return only positive angles in [0,360] */
464 if ( euler[0] < 0 ) {
467 if ( euler[1] < 0 ) {
470 if ( euler[2] < 0 ) {
475 void m4x4_get_scale_vec3( const m4x4_t matrix, vec3_t scale ){
476 scale[0] = VectorLength( matrix + 0 );
477 scale[1] = VectorLength( matrix + 4 );
478 scale[2] = VectorLength( matrix + 8 );
481 void m4x4_get_transform_vec3( const m4x4_t matrix, vec3_t translation, vec3_t euler, eulerOrder_t order, vec3_t scale ){
483 m4x4_assign( normalised, matrix );
484 scale[0] = VectorNormalize( normalised + 0, normalised + 0 );
485 scale[1] = VectorNormalize( normalised + 4, normalised + 4 );
486 scale[2] = VectorNormalize( normalised + 8, normalised + 8 );
487 if ( m4x4_handedness( normalised ) == eLeftHanded ) {
488 VectorNegate( normalised + 0, normalised + 0 );
489 VectorNegate( normalised + 4, normalised + 4 );
490 VectorNegate( normalised + 8, normalised + 8 );
491 scale[0] = -scale[0];
492 scale[1] = -scale[1];
493 scale[2] = -scale[2];
495 m4x4_get_rotation_vec3( normalised, euler, order );
496 m4x4_get_translation_vec3( matrix, translation );
499 void m4x4_translate_by_vec3( m4x4_t matrix, const vec3_t translation ){
501 m4x4_translation_for_vec3( temp, translation );
502 m4x4_multiply_by_m4x4( matrix, temp );
505 void m4x4_rotate_by_vec3( m4x4_t matrix, const vec3_t euler, eulerOrder_t order ){
507 m4x4_rotation_for_vec3( temp, euler, order );
508 m4x4_multiply_by_m4x4( matrix, temp );
511 void m4x4_scale_by_vec3( m4x4_t matrix, const vec3_t scale ){
513 m4x4_scale_for_vec3( temp, scale );
514 m4x4_multiply_by_m4x4( matrix, temp );
517 void m4x4_rotate_by_quat( m4x4_t matrix, const vec4_t rotation ){
519 m4x4_rotation_for_quat( temp, rotation );
520 m4x4_multiply_by_m4x4( matrix, temp );
523 void m4x4_rotate_by_axisangle( m4x4_t matrix, const vec3_t axis, double angle ){
525 m4x4_rotation_for_axisangle( temp, axis, angle );
526 m4x4_multiply_by_m4x4( matrix, temp );
529 void m4x4_transform_by_vec3( m4x4_t matrix, const vec3_t translation, const vec3_t euler, eulerOrder_t order, const vec3_t scale ){
530 m4x4_translate_by_vec3( matrix, translation );
531 m4x4_rotate_by_vec3( matrix, euler, order );
532 m4x4_scale_by_vec3( matrix, scale );
535 void m4x4_pivoted_rotate_by_vec3( m4x4_t matrix, const vec3_t euler, eulerOrder_t order, const vec3_t pivotpoint ){
537 VectorNegate( pivotpoint, vec3_temp );
539 m4x4_translate_by_vec3( matrix, pivotpoint );
540 m4x4_rotate_by_vec3( matrix, euler, order );
541 m4x4_translate_by_vec3( matrix, vec3_temp );
544 void m4x4_pivoted_scale_by_vec3( m4x4_t matrix, const vec3_t scale, const vec3_t pivotpoint ){
546 VectorNegate( pivotpoint, vec3_temp );
548 m4x4_translate_by_vec3( matrix, pivotpoint );
549 m4x4_scale_by_vec3( matrix, scale );
550 m4x4_translate_by_vec3( matrix, vec3_temp );
553 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 ){
556 VectorAdd( pivotpoint, translation, vec3_temp );
557 m4x4_translate_by_vec3( matrix, vec3_temp );
558 m4x4_rotate_by_vec3( matrix, euler, order );
559 m4x4_scale_by_vec3( matrix, scale );
560 VectorNegate( pivotpoint, vec3_temp );
561 m4x4_translate_by_vec3( matrix, vec3_temp );
564 void m4x4_pivoted_transform_by_rotation( m4x4_t matrix, const vec3_t translation, const m4x4_t rotation, const vec3_t scale, const vec3_t pivotpoint ){
567 VectorAdd( pivotpoint, translation, vec3_temp );
568 m4x4_translate_by_vec3( matrix, vec3_temp );
569 m4x4_multiply_by_m4x4( matrix, rotation );
570 m4x4_scale_by_vec3( matrix, scale );
571 VectorNegate( pivotpoint, vec3_temp );
572 m4x4_translate_by_vec3( matrix, vec3_temp );
575 void m4x4_pivoted_rotate_by_quat( m4x4_t matrix, const vec4_t rotation, const vec3_t pivotpoint ){
577 VectorNegate( pivotpoint, vec3_temp );
579 m4x4_translate_by_vec3( matrix, pivotpoint );
580 m4x4_rotate_by_quat( matrix, rotation );
581 m4x4_translate_by_vec3( matrix, vec3_temp );
584 void m4x4_pivoted_rotate_by_axisangle( m4x4_t matrix, const vec3_t axis, double angle, const vec3_t pivotpoint ){
586 VectorNegate( pivotpoint, vec3_temp );
588 m4x4_translate_by_vec3( matrix, pivotpoint );
589 m4x4_rotate_by_axisangle( matrix, axis, angle );
590 m4x4_translate_by_vec3( matrix, vec3_temp );
596 A0 = B0 * A0 + B1 * A4 + B2 * A8 + B3 * A12
597 A4 = B4 * A0 + B5 * A4 + B6 * A8 + B7 * A12
598 A8 = B8 * A0 + B9 * A4 + B10* A8 + B11* A12
599 A12= B12* A0 + B13* A4 + B14* A8 + B15* A12
601 A1 = B0 * A1 + B1 * A5 + B2 * A9 + B3 * A13
602 A5 = B4 * A1 + B5 * A5 + B6 * A9 + B7 * A13
603 A9 = B8 * A1 + B9 * A5 + B10* A9 + B11* A13
604 A13= B12* A1 + B13* A5 + B14* A9 + B15* A13
606 A2 = B0 * A2 + B1 * A6 + B2 * A10+ B3 * A14
607 A6 = B4 * A2 + B5 * A6 + B6 * A10+ B7 * A14
608 A10= B8 * A2 + B9 * A6 + B10* A10+ B11* A14
609 A14= B12* A2 + B13* A6 + B14* A10+ B15* A14
611 A3 = B0 * A3 + B1 * A7 + B2 * A11+ B3 * A15
612 A7 = B4 * A3 + B5 * A7 + B6 * A11+ B7 * A15
613 A11= B8 * A3 + B9 * A7 + B10* A11+ B11* A15
614 A15= B12* A3 + B13* A7 + B14* A11+ B15* A15
617 void m4x4_multiply_by_m4x4( m4x4_t dst, const m4x4_t src ){
618 vec_t dst0, dst1, dst2, dst3;
622 dst0 = src[0] * dst[0] + src[1] * dst[4] + src[2] * dst[8] + src[3] * dst[12];
623 dst1 = src[4] * dst[0] + src[5] * dst[4] + src[6] * dst[8] + src[7] * dst[12];
624 dst2 = src[8] * dst[0] + src[9] * dst[4] + src[10] * dst[8] + src[11] * dst[12];
625 dst3 = src[12] * dst[0] + src[13] * dst[4] + src[14] * dst[8] + src[15] * dst[12];
626 dst[0] = dst0; dst[4] = dst1; dst[8] = dst2; dst[12] = dst3;
628 dst0 = src[0] * dst[1] + src[1] * dst[5] + src[2] * dst[9] + src[3] * dst[13];
629 dst1 = src[4] * dst[1] + src[5] * dst[5] + src[6] * dst[9] + src[7] * dst[13];
630 dst2 = src[8] * dst[1] + src[9] * dst[5] + src[10] * dst[9] + src[11] * dst[13];
631 dst3 = src[12] * dst[1] + src[13] * dst[5] + src[14] * dst[9] + src[15] * dst[13];
632 dst[1] = dst0; dst[5] = dst1; dst[9] = dst2; dst[13] = dst3;
634 dst0 = src[0] * dst[2] + src[1] * dst[6] + src[2] * dst[10] + src[3] * dst[14];
635 dst1 = src[4] * dst[2] + src[5] * dst[6] + src[6] * dst[10] + src[7] * dst[14];
636 dst2 = src[8] * dst[2] + src[9] * dst[6] + src[10] * dst[10] + src[11] * dst[14];
637 dst3 = src[12] * dst[2] + src[13] * dst[6] + src[14] * dst[10] + src[15] * dst[14];
638 dst[2] = dst0; dst[6] = dst1; dst[10] = dst2; dst[14] = dst3;
640 dst0 = src[0] * dst[3] + src[1] * dst[7] + src[2] * dst[11] + src[3] * dst[15];
641 dst1 = src[4] * dst[3] + src[5] * dst[7] + src[6] * dst[11] + src[7] * dst[15];
642 dst2 = src[8] * dst[3] + src[9] * dst[7] + src[10] * dst[11] + src[11] * dst[15];
643 dst3 = src[12] * dst[3] + src[13] * dst[7] + src[14] * dst[11] + src[15] * dst[15];
644 dst[3] = dst0; dst[7] = dst1; dst[11] = dst2; dst[15] = dst3;
649 for ( int i = 0; i < 4; i++ )
651 dst1 = src[0] * p[0];
652 dst1 += src[1] * p[4];
653 dst1 += src[2] * p[8];
654 dst1 += src[3] * p[12];
655 dst2 = src[4] * p[0];
656 dst2 += src[5] * p[4];
657 dst2 += src[6] * p[8];
658 dst2 += src[7] * p[12];
659 dst3 = src[8] * p[0];
660 dst3 += src[9] * p[4];
661 dst3 += src[10] * p[8];
662 dst3 += src[11] * p[12];
663 dst4 = src[12] * p[0];
664 dst4 += src[13] * p[4];
665 dst4 += src[14] * p[8];
666 dst4 += src[15] * p[12];
681 A0 = A0 * B0 + A1 * B4 + A2 * B8 + A3 * B12
682 A1 = A0 * B1 + A1 * B5 + A2 * B9 + A3 * B13
683 A2 = A0 * B2 + A1 * B6 + A2 * B10+ A3 * B14
684 A3 = A0 * B3 + A1 * B7 + A2 * B11+ A3 * B15
686 A4 = A4 * B0 + A5 * B4 + A6 * B8 + A7 * B12
687 A5 = A4 * B1 + A5 * B5 + A6 * B9 + A7 * B13
688 A6 = A4 * B2 + A5 * B6 + A6 * B10+ A7 * B14
689 A7 = A4 * B3 + A5 * B7 + A6 * B11+ A7 * B15
691 A8 = A8 * B0 + A9 * B4 + A10* B8 + A11* B12
692 A9 = A8 * B1 + A9 * B5 + A10* B9 + A11* B13
693 A10= A8 * B2 + A9 * B6 + A10* B10+ A11* B14
694 A11= A8 * B3 + A9 * B7 + A10* B11+ A11* B15
696 A12= A12* B0 + A13* B4 + A14* B8 + A15* B12
697 A13= A12* B1 + A13* B5 + A14* B9 + A15* B13
698 A14= A12* B2 + A13* B6 + A14* B10+ A15* B14
699 A15= A12* B3 + A13* B7 + A14* B11+ A15* B15
702 void m4x4_premultiply_by_m4x4( m4x4_t dst, const m4x4_t src ){
703 vec_t dst0, dst1, dst2, dst3;
707 dst0 = dst[0] * src[0] + dst[1] * src[4] + dst[2] * src[8] + dst[3] * src[12];
708 dst1 = dst[0] * src[1] + dst[1] * src[5] + dst[2] * src[9] + dst[3] * src[13];
709 dst2 = dst[0] * src[2] + dst[1] * src[6] + dst[2] * src[10] + dst[3] * src[14];
710 dst3 = dst[0] * src[3] + dst[1] * src[7] + dst[2] * src[11] + dst[3] * src[15];
711 dst[0] = dst0; dst[1] = dst1; dst[2] = dst2; dst[3] = dst3;
713 dst0 = dst[4] * src[0] + dst[5] * src[4] + dst[6] * src[8] + dst[7] * src[12];
714 dst1 = dst[4] * src[1] + dst[5] * src[5] + dst[6] * src[9] + dst[7] * src[13];
715 dst2 = dst[4] * src[2] + dst[5] * src[6] + dst[6] * src[10] + dst[7] * src[14];
716 dst3 = dst[4] * src[3] + dst[5] * src[7] + dst[6] * src[11] + dst[7] * src[15];
717 dst[4] = dst0; dst[5] = dst1; dst[6] = dst2; dst[7] = dst3;
719 dst0 = dst[8] * src[0] + dst[9] * src[4] + dst[10] * src[8] + dst[11] * src[12];
720 dst1 = dst[8] * src[1] + dst[9] * src[5] + dst[10] * src[9] + dst[11] * src[13];
721 dst2 = dst[8] * src[2] + dst[9] * src[6] + dst[10] * src[10] + dst[11] * src[14];
722 dst3 = dst[8] * src[3] + dst[9] * src[7] + dst[10] * src[11] + dst[11] * src[15];
723 dst[8] = dst0; dst[9] = dst1; dst[10] = dst2; dst[11] = dst3;
725 dst0 = dst[12] * src[0] + dst[13] * src[4] + dst[14] * src[8] + dst[15] * src[12];
726 dst1 = dst[12] * src[1] + dst[13] * src[5] + dst[14] * src[9] + dst[15] * src[13];
727 dst2 = dst[12] * src[2] + dst[13] * src[6] + dst[14] * src[10] + dst[15] * src[14];
728 dst3 = dst[12] * src[3] + dst[13] * src[7] + dst[14] * src[11] + dst[15] * src[15];
729 dst[12] = dst0; dst[13] = dst1; dst[14] = dst2; dst[15] = dst3;
734 for ( int i = 0; i < 4; i++ )
736 dst1 = src[0] * p[0];
737 dst2 = src[1] * p[0];
738 dst3 = src[2] * p[0];
739 dst4 = src[3] * p[0];
740 dst1 += src[4] * p[1];
741 dst2 += src[5] * p[1];
742 dst3 += src[6] * p[1];
743 dst4 += src[7] * p[1];
744 dst1 += src[8] * p[2];
745 dst2 += src[9] * p[2];
746 dst4 += src[11] * p[2];
747 dst3 += src[10] * p[2];
748 dst1 += src[12] * p[3];
749 dst2 += src[13] * p[3];
750 dst3 += src[14] * p[3];
751 dst4 += src[15] * p[3];
762 void m4x4_orthogonal_multiply_by_m4x4( m4x4_t dst, const m4x4_t src ){
763 vec_t dst0, dst1, dst2, dst3;
765 dst0 = src[0] * dst[0] + src[1] * dst[4] + src[2] * dst[8];
766 dst1 = src[4] * dst[0] + src[5] * dst[4] + src[6] * dst[8];
767 dst2 = src[8] * dst[0] + src[9] * dst[4] + src[10] * dst[8];
768 dst3 = src[12] * dst[0] + src[13] * dst[4] + src[14] * dst[8] + dst[12];
769 dst[0] = dst0; dst[4] = dst1; dst[8] = dst2; dst[12] = dst3;
771 dst0 = src[0] * dst[1] + src[1] * dst[5] + src[2] * dst[9];
772 dst1 = src[4] * dst[1] + src[5] * dst[5] + src[6] * dst[9];
773 dst2 = src[8] * dst[1] + src[9] * dst[5] + src[10] * dst[9];
774 dst3 = src[12] * dst[1] + src[13] * dst[5] + src[14] * dst[9] + dst[13];
775 dst[1] = dst0; dst[5] = dst1; dst[9] = dst2; dst[13] = dst3;
777 dst0 = src[0] * dst[2] + src[1] * dst[6] + src[2] * dst[10];
778 dst1 = src[4] * dst[2] + src[5] * dst[6] + src[6] * dst[10];
779 dst2 = src[8] * dst[2] + src[9] * dst[6] + src[10] * dst[10];
780 dst3 = src[12] * dst[2] + src[13] * dst[6] + src[14] * dst[10] + dst[14];
781 dst[2] = dst0; dst[6] = dst1; dst[10] = dst2; dst[14] = dst3;
784 void m4x4_orthogonal_premultiply_by_m4x4( m4x4_t dst, const m4x4_t src ){
785 vec_t dst0, dst1, dst2;
787 dst0 = dst[0] * src[0] + dst[1] * src[4] + dst[2] * src[8];
788 dst1 = dst[0] * src[1] + dst[1] * src[5] + dst[2] * src[9];
789 dst2 = dst[0] * src[2] + dst[1] * src[6] + dst[2] * src[10];
790 dst[0] = dst0; dst[1] = dst1; dst[2] = dst2;
792 dst0 = dst[4] * src[0] + dst[5] * src[4] + dst[6] * src[8];
793 dst1 = dst[4] * src[1] + dst[5] * src[5] + dst[6] * src[9];
794 dst2 = dst[4] * src[2] + dst[5] * src[6] + dst[6] * src[10];
795 dst[4] = dst0; dst[5] = dst1; dst[6] = dst2;
797 dst0 = dst[8] * src[0] + dst[9] * src[4] + dst[10] * src[8];
798 dst1 = dst[8] * src[1] + dst[9] * src[5] + dst[10] * src[9];
799 dst2 = dst[8] * src[2] + dst[9] * src[6] + dst[10] * src[10];
800 dst[8] = dst0; dst[9] = dst1; dst[10] = dst2;
802 dst0 = dst[12] * src[0] + dst[13] * src[4] + dst[14] * src[8] + dst[15] * src[12];
803 dst1 = dst[12] * src[1] + dst[13] * src[5] + dst[14] * src[9] + dst[15] * src[13];
804 dst2 = dst[12] * src[2] + dst[13] * src[6] + dst[14] * src[10] + dst[15] * src[14];
805 dst[12] = dst0; dst[13] = dst1; dst[14] = dst2;
808 void m4x4_transform_point( const m4x4_t matrix, vec3_t point ){
809 float out1, out2, out3;
811 out1 = matrix[0] * point[0] + matrix[4] * point[1] + matrix[8] * point[2] + matrix[12];
812 out2 = matrix[1] * point[0] + matrix[5] * point[1] + matrix[9] * point[2] + matrix[13];
813 out3 = matrix[2] * point[0] + matrix[6] * point[1] + matrix[10] * point[2] + matrix[14];
820 void m4x4_transform_normal( const m4x4_t matrix, vec3_t normal ){
821 float out1, out2, out3;
823 out1 = matrix[0] * normal[0] + matrix[4] * normal[1] + matrix[8] * normal[2];
824 out2 = matrix[1] * normal[0] + matrix[5] * normal[1] + matrix[9] * normal[2];
825 out3 = matrix[2] * normal[0] + matrix[6] * normal[1] + matrix[10] * normal[2];
832 void m4x4_transform_vec4( const m4x4_t matrix, vec4_t vector ){
833 float out1, out2, out3, out4;
835 out1 = matrix[0] * vector[0] + matrix[4] * vector[1] + matrix[8] * vector[2] + matrix[12] * vector[3];
836 out2 = matrix[1] * vector[0] + matrix[5] * vector[1] + matrix[9] * vector[2] + matrix[13] * vector[3];
837 out3 = matrix[2] * vector[0] + matrix[6] * vector[1] + matrix[10] * vector[2] + matrix[14] * vector[3];
838 out4 = matrix[3] * vector[0] + matrix[7] * vector[1] + matrix[11] * vector[2] + matrix[15] * vector[3];
846 #define CLIP_X_LT_W( p ) ( ( p )[0] < ( p )[3] )
847 #define CLIP_X_GT_W( p ) ( ( p )[0] > -( p )[3] )
848 #define CLIP_Y_LT_W( p ) ( ( p )[1] < ( p )[3] )
849 #define CLIP_Y_GT_W( p ) ( ( p )[1] > -( p )[3] )
850 #define CLIP_Z_LT_W( p ) ( ( p )[2] < ( p )[3] )
851 #define CLIP_Z_GT_W( p ) ( ( p )[2] > -( p )[3] )
853 clipmask_t homogenous_clip_point( const vec4_t clipped ){
854 clipmask_t result = CLIP_FAIL;
855 if ( CLIP_X_LT_W( clipped ) ) {
856 result &= ~CLIP_LT_X; // X < W
858 if ( CLIP_X_GT_W( clipped ) ) {
859 result &= ~CLIP_GT_X; // X > -W
861 if ( CLIP_Y_LT_W( clipped ) ) {
862 result &= ~CLIP_LT_Y; // Y < W
864 if ( CLIP_Y_GT_W( clipped ) ) {
865 result &= ~CLIP_GT_Y; // Y > -W
867 if ( CLIP_Z_LT_W( clipped ) ) {
868 result &= ~CLIP_LT_Z; // Z < W
870 if ( CLIP_Z_GT_W( clipped ) ) {
871 result &= ~CLIP_GT_Z; // Z > -W
876 clipmask_t m4x4_clip_point( const m4x4_t matrix, const vec3_t point, vec4_t clipped ){
877 clipped[0] = point[0];
878 clipped[1] = point[1];
879 clipped[2] = point[2];
881 m4x4_transform_vec4( matrix, clipped );
882 return homogenous_clip_point( clipped );
886 unsigned int homogenous_clip_triangle( vec4_t clipped[9] ){
888 unsigned int rcount = 3;
889 unsigned int wcount = 0;
890 vec_t const* rptr = clipped[0];
891 vec_t* wptr = buffer[0];
894 unsigned char b0, b1;
900 b0 = CLIP_X_LT_W( p0 );
901 for ( i = 0; i < rcount; ++i )
903 p1 = ( i + 1 != rcount ) ? p0 + 4 : rptr;
904 b1 = CLIP_X_LT_W( p1 );
906 wptr[0] = p1[0] - p0[0];
907 wptr[1] = p1[1] - p0[1];
908 wptr[2] = p1[2] - p0[2];
909 wptr[3] = p1[3] - p0[3];
911 scale = ( p0[0] - p0[3] ) / ( wptr[3] - wptr[0] );
913 wptr[0] = (vec_t)( p0[0] + scale * ( wptr[0] ) );
914 wptr[1] = (vec_t)( p0[1] + scale * ( wptr[1] ) );
915 wptr[2] = (vec_t)( p0[2] + scale * ( wptr[2] ) );
916 wptr[3] = (vec_t)( p0[3] + scale * ( wptr[3] ) );
941 b0 = CLIP_X_GT_W( p0 );
943 for ( i = 0; i < rcount; ++i )
945 p1 = ( i + 1 != rcount ) ? p0 + 4 : rptr;
946 b1 = CLIP_X_GT_W( p1 );
948 wptr[0] = p1[0] - p0[0];
949 wptr[1] = p1[1] - p0[1];
950 wptr[2] = p1[2] - p0[2];
951 wptr[3] = p1[3] - p0[3];
953 scale = ( p0[0] + p0[3] ) / ( -wptr[3] - wptr[0] );
955 wptr[0] = (vec_t)( p0[0] + scale * ( wptr[0] ) );
956 wptr[1] = (vec_t)( p0[1] + scale * ( wptr[1] ) );
957 wptr[2] = (vec_t)( p0[2] + scale * ( wptr[2] ) );
958 wptr[3] = (vec_t)( p0[3] + scale * ( wptr[3] ) );
983 b0 = CLIP_Y_LT_W( p0 );
985 for ( i = 0; i < rcount; ++i )
987 p1 = ( i + 1 != rcount ) ? p0 + 4 : rptr;
988 b1 = CLIP_Y_LT_W( p1 );
990 wptr[0] = p1[0] - p0[0];
991 wptr[1] = p1[1] - p0[1];
992 wptr[2] = p1[2] - p0[2];
993 wptr[3] = p1[3] - p0[3];
995 scale = ( p0[1] - p0[3] ) / ( wptr[3] - wptr[1] );
997 wptr[0] = (vec_t)( p0[0] + scale * ( wptr[0] ) );
998 wptr[1] = (vec_t)( p0[1] + scale * ( wptr[1] ) );
999 wptr[2] = (vec_t)( p0[2] + scale * ( wptr[2] ) );
1000 wptr[3] = (vec_t)( p0[3] + scale * ( wptr[3] ) );
1025 b0 = CLIP_Y_GT_W( p0 );
1027 for ( i = 0; i < rcount; ++i )
1029 p1 = ( i + 1 != rcount ) ? p0 + 4 : rptr;
1030 b1 = CLIP_Y_GT_W( p1 );
1032 wptr[0] = p1[0] - p0[0];
1033 wptr[1] = p1[1] - p0[1];
1034 wptr[2] = p1[2] - p0[2];
1035 wptr[3] = p1[3] - p0[3];
1037 scale = ( p0[1] + p0[3] ) / ( -wptr[3] - wptr[1] );
1039 wptr[0] = (vec_t)( p0[0] + scale * ( wptr[0] ) );
1040 wptr[1] = (vec_t)( p0[1] + scale * ( wptr[1] ) );
1041 wptr[2] = (vec_t)( p0[2] + scale * ( wptr[2] ) );
1042 wptr[3] = (vec_t)( p0[3] + scale * ( wptr[3] ) );
1067 b0 = CLIP_Z_LT_W( p0 );
1069 for ( i = 0; i < rcount; ++i )
1071 p1 = ( i + 1 != rcount ) ? p0 + 4 : rptr;
1072 b1 = CLIP_Z_LT_W( p1 );
1074 wptr[0] = p1[0] - p0[0];
1075 wptr[1] = p1[1] - p0[1];
1076 wptr[2] = p1[2] - p0[2];
1077 wptr[3] = p1[3] - p0[3];
1079 scale = ( p0[2] - p0[3] ) / ( wptr[3] - wptr[2] );
1081 wptr[0] = (vec_t)( p0[0] + scale * ( wptr[0] ) );
1082 wptr[1] = (vec_t)( p0[1] + scale * ( wptr[1] ) );
1083 wptr[2] = (vec_t)( p0[2] + scale * ( wptr[2] ) );
1084 wptr[3] = (vec_t)( p0[3] + scale * ( wptr[3] ) );
1109 b0 = CLIP_Z_GT_W( p0 );
1111 for ( i = 0; i < rcount; ++i )
1113 p1 = ( i + 1 != rcount ) ? p0 + 4 : rptr;
1114 b1 = CLIP_Z_GT_W( p1 );
1116 wptr[0] = p1[0] - p0[0];
1117 wptr[1] = p1[1] - p0[1];
1118 wptr[2] = p1[2] - p0[2];
1119 wptr[3] = p1[3] - p0[3];
1121 scale = ( p0[2] + p0[3] ) / ( -wptr[3] - wptr[2] );
1123 wptr[0] = (vec_t)( p0[0] + scale * ( wptr[0] ) );
1124 wptr[1] = (vec_t)( p0[1] + scale * ( wptr[1] ) );
1125 wptr[2] = (vec_t)( p0[2] + scale * ( wptr[2] ) );
1126 wptr[3] = (vec_t)( p0[3] + scale * ( wptr[3] ) );
1149 unsigned int m4x4_clip_triangle( const m4x4_t matrix, const vec3_t p0, const vec3_t p1, const vec3_t p2, vec4_t clipped[9] ){
1150 clipped[0][0] = p0[0];
1151 clipped[0][1] = p0[1];
1152 clipped[0][2] = p0[2];
1154 clipped[1][0] = p1[0];
1155 clipped[1][1] = p1[1];
1156 clipped[1][2] = p1[2];
1158 clipped[2][0] = p2[0];
1159 clipped[2][1] = p2[1];
1160 clipped[2][2] = p2[2];
1163 m4x4_transform_vec4( matrix, clipped[0] );
1164 m4x4_transform_vec4( matrix, clipped[1] );
1165 m4x4_transform_vec4( matrix, clipped[2] );
1167 return homogenous_clip_triangle( clipped );
1170 unsigned int homogenous_clip_line( vec4_t clipped[2] ){
1173 const vec_t* const p0 = clipped[0];
1174 const vec_t* const p1 = clipped[1];
1178 clipmask_t mask0 = homogenous_clip_point( clipped[0] );
1179 clipmask_t mask1 = homogenous_clip_point( clipped[1] );
1181 if ( ( mask0 | mask1 ) == CLIP_PASS ) { // both points passed all planes
1185 if ( mask0 & mask1 ) { // both points failed any one plane
1191 const unsigned int index = CLIP_X_LT_W( p0 );
1192 if ( index ^ CLIP_X_LT_W( p1 ) ) {
1193 clip[0] = p1[0] - p0[0];
1194 clip[1] = p1[1] - p0[1];
1195 clip[2] = p1[2] - p0[2];
1196 clip[3] = p1[3] - p0[3];
1198 scale = ( p0[0] - p0[3] ) / ( clip[3] - clip[0] );
1200 clip[0] = (vec_t)( p0[0] + scale * ( clip[0] ) );
1201 clip[1] = (vec_t)( p0[1] + scale * ( clip[1] ) );
1202 clip[2] = (vec_t)( p0[2] + scale * ( clip[2] ) );
1203 clip[3] = (vec_t)( p0[3] + scale * ( clip[3] ) );
1205 clipped[index][0] = clip[0];
1206 clipped[index][1] = clip[1];
1207 clipped[index][2] = clip[2];
1208 clipped[index][3] = clip[3];
1210 else if ( index == 0 ) {
1216 const unsigned int index = CLIP_X_GT_W( p0 );
1217 if ( index ^ CLIP_X_GT_W( p1 ) ) {
1218 clip[0] = p1[0] - p0[0];
1219 clip[1] = p1[1] - p0[1];
1220 clip[2] = p1[2] - p0[2];
1221 clip[3] = p1[3] - p0[3];
1223 scale = ( p0[0] + p0[3] ) / ( -clip[3] - clip[0] );
1225 clip[0] = (vec_t)( p0[0] + scale * ( clip[0] ) );
1226 clip[1] = (vec_t)( p0[1] + scale * ( clip[1] ) );
1227 clip[2] = (vec_t)( p0[2] + scale * ( clip[2] ) );
1228 clip[3] = (vec_t)( p0[3] + scale * ( clip[3] ) );
1230 clipped[index][0] = clip[0];
1231 clipped[index][1] = clip[1];
1232 clipped[index][2] = clip[2];
1233 clipped[index][3] = clip[3];
1235 else if ( index == 0 ) {
1241 const unsigned int index = CLIP_Y_LT_W( p0 );
1242 if ( index ^ CLIP_Y_LT_W( p1 ) ) {
1243 clip[0] = p1[0] - p0[0];
1244 clip[1] = p1[1] - p0[1];
1245 clip[2] = p1[2] - p0[2];
1246 clip[3] = p1[3] - p0[3];
1248 scale = ( p0[1] - p0[3] ) / ( clip[3] - clip[1] );
1250 clip[0] = (vec_t)( p0[0] + scale * ( clip[0] ) );
1251 clip[1] = (vec_t)( p0[1] + scale * ( clip[1] ) );
1252 clip[2] = (vec_t)( p0[2] + scale * ( clip[2] ) );
1253 clip[3] = (vec_t)( p0[3] + scale * ( clip[3] ) );
1255 clipped[index][0] = clip[0];
1256 clipped[index][1] = clip[1];
1257 clipped[index][2] = clip[2];
1258 clipped[index][3] = clip[3];
1260 else if ( index == 0 ) {
1266 const unsigned int index = CLIP_Y_GT_W( p0 );
1267 if ( index ^ CLIP_Y_GT_W( p1 ) ) {
1268 clip[0] = p1[0] - p0[0];
1269 clip[1] = p1[1] - p0[1];
1270 clip[2] = p1[2] - p0[2];
1271 clip[3] = p1[3] - p0[3];
1273 scale = ( p0[1] + p0[3] ) / ( -clip[3] - clip[1] );
1275 clip[0] = (vec_t)( p0[0] + scale * ( clip[0] ) );
1276 clip[1] = (vec_t)( p0[1] + scale * ( clip[1] ) );
1277 clip[2] = (vec_t)( p0[2] + scale * ( clip[2] ) );
1278 clip[3] = (vec_t)( p0[3] + scale * ( clip[3] ) );
1280 clipped[index][0] = clip[0];
1281 clipped[index][1] = clip[1];
1282 clipped[index][2] = clip[2];
1283 clipped[index][3] = clip[3];
1285 else if ( index == 0 ) {
1291 const unsigned int index = CLIP_Z_LT_W( p0 );
1292 if ( index ^ CLIP_Z_LT_W( p1 ) ) {
1293 clip[0] = p1[0] - p0[0];
1294 clip[1] = p1[1] - p0[1];
1295 clip[2] = p1[2] - p0[2];
1296 clip[3] = p1[3] - p0[3];
1298 scale = ( p0[2] - p0[3] ) / ( clip[3] - clip[2] );
1300 clip[0] = (vec_t)( p0[0] + scale * ( clip[0] ) );
1301 clip[1] = (vec_t)( p0[1] + scale * ( clip[1] ) );
1302 clip[2] = (vec_t)( p0[2] + scale * ( clip[2] ) );
1303 clip[3] = (vec_t)( p0[3] + scale * ( clip[3] ) );
1305 clipped[index][0] = clip[0];
1306 clipped[index][1] = clip[1];
1307 clipped[index][2] = clip[2];
1308 clipped[index][3] = clip[3];
1310 else if ( index == 0 ) {
1316 const unsigned int index = CLIP_Z_GT_W( p0 );
1317 if ( index ^ CLIP_Z_GT_W( p1 ) ) {
1318 clip[0] = p1[0] - p0[0];
1319 clip[1] = p1[1] - p0[1];
1320 clip[2] = p1[2] - p0[2];
1321 clip[3] = p1[3] - p0[3];
1323 scale = ( p0[2] + p0[3] ) / ( -clip[3] - clip[2] );
1325 clip[0] = (vec_t)( p0[0] + scale * ( clip[0] ) );
1326 clip[1] = (vec_t)( p0[1] + scale * ( clip[1] ) );
1327 clip[2] = (vec_t)( p0[2] + scale * ( clip[2] ) );
1328 clip[3] = (vec_t)( p0[3] + scale * ( clip[3] ) );
1330 clipped[index][0] = clip[0];
1331 clipped[index][1] = clip[1];
1332 clipped[index][2] = clip[2];
1333 clipped[index][3] = clip[3];
1335 else if ( index == 0 ) {
1343 unsigned int m4x4_clip_line( const m4x4_t matrix, const vec3_t p0, const vec3_t p1, vec4_t clipped[2] ){
1344 clipped[0][0] = p0[0];
1345 clipped[0][1] = p0[1];
1346 clipped[0][2] = p0[2];
1348 clipped[1][0] = p1[0];
1349 clipped[1][1] = p1[1];
1350 clipped[1][2] = p1[2];
1353 m4x4_transform_vec4( matrix, clipped[0] );
1354 m4x4_transform_vec4( matrix, clipped[1] );
1356 return homogenous_clip_line( clipped );
1359 void m4x4_transpose( m4x4_t matrix ){
1361 float temp, *p1, *p2;
1363 for ( i = 1; i < 4; i++ ) {
1364 for ( j = 0; j < i; j++ ) {
1365 p1 = matrix + ( j * 4 + i );
1366 p2 = matrix + ( i * 4 + j );
1374 /* adapted from Graphics Gems 2
1375 invert a 3d matrix (4x3) */
1376 int m4x4_orthogonal_invert( m4x4_t matrix ){
1380 m4x4_assign( src, matrix );
1382 /* Calculate the determinant of upper left 3x3 submatrix and
1383 * determine if the matrix is singular.
1389 float det = src[0] * src[5] * src[10];
1395 det = src[1] * src[6] * src[8];
1401 det = src[2] * src[4] * src[9];
1407 det = -src[2] * src[5] * src[8];
1413 det = -src[1] * src[4] * src[10];
1419 det = -src[0] * src[6] * src[9];
1428 = ( src[0] * src[5] * src[10] )
1429 + ( src[1] * src[6] * src[8] )
1430 + ( src[2] * src[4] * src[9] )
1431 - ( src[2] * src[5] * src[8] )
1432 - ( src[1] * src[4] * src[10] )
1433 - ( src[0] * src[6] * src[9] );
1436 = src[0] * ( src[5] * src[10] - src[9] * src[6] )
1437 - src[1] * ( src[4] * src[10] - src[8] * src[6] )
1438 + src[2] * ( src[4] * src[9] - src[8] * src[5] );
1442 if ( det * det < 1e-25 ) {
1447 matrix[0] = ( ( src[5] * src[10] - src[6] * src[9] ) * det );
1448 matrix[1] = ( -( src[1] * src[10] - src[2] * src[9] ) * det );
1449 matrix[2] = ( ( src[1] * src[6] - src[2] * src[5] ) * det );
1450 matrix[4] = ( -( src[4] * src[10] - src[6] * src[8] ) * det );
1451 matrix[5] = ( ( src[0] * src[10] - src[2] * src[8] ) * det );
1452 matrix[6] = ( -( src[0] * src[6] - src[2] * src[4] ) * det );
1453 matrix[8] = ( ( src[4] * src[9] - src[5] * src[8] ) * det );
1454 matrix[9] = ( -( src[0] * src[9] - src[1] * src[8] ) * det );
1455 matrix[10] = ( ( src[0] * src[5] - src[1] * src[4] ) * det );
1458 /* Do the translation part */
1459 matrix[12] = -( src[12] * matrix[0] +
1460 src[13] * matrix[4] +
1461 src[14] * matrix[8] );
1462 matrix[13] = -( src[12] * matrix[1] +
1463 src[13] * matrix[5] +
1464 src[14] * matrix[9] );
1465 matrix[14] = -( src[12] * matrix[2] +
1466 src[13] * matrix[6] +
1467 src[14] * matrix[10] );
1472 void quat_identity( vec4_t quat ){
1473 quat[0] = quat[1] = quat[2] = 0;
1477 void quat_multiply_by_quat( vec4_t quat, const vec4_t other ){
1478 const vec_t x = quat[3] * other[0] + quat[0] * other[3] + quat[1] * other[2] - quat[2] * other[1];
1479 const vec_t y = quat[3] * other[1] + quat[1] * other[3] + quat[2] * other[0] - quat[0] * other[2];
1480 const vec_t z = quat[3] * other[2] + quat[2] * other[3] + quat[0] * other[1] - quat[1] * other[0];
1481 const vec_t w = quat[3] * other[3] - quat[0] * other[0] - quat[1] * other[1] - quat[2] * other[2];
1488 void quat_conjugate( vec4_t quat ){
1489 VectorNegate( quat, quat );
1492 //! quaternion from two unit vectors
1493 void quat_for_unit_vectors( vec4_t quat, const vec3_t from, const vec3_t to ){
1494 CrossProduct( from, to, quat );
1495 quat[3] = DotProduct( from, to );
1498 void quat_normalise( vec4_t quat ){
1499 const vec_t n = 1 / ( quat[0] * quat[0] + quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3] );
1506 void quat_for_axisangle( vec4_t quat, const vec3_t axis, double angle ){
1509 quat[3] = (float)sin( angle );
1511 quat[0] = axis[0] * quat[3];
1512 quat[1] = axis[1] * quat[3];
1513 quat[2] = axis[2] * quat[3];
1514 quat[3] = (float)cos( angle );
1517 void m3x3_multiply_by_m3x3( m3x3_t matrix, const m3x3_t matrix_src ){
1518 float *pDest = matrix;
1519 float out1, out2, out3;
1522 for ( i = 0; i < 3; i++ )
1524 out1 = matrix_src[0] * pDest[0];
1525 out1 += matrix_src[1] * pDest[3];
1526 out1 += matrix_src[2] * pDest[6];
1527 out2 = matrix_src[3] * pDest[0];
1528 out2 += matrix_src[4] * pDest[3];
1529 out2 += matrix_src[5] * pDest[6];
1530 out3 = matrix_src[6] * pDest[0];
1531 out3 += matrix_src[7] * pDest[3];
1532 out3 += matrix_src[8] * pDest[6];
1542 void m3x3_transform_vec3( const m3x3_t matrix, vec3_t vector ){
1543 float out1, out2, out3;
1545 out1 = matrix[0] * vector[0];
1546 out1 += matrix[3] * vector[1];
1547 out1 += matrix[6] * vector[2];
1548 out2 = matrix[1] * vector[0];
1549 out2 += matrix[4] * vector[1];
1550 out2 += matrix[7] * vector[2];
1551 out3 = matrix[2] * vector[0];
1552 out3 += matrix[5] * vector[1];
1553 out3 += matrix[8] * vector[2];
1560 float m3_det( m3x3_t mat ){
1563 det = mat[0] * ( mat[4] * mat[8] - mat[7] * mat[5] )
1564 - mat[1] * ( mat[3] * mat[8] - mat[6] * mat[5] )
1565 + mat[2] * ( mat[3] * mat[7] - mat[6] * mat[4] );
1570 int m3_inverse( m3x3_t mr, m3x3_t ma ){
1571 float det = m3_det( ma );
1578 mr[0] = ma[4] * ma[8] - ma[5] * ma[7] / det;
1579 mr[1] = -( ma[1] * ma[8] - ma[7] * ma[2] ) / det;
1580 mr[2] = ma[1] * ma[5] - ma[4] * ma[2] / det;
1582 mr[3] = -( ma[3] * ma[8] - ma[5] * ma[6] ) / det;
1583 mr[4] = ma[0] * ma[8] - ma[6] * ma[2] / det;
1584 mr[5] = -( ma[0] * ma[5] - ma[3] * ma[2] ) / det;
1586 mr[6] = ma[3] * ma[7] - ma[6] * ma[4] / det;
1587 mr[7] = -( ma[0] * ma[7] - ma[6] * ma[1] ) / det;
1588 mr[8] = ma[0] * ma[4] - ma[1] * ma[3] / det;
1593 void m4_submat( m4x4_t mr, m3x3_t mb, int i, int j ){
1594 int ti, tj, idst, jdst;
1596 for ( ti = 0; ti < 4; ti++ )
1601 else if ( ti > i ) {
1608 for ( tj = 0; tj < 4; tj++ )
1613 else if ( tj > j ) {
1620 mb[idst * 3 + jdst] = mr[ti * 4 + tj ];
1625 float m4_det( m4x4_t mr ){
1626 float det, result = 0, i = 1;
1630 for ( n = 0; n < 4; n++, i *= -1 )
1632 m4_submat( mr, msub3, 0, n );
1634 det = m3_det( msub3 );
1635 result += mr[n] * det * i;
1641 int m4x4_invert( m4x4_t matrix ){
1642 float mdet = m4_det( matrix );
1648 if ( fabs( mdet ) < 0.0000000001 ) {
1653 m4x4_assign( m4x4_temp, matrix );
1655 for ( i = 0; i < 4; i++ )
1656 for ( j = 0; j < 4; j++ )
1658 sign = 1 - ( ( i + j ) % 2 ) * 2;
1660 m4_submat( m4x4_temp, mtemp, i, j );
1662 matrix[i + j * 4] = ( m3_det( mtemp ) * sign ) / mdet; /* FIXME: try using * inverse det and see if speed/accuracy are good enough */
1668 void m4x4_solve_ge( m4x4_t matrix, vec4_t x ){
1679 for ( r = 0; r < 4; r++ )
1685 for ( r = 0; r < 4; r++ )
1688 for ( c = 0; c < 4; c++, p++ )
1690 if ( fabs( *p ) > scale[r] ) {
1691 scale[r] = (float)fabs( *p );
1696 for ( c = 0; c < 3; c++ )
1699 for ( r = c; r < 4; r++ )
1701 f = (float)fabs( matrix[( indx[r] << 2 ) + c] ) / scale[indx[r]];
1709 indx[c] = indx[best];
1712 recip = 1 / matrix[( indx[c] << 2 ) + c];
1714 for ( r = c + 1; r < 4; r++ )
1716 p = matrix + ( indx[r] << 2 );
1717 ratio = p[c] * recip;
1719 for ( i = c + 1; i < 4; i++ )
1720 p[i] -= ratio * matrix[( indx[c] << 2 ) + i];
1721 aug[indx[r]] -= ratio * aug[indx[c]];
1725 x[indx[3]] = aug[indx[3]] / matrix[( indx[3] << 2 ) + 3];
1726 for ( r = 2; r >= 0; r-- )
1729 p = matrix + ( indx[r] << 2 );
1731 for ( c = ( r + 1 ); c < 4; c++ )
1733 f -= ( p[c] * x[indx[c]] );
1735 x[indx[r]] = f * recip;
1742 int matrix_solve_ge( vec_t* matrix, vec_t* aug, vec3_t x ){
1752 for ( r = 0; r < N; r++ )
1757 for ( r = 0; r < N; r++ )
1761 for ( c = 0; c < N; c++, p++ )
1763 if ( fabs( *p ) > scale[r] ) {
1764 scale[r] = (float)fabs( *p );
1769 for ( c = 0; c < N; c++ )
1773 for ( r = c; r < N; r++ )
1775 f = (float)fabs( matrix[( indx[r] * N ) + c] ) / scale[indx[r]];
1787 indx[c] = indx[best];
1790 for ( r = c + 1; r < N; r++ )
1792 p = matrix + ( indx[r] * N );
1793 ratio = p[c] / matrix[( indx[c] * N ) + c];
1795 for ( i = c + 1; i < N; i++ ) p[i] -= ratio * matrix[( indx[c] * N ) + i];
1796 aug[indx[r]] -= ratio * aug[indx[c]];
1800 x[N - 1] = aug[indx[N - 1]] / matrix[( indx[N - 1] * N ) + N - 1];
1801 for ( r = 1; r >= 0; r-- )
1804 p = matrix + ( indx[r] * N );
1805 for ( c = ( r + 1 ); c < N; c++ ) f -= ( p[c] * x[c] );
1811 #ifdef YOU_WANT_IT_TO_BORK
1812 /* Gaussian elimination */
1813 for ( i = 0; i < 4; i++ )
1815 for ( j = ( i + 1 ); j < 4; j++ )
1817 ratio = matrix[j][i] / matrix[i][i];
1818 for ( count = i; count < n; count++ ) {
1819 matrix[j][count] -= ( ratio * matrix[i][count] );
1821 b[j] -= ( ratio * b[i] );
1825 /* Back substitution */
1826 x[n - 1] = b[n - 1] / matrix[n - 1][n - 1];
1827 for ( i = ( n - 2 ); i >= 0; i-- )
1830 for ( j = ( i + 1 ); j < n; j++ )
1832 temp -= ( matrix[i][j] * x[j] );
1834 x[i] = temp / matrix[i][i];
1838 int plane_intersect_planes( const vec4_t plane1, const vec4_t plane2, const vec4_t plane3, vec3_t intersection ){
1841 VectorCopy( plane1, planes + 0 );
1843 VectorCopy( plane2, planes + 3 );
1845 VectorCopy( plane3, planes + 6 );
1848 return matrix_solve_ge( planes, b, intersection );