1 /*
2 Copyright (C) 2001-2006, William Joseph.
3 All Rights Reserved.
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
24 const m4x4_t g_m4x4_identity = {
25 1, 0, 0, 0,
26 0, 1, 0, 0,
27 0, 0, 1, 0,
28 0, 0, 0, 1,
29 };
30
m4x4_identity(m4x4_t matrix)31 void m4x4_identity(m4x4_t matrix)
32 {
33 matrix[1] = matrix[2] = matrix[3] =
34 matrix[4] = matrix[6] = matrix[7] =
35 matrix[8] = matrix[9] = matrix[11] =
36 matrix[12] = matrix[13] = matrix[14] = 0;
37
38 matrix[0] = matrix[5] = matrix[10] = matrix[15] = 1;
39 }
40
m4x4_handedness(const m4x4_t matrix)41 m4x4Handedness_t m4x4_handedness(const m4x4_t matrix)
42 {
43 vec3_t cross;
44 CrossProduct(matrix+0, matrix+4, cross);
45 return (DotProduct(matrix+8, cross) < 0) ? eLeftHanded : eRightHanded;
46 }
47
m4x4_assign(m4x4_t matrix,const m4x4_t other)48 void m4x4_assign(m4x4_t matrix, const m4x4_t other)
49 {
50 M4X4_COPY(matrix, other);
51 }
52
m4x4_translation_for_vec3(m4x4_t matrix,const vec3_t translation)53 void m4x4_translation_for_vec3(m4x4_t matrix, const vec3_t translation)
54 {
55 matrix[1] = matrix[2] = matrix[3] =
56 matrix[4] = matrix[6] = matrix[7] =
57 matrix[8] = matrix[9] = matrix[11] = 0;
58
59 matrix[0] = matrix[5] = matrix[10] = matrix[15] = 1;
60
61 matrix[12] = translation[0];
62 matrix[13] = translation[1];
63 matrix[14] = translation[2];
64 }
65
66 /*
67 clockwise rotation around X, Y, Z, facing along axis
68 1 0 0 cy 0 sy cz sz 0
69 0 cx sx 0 1 0 -sz cz 0
70 0 -sx cx -sy 0 cy 0 0 1
71
72 rows of Z by cols of Y
73 cy*cz -sy*cz+sz -sy*sz+cz
74 -sz*cy -sz*sy+cz
75
76 .. or something like that..
77
78 final rotation is Z * Y * X
79 cy*cz -sx*-sy*cz+cx*sz cx*-sy*sz+sx*cz
80 -cy*sz sx*sy*sz+cx*cz -cx*-sy*sz+sx*cz
81 sy -sx*cy cx*cy
82 */
83
84 /* transposed
85 | cy.cz + 0.sz + sy.0 cy.-sz + 0 .cz + sy.0 cy.0 + 0 .0 + sy.1 |
86 | sx.sy.cz + cx.sz + -sx.cy.0 sx.sy.-sz + cx.cz + -sx.cy.0 sx.sy.0 + cx.0 + -sx.cy.1 |
87 | -cx.sy.cz + sx.sz + cx.cy.0 -cx.sy.-sz + sx.cz + cx.cy.0 -cx.sy.0 + 0 .0 + cx.cy.1 |
88 */
m4x4_rotation_for_vec3(m4x4_t matrix,const vec3_t euler,eulerOrder_t order)89 void m4x4_rotation_for_vec3(m4x4_t matrix, const vec3_t euler, eulerOrder_t order)
90 {
91 double cx, sx, cy, sy, cz, sz;
92
93 cx = cos(DEG2RAD(euler[0]));
94 sx = sin(DEG2RAD(euler[0]));
95 cy = cos(DEG2RAD(euler[1]));
96 sy = sin(DEG2RAD(euler[1]));
97 cz = cos(DEG2RAD(euler[2]));
98 sz = sin(DEG2RAD(euler[2]));
99
100 switch(order)
101 {
102 case eXYZ:
103
104 #if 1
105
106 {
107 matrix[0] = (vec_t)(cy*cz);
108 matrix[1] = (vec_t)(cy*sz);
109 matrix[2] = (vec_t)-sy;
110 matrix[4] = (vec_t)(sx*sy*cz + cx*-sz);
111 matrix[5] = (vec_t)(sx*sy*sz + cx*cz);
112 matrix[6] = (vec_t)(sx*cy);
113 matrix[8] = (vec_t)(cx*sy*cz + sx*sz);
114 matrix[9] = (vec_t)(cx*sy*sz + -sx*cz);
115 matrix[10] = (vec_t)(cx*cy);
116 }
117
118 matrix[12] = matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0;
119 matrix[15] = 1;
120
121 #else
122
123 m4x4_identity(matrix);
124 matrix[5] =(vec_t) cx; matrix[6] =(vec_t) sx;
125 matrix[9] =(vec_t)-sx; matrix[10]=(vec_t) cx;
126
127 {
128 m4x4_t temp;
129 m4x4_identity(temp);
130 temp[0] =(vec_t) cy; temp[2] =(vec_t)-sy;
131 temp[8] =(vec_t) sy; temp[10]=(vec_t) cy;
132 m4x4_premultiply_by_m4x4(matrix, temp);
133 m4x4_identity(temp);
134 temp[0] =(vec_t) cz; temp[1] =(vec_t) sz;
135 temp[4] =(vec_t)-sz; temp[5] =(vec_t) cz;
136 m4x4_premultiply_by_m4x4(matrix, temp);
137 }
138 #endif
139
140 break;
141
142 case eYZX:
143 m4x4_identity(matrix);
144 matrix[0] =(vec_t) cy; matrix[2] =(vec_t)-sy;
145 matrix[8] =(vec_t) sy; matrix[10]=(vec_t) cy;
146
147 {
148 m4x4_t temp;
149 m4x4_identity(temp);
150 temp[5] =(vec_t) cx; temp[6] =(vec_t) sx;
151 temp[9] =(vec_t)-sx; temp[10]=(vec_t) cx;
152 m4x4_premultiply_by_m4x4(matrix, temp);
153 m4x4_identity(temp);
154 temp[0] =(vec_t) cz; temp[1] =(vec_t) sz;
155 temp[4] =(vec_t)-sz; temp[5] =(vec_t) cz;
156 m4x4_premultiply_by_m4x4(matrix, temp);
157 }
158 break;
159
160 case eZXY:
161 m4x4_identity(matrix);
162 matrix[0] =(vec_t) cz; matrix[1] =(vec_t) sz;
163 matrix[4] =(vec_t)-sz; matrix[5] =(vec_t) cz;
164
165 {
166 m4x4_t temp;
167 m4x4_identity(temp);
168 temp[5] =(vec_t) cx; temp[6] =(vec_t) sx;
169 temp[9] =(vec_t)-sx; temp[10]=(vec_t) cx;
170 m4x4_premultiply_by_m4x4(matrix, temp);
171 m4x4_identity(temp);
172 temp[0] =(vec_t) cy; temp[2] =(vec_t)-sy;
173 temp[8] =(vec_t) sy; temp[10]=(vec_t) cy;
174 m4x4_premultiply_by_m4x4(matrix, temp);
175 }
176 break;
177
178 case eXZY:
179 m4x4_identity(matrix);
180 matrix[5] =(vec_t) cx; matrix[6] =(vec_t) sx;
181 matrix[9] =(vec_t)-sx; matrix[10]=(vec_t) cx;
182
183 {
184 m4x4_t temp;
185 m4x4_identity(temp);
186 temp[0] =(vec_t) cz; temp[1] =(vec_t) sz;
187 temp[4] =(vec_t)-sz; temp[5] =(vec_t) cz;
188 m4x4_premultiply_by_m4x4(matrix, temp);
189 m4x4_identity(temp);
190 temp[0] =(vec_t) cy; temp[2] =(vec_t)-sy;
191 temp[8] =(vec_t) sy; temp[10]=(vec_t) cy;
192 m4x4_premultiply_by_m4x4(matrix, temp);
193 }
194 break;
195
196 case eYXZ:
197
198 /* transposed
199 | cy.cz + sx.sy.-sz + -cx.sy.0 0.cz + cx.-sz + sx.0 sy.cz + -sx.cy.-sz + cx.cy.0 |
200 | cy.sz + sx.sy.cz + -cx.sy.0 0.sz + cx.cz + sx.0 sy.sz + -sx.cy.cz + cx.cy.0 |
201 | cy.0 + sx.sy.0 + -cx.sy.1 0.0 + cx.0 + sx.1 sy.0 + -sx.cy.0 + cx.cy.1 |
202 */
203
204 #if 1
205
206 {
207 matrix[0] = (vec_t)(cy*cz + sx*sy*-sz);
208 matrix[1] = (vec_t)(cy*sz + sx*sy*cz);
209 matrix[2] = (vec_t)(-cx*sy);
210 matrix[4] = (vec_t)(cx*-sz);
211 matrix[5] = (vec_t)(cx*cz);
212 matrix[6] = (vec_t)(sx);
213 matrix[8] = (vec_t)(sy*cz + -sx*cy*-sz);
214 matrix[9] = (vec_t)(sy*sz + -sx*cy*cz);
215 matrix[10] = (vec_t)(cx*cy);
216 }
217
218 matrix[12] = matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0;
219 matrix[15] = 1;
220
221 #else
222
223 m4x4_identity(matrix);
224 matrix[0] =(vec_t) cy; matrix[2] =(vec_t)-sy;
225 matrix[8] =(vec_t) sy; matrix[10]=(vec_t) cy;
226
227 {
228 m4x4_t 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 m4x4_identity(temp);
234 temp[0] =(vec_t) cz; temp[1] =(vec_t) sz;
235 temp[4] =(vec_t)-sz; temp[5] =(vec_t) cz;
236 m4x4_premultiply_by_m4x4(matrix, temp);
237 }
238 #endif
239 break;
240
241 case eZYX:
242 #if 1
243
244 {
245 matrix[0] = (vec_t)(cy*cz);
246 matrix[4] = (vec_t)(cy*-sz);
247 matrix[8] = (vec_t)sy;
248 matrix[1] = (vec_t)(sx*sy*cz + cx*sz);
249 matrix[5] = (vec_t)(sx*sy*-sz + cx*cz);
250 matrix[9] = (vec_t)(-sx*cy);
251 matrix[2] = (vec_t)(cx*-sy*cz + sx*sz);
252 matrix[6] = (vec_t)(cx*-sy*-sz + sx*cz);
253 matrix[10] = (vec_t)(cx*cy);
254 }
255
256 matrix[12] = matrix[13] = matrix[14] = matrix[3] = matrix[7] = matrix[11] = 0;
257 matrix[15] = 1;
258
259 #else
260
261 m4x4_identity(matrix);
262 matrix[0] =(vec_t) cz; matrix[1] =(vec_t) sz;
263 matrix[4] =(vec_t)-sz; matrix[5] =(vec_t) cz;
264 {
265 m4x4_t temp;
266 m4x4_identity(temp);
267 temp[0] =(vec_t) cy; temp[2] =(vec_t)-sy;
268 temp[8] =(vec_t) sy; temp[10]=(vec_t) cy;
269 m4x4_premultiply_by_m4x4(matrix, temp);
270 m4x4_identity(temp);
271 temp[5] =(vec_t) cx; temp[6] =(vec_t) sx;
272 temp[9] =(vec_t)-sx; temp[10]=(vec_t) cx;
273 m4x4_premultiply_by_m4x4(matrix, temp);
274 }
275
276 #endif
277 break;
278
279 }
280 }
281
m4x4_scale_for_vec3(m4x4_t matrix,const vec3_t scale)282 void m4x4_scale_for_vec3(m4x4_t matrix, const vec3_t scale)
283 {
284 matrix[1] = matrix[2] = matrix[3] =
285 matrix[4] = matrix[6] = matrix[7] =
286 matrix[8] = matrix[9] = matrix[11] =
287 matrix[12] = matrix[13] = matrix[14] = 0;
288
289 matrix[15] = 1;
290
291 matrix[0] = scale[0];
292 matrix[5] = scale[1];
293 matrix[10] = scale[2];
294 }
295
m4x4_rotation_for_quat(m4x4_t matrix,const vec4_t quat)296 void m4x4_rotation_for_quat(m4x4_t matrix, const vec4_t quat)
297 {
298 #if 0
299 const double xx = quat[0] * quat[0];
300 const double xy = quat[0] * quat[1];
301 const double xz = quat[0] * quat[2];
302 const double xw = quat[0] * quat[3];
303
304 const double yy = quat[1] * quat[1];
305 const double yz = quat[1] * quat[2];
306 const double yw = quat[1] * quat[3];
307
308 const double zz = quat[2] * quat[2];
309 const double zw = quat[2] * quat[3];
310
311 matrix[0] = 1 - 2 * ( yy + zz );
312 matrix[4] = 2 * ( xy - zw );
313 matrix[8] = 2 * ( xz + yw );
314
315 matrix[1] = 2 * ( xy + zw );
316 matrix[5] = 1 - 2 * ( xx + zz );
317 matrix[9] = 2 * ( yz - xw );
318
319 matrix[2] = 2 * ( xz - yw );
320 matrix[6] = 2 * ( yz + xw );
321 matrix[10] = 1 - 2 * ( xx + yy );
322 #else
323 const double x2 = quat[0] + quat[0];
324 const double y2 = quat[1] + quat[1];
325 const double z2 = quat[2] + quat[2];
326 const double xx = quat[0] * x2;
327 const double xy = quat[0] * y2;
328 const double xz = quat[0] * z2;
329 const double yy = quat[1] * y2;
330 const double yz = quat[1] * z2;
331 const double zz = quat[2] * z2;
332 const double wx = quat[3] * x2;
333 const double wy = quat[3] * y2;
334 const double wz = quat[3] * z2;
335
336 matrix[0] = (vec_t)( 1.0 - (yy + zz) );
337 matrix[4] = (vec_t)(xy - wz);
338 matrix[8] = (vec_t)(xz + wy);
339
340 matrix[1] = (vec_t)(xy + wz);
341 matrix[5] = (vec_t)( 1.0 - (xx + zz) );
342 matrix[9] = (vec_t)(yz - wx);
343
344 matrix[2] = (vec_t)(xz - wy);
345 matrix[6] = (vec_t)(yz + wx);
346 matrix[10] = (vec_t)( 1.0 - (xx + yy) );
347 #endif
348
349 matrix[3] = matrix[7] = matrix[11] = matrix[12] = matrix[13] = matrix[14] = 0;
350 matrix[15] = 1;
351 }
352
m4x4_rotation_for_axisangle(m4x4_t matrix,const vec3_t axis,double angle)353 void m4x4_rotation_for_axisangle(m4x4_t matrix, const vec3_t axis, double angle)
354 {
355 vec4_t quat;
356 quat_for_axisangle(quat, axis, angle);
357 m4x4_rotation_for_quat(matrix, quat);
358 }
359
m4x4_frustum(m4x4_t matrix,vec_t left,vec_t right,vec_t bottom,vec_t top,vec_t nearval,vec_t farval)360 void m4x4_frustum(m4x4_t matrix,
361 vec_t left, vec_t right,
362 vec_t bottom, vec_t top,
363 vec_t nearval, vec_t farval)
364 {
365 matrix[0] = (vec_t)( (2*nearval) / (right-left) );
366 matrix[1] = 0;
367 matrix[2] = 0;
368 matrix[3] = 0;
369
370 matrix[4] = 0;
371 matrix[5] = (vec_t)( (2*nearval) / (top-bottom) );
372 matrix[6] = 0;
373 matrix[7] = 0;
374
375 matrix[8] = (vec_t)( (right+left) / (right-left) );
376 matrix[9] = (vec_t)( (top+bottom) / (top-bottom) );
377 matrix[10] = (vec_t)( -(farval+nearval) / (farval-nearval) );
378 matrix[11] =-1;
379
380 matrix[12] = 0;
381 matrix[13] = 0;
382 matrix[14] = (vec_t)( -(2*farval*nearval) / (farval-nearval) );
383 matrix[15] = 0;
384 }
385
386
m4x4_get_translation_vec3(const m4x4_t matrix,vec3_t translation)387 void m4x4_get_translation_vec3(const m4x4_t matrix, vec3_t translation)
388 {
389 translation[0] = matrix[12];
390 translation[1] = matrix[13];
391 translation[2] = matrix[14];
392 }
393
m4x4_get_rotation_vec3(const m4x4_t matrix,vec3_t euler,eulerOrder_t order)394 void m4x4_get_rotation_vec3(const m4x4_t matrix, vec3_t euler, eulerOrder_t order)
395 {
396 double a, ca;
397
398 switch(order)
399 {
400 case eXYZ:
401 a = asin(-matrix[2]);
402 ca = cos(a);
403 euler[1] = (vec_t)RAD2DEG(a); /* Calculate Y-axis angle */
404
405 if (fabs(ca) > 0.005) /* Gimbal lock? */
406 {
407 /* No, so get Z-axis angle */
408 euler[2] = (vec_t)RAD2DEG(atan2(matrix[1] / ca, matrix[0]/ ca));
409
410 /* Get X-axis angle */
411 euler[0] = (vec_t)RAD2DEG(atan2(matrix[6] / ca, matrix[10] / ca));
412 }
413 else /* Gimbal lock has occurred */
414 {
415 /* Set Z-axis angle to zero */
416 euler[2] = 0;
417
418 /* And calculate X-axis angle */
419 euler[0] = (vec_t)RAD2DEG(atan2(-matrix[9], matrix[5]));
420 }
421 break;
422 case eYZX:
423 /* NOT IMPLEMENTED */
424 break;
425 case eZXY:
426 /* NOT IMPLEMENTED */
427 break;
428 case eXZY:
429 /* NOT IMPLEMENTED */
430 break;
431 case eYXZ:
432 a = asin(matrix[6]);
433 ca = cos(a);
434 euler[0] = (vec_t)RAD2DEG(a); /* Calculate X-axis angle */
435
436 if (fabs(ca) > 0.005) /* Gimbal lock? */
437 {
438 /* No, so get Y-axis angle */
439 euler[1] = (vec_t)RAD2DEG(atan2(-matrix[2] / ca, matrix[10]/ ca));
440
441 /* Get Z-axis angle */
442 euler[2] = (vec_t)RAD2DEG(atan2(-matrix[4] / ca, matrix[5] / ca));
443 }
444 else /* Gimbal lock has occurred */
445 {
446 /* Set Z-axis angle to zero */
447 euler[2] = 0;
448
449 /* And calculate Y-axis angle */
450 euler[1] = (vec_t)RAD2DEG(atan2(matrix[8], matrix[0]));
451 }
452 break;
453 case eZYX:
454 a = asin(matrix[8]);
455 ca = cos(a);
456 euler[1] = (vec_t)RAD2DEG(a); /* Calculate Y-axis angle */
457
458 if (fabs(ca) > 0.005) /* Gimbal lock? */
459 {
460 /* No, so get X-axis angle */
461 euler[0] = (vec_t)RAD2DEG(atan2(-matrix[9] / ca, matrix[10]/ ca));
462
463 /* Get Z-axis angle */
464 euler[2] = (vec_t)RAD2DEG(atan2(-matrix[4] / ca, matrix[0] / ca));
465 }
466 else /* Gimbal lock has occurred */
467 {
468 /* Set X-axis angle to zero */
469 euler[0] = 0;
470
471 /* And calculate Z-axis angle */
472 euler[2] = (vec_t)RAD2DEG(atan2(matrix[1], matrix[5]));
473 }
474 break;
475 }
476
477 /* return only positive angles in [0,360] */
478 if (euler[0] < 0) euler[0] += 360;
479 if (euler[1] < 0) euler[1] += 360;
480 if (euler[2] < 0) euler[2] += 360;
481 }
482
m4x4_get_scale_vec3(const m4x4_t matrix,vec3_t scale)483 void m4x4_get_scale_vec3(const m4x4_t matrix, vec3_t scale)
484 {
485 scale[0] = VectorLength(matrix+0);
486 scale[1] = VectorLength(matrix+4);
487 scale[2] = VectorLength(matrix+8);
488 }
489
m4x4_get_transform_vec3(const m4x4_t matrix,vec3_t translation,vec3_t euler,eulerOrder_t order,vec3_t scale)490 void m4x4_get_transform_vec3(const m4x4_t matrix, vec3_t translation, vec3_t euler, eulerOrder_t order, vec3_t scale)
491 {
492 m4x4_t normalised;
493 m4x4_assign(normalised, matrix);
494 scale[0] = VectorNormalize(normalised+0, normalised+0);
495 scale[1] = VectorNormalize(normalised+4, normalised+4);
496 scale[2] = VectorNormalize(normalised+8, normalised+8);
497 if(m4x4_handedness(normalised) == eLeftHanded)
498 {
499 VectorNegate(normalised+0, normalised+0);
500 VectorNegate(normalised+4, normalised+4);
501 VectorNegate(normalised+8, normalised+8);
502 scale[0] = -scale[0];
503 scale[1] = -scale[1];
504 scale[2] = -scale[2];
505 }
506 m4x4_get_rotation_vec3(normalised, euler, order);
507 m4x4_get_translation_vec3(matrix, translation);
508 }
509
m4x4_translate_by_vec3(m4x4_t matrix,const vec3_t translation)510 void m4x4_translate_by_vec3(m4x4_t matrix, const vec3_t translation)
511 {
512 m4x4_t temp;
513 m4x4_translation_for_vec3(temp, translation);
514 m4x4_multiply_by_m4x4(matrix, temp);
515 }
516
m4x4_rotate_by_vec3(m4x4_t matrix,const vec3_t euler,eulerOrder_t order)517 void m4x4_rotate_by_vec3(m4x4_t matrix, const vec3_t euler, eulerOrder_t order)
518 {
519 m4x4_t temp;
520 m4x4_rotation_for_vec3(temp, euler, order);
521 m4x4_multiply_by_m4x4(matrix, temp);
522 }
523
m4x4_scale_by_vec3(m4x4_t matrix,const vec3_t scale)524 void m4x4_scale_by_vec3(m4x4_t matrix, const vec3_t scale)
525 {
526 m4x4_t temp;
527 m4x4_scale_for_vec3(temp, scale);
528 m4x4_multiply_by_m4x4(matrix, temp);
529 }
530
m4x4_rotate_by_quat(m4x4_t matrix,const vec4_t rotation)531 void m4x4_rotate_by_quat(m4x4_t matrix, const vec4_t rotation)
532 {
533 m4x4_t temp;
534 m4x4_rotation_for_quat(temp, rotation);
535 m4x4_multiply_by_m4x4(matrix, temp);
536 }
537
m4x4_rotate_by_axisangle(m4x4_t matrix,const vec3_t axis,double angle)538 void m4x4_rotate_by_axisangle(m4x4_t matrix, const vec3_t axis, double angle)
539 {
540 m4x4_t temp;
541 m4x4_rotation_for_axisangle(temp, axis, angle);
542 m4x4_multiply_by_m4x4(matrix, temp);
543 }
544
m4x4_transform_by_vec3(m4x4_t matrix,const vec3_t translation,const vec3_t euler,eulerOrder_t order,const vec3_t scale)545 void m4x4_transform_by_vec3(m4x4_t matrix, const vec3_t translation, const vec3_t euler, eulerOrder_t order, const vec3_t scale)
546 {
547 m4x4_translate_by_vec3(matrix, translation);
548 m4x4_rotate_by_vec3(matrix, euler, order);
549 m4x4_scale_by_vec3(matrix, scale);
550 }
551
m4x4_pivoted_rotate_by_vec3(m4x4_t matrix,const vec3_t euler,eulerOrder_t order,const vec3_t pivotpoint)552 void m4x4_pivoted_rotate_by_vec3(m4x4_t matrix, const vec3_t euler, eulerOrder_t order, const vec3_t pivotpoint)
553 {
554 vec3_t vec3_temp;
555 VectorNegate(pivotpoint, vec3_temp);
556
557 m4x4_translate_by_vec3(matrix, pivotpoint);
558 m4x4_rotate_by_vec3(matrix, euler, order);
559 m4x4_translate_by_vec3(matrix, vec3_temp);
560 }
561
m4x4_pivoted_scale_by_vec3(m4x4_t matrix,const vec3_t scale,const vec3_t pivotpoint)562 void m4x4_pivoted_scale_by_vec3(m4x4_t matrix, const vec3_t scale, const vec3_t pivotpoint)
563 {
564 vec3_t vec3_temp;
565 VectorNegate(pivotpoint, vec3_temp);
566
567 m4x4_translate_by_vec3(matrix, pivotpoint);
568 m4x4_scale_by_vec3(matrix, scale);
569 m4x4_translate_by_vec3(matrix, vec3_temp);
570 }
571
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)572 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)
573 {
574 vec3_t vec3_temp;
575
576 VectorAdd(pivotpoint, translation, vec3_temp);
577 m4x4_translate_by_vec3(matrix, vec3_temp);
578 m4x4_rotate_by_vec3(matrix, euler, order);
579 m4x4_scale_by_vec3(matrix, scale);
580 VectorNegate(pivotpoint, vec3_temp);
581 m4x4_translate_by_vec3(matrix, vec3_temp);
582 }
583
m4x4_pivoted_transform_by_rotation(m4x4_t matrix,const vec3_t translation,const m4x4_t rotation,const vec3_t scale,const vec3_t pivotpoint)584 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)
585 {
586 vec3_t vec3_temp;
587
588 VectorAdd(pivotpoint, translation, vec3_temp);
589 m4x4_translate_by_vec3(matrix, vec3_temp);
590 m4x4_multiply_by_m4x4(matrix, rotation);
591 m4x4_scale_by_vec3(matrix, scale);
592 VectorNegate(pivotpoint, vec3_temp);
593 m4x4_translate_by_vec3(matrix, vec3_temp);
594 }
595
m4x4_pivoted_rotate_by_quat(m4x4_t matrix,const vec4_t rotation,const vec3_t pivotpoint)596 void m4x4_pivoted_rotate_by_quat(m4x4_t matrix, const vec4_t rotation, const vec3_t pivotpoint)
597 {
598 vec3_t vec3_temp;
599 VectorNegate(pivotpoint, vec3_temp);
600
601 m4x4_translate_by_vec3(matrix, pivotpoint);
602 m4x4_rotate_by_quat(matrix, rotation);
603 m4x4_translate_by_vec3(matrix, vec3_temp);
604 }
605
m4x4_pivoted_rotate_by_axisangle(m4x4_t matrix,const vec3_t axis,double angle,const vec3_t pivotpoint)606 void m4x4_pivoted_rotate_by_axisangle(m4x4_t matrix, const vec3_t axis, double angle, const vec3_t pivotpoint)
607 {
608 vec3_t vec3_temp;
609 VectorNegate(pivotpoint, vec3_temp);
610
611 m4x4_translate_by_vec3(matrix, pivotpoint);
612 m4x4_rotate_by_axisangle(matrix, axis, angle);
613 m4x4_translate_by_vec3(matrix, vec3_temp);
614 }
615
616 /*
617 A = A.B
618
619 A0 = B0 * A0 + B1 * A4 + B2 * A8 + B3 * A12
620 A4 = B4 * A0 + B5 * A4 + B6 * A8 + B7 * A12
621 A8 = B8 * A0 + B9 * A4 + B10* A8 + B11* A12
622 A12= B12* A0 + B13* A4 + B14* A8 + B15* A12
623
624 A1 = B0 * A1 + B1 * A5 + B2 * A9 + B3 * A13
625 A5 = B4 * A1 + B5 * A5 + B6 * A9 + B7 * A13
626 A9 = B8 * A1 + B9 * A5 + B10* A9 + B11* A13
627 A13= B12* A1 + B13* A5 + B14* A9 + B15* A13
628
629 A2 = B0 * A2 + B1 * A6 + B2 * A10+ B3 * A14
630 A6 = B4 * A2 + B5 * A6 + B6 * A10+ B7 * A14
631 A10= B8 * A2 + B9 * A6 + B10* A10+ B11* A14
632 A14= B12* A2 + B13* A6 + B14* A10+ B15* A14
633
634 A3 = B0 * A3 + B1 * A7 + B2 * A11+ B3 * A15
635 A7 = B4 * A3 + B5 * A7 + B6 * A11+ B7 * A15
636 A11= B8 * A3 + B9 * A7 + B10* A11+ B11* A15
637 A15= B12* A3 + B13* A7 + B14* A11+ B15* A15
638 */
639
m4x4_multiply_by_m4x4(m4x4_t dst,const m4x4_t src)640 void m4x4_multiply_by_m4x4(m4x4_t dst, const m4x4_t src)
641 {
642 vec_t dst0, dst1, dst2, dst3;
643
644 #if 1
645
646 dst0 = src[0] * dst[0] + src[1] * dst[4] + src[2] * dst[8] + src[3] * dst[12];
647 dst1 = src[4] * dst[0] + src[5] * dst[4] + src[6] * dst[8] + src[7] * dst[12];
648 dst2 = src[8] * dst[0] + src[9] * dst[4] + src[10]* dst[8] + src[11]* dst[12];
649 dst3 = src[12]* dst[0] + src[13]* dst[4] + src[14]* dst[8] + src[15]* dst[12];
650 dst[0] = dst0; dst[4] = dst1; dst[8] = dst2; dst[12]= dst3;
651
652 dst0 = src[0] * dst[1] + src[1] * dst[5] + src[2] * dst[9] + src[3] * dst[13];
653 dst1 = src[4] * dst[1] + src[5] * dst[5] + src[6] * dst[9] + src[7] * dst[13];
654 dst2 = src[8] * dst[1] + src[9] * dst[5] + src[10]* dst[9] + src[11]* dst[13];
655 dst3 = src[12]* dst[1] + src[13]* dst[5] + src[14]* dst[9] + src[15]* dst[13];
656 dst[1] = dst0; dst[5] = dst1; dst[9] = dst2; dst[13]= dst3;
657
658 dst0 = src[0] * dst[2] + src[1] * dst[6] + src[2] * dst[10]+ src[3] * dst[14];
659 dst1 = src[4] * dst[2] + src[5] * dst[6] + src[6] * dst[10]+ src[7] * dst[14];
660 dst2 = src[8] * dst[2] + src[9] * dst[6] + src[10]* dst[10]+ src[11]* dst[14];
661 dst3 = src[12]* dst[2] + src[13]* dst[6] + src[14]* dst[10]+ src[15]* dst[14];
662 dst[2] = dst0; dst[6] = dst1; dst[10]= dst2; dst[14]= dst3;
663
664 dst0 = src[0] * dst[3] + src[1] * dst[7] + src[2] * dst[11]+ src[3] * dst[15];
665 dst1 = src[4] * dst[3] + src[5] * dst[7] + src[6] * dst[11]+ src[7] * dst[15];
666 dst2 = src[8] * dst[3] + src[9] * dst[7] + src[10]* dst[11]+ src[11]* dst[15];
667 dst3 = src[12]* dst[3] + src[13]* dst[7] + src[14]* dst[11]+ src[15]* dst[15];
668 dst[3] = dst0; dst[7] = dst1; dst[11]= dst2; dst[15]= dst3;
669
670 #else
671
672 vec_t * p = dst;
673 for(int i=0;i<4;i++)
674 {
675 dst1 = src[0] * p[0];
676 dst1 += src[1] * p[4];
677 dst1 += src[2] * p[8];
678 dst1 += src[3] * p[12];
679 dst2 = src[4] * p[0];
680 dst2 += src[5] * p[4];
681 dst2 += src[6] * p[8];
682 dst2 += src[7] * p[12];
683 dst3 = src[8] * p[0];
684 dst3 += src[9] * p[4];
685 dst3 += src[10] * p[8];
686 dst3 += src[11] * p[12];
687 dst4 = src[12] * p[0];
688 dst4 += src[13] * p[4];
689 dst4 += src[14] * p[8];
690 dst4 += src[15] * p[12];
691
692 p[0] = dst1;
693 p[4] = dst2;
694 p[8] = dst3;
695 p[12] = dst4;
696 p++;
697 }
698
699 #endif
700 }
701
702 /*
703 A = B.A
704
705 A0 = A0 * B0 + A1 * B4 + A2 * B8 + A3 * B12
706 A1 = A0 * B1 + A1 * B5 + A2 * B9 + A3 * B13
707 A2 = A0 * B2 + A1 * B6 + A2 * B10+ A3 * B14
708 A3 = A0 * B3 + A1 * B7 + A2 * B11+ A3 * B15
709
710 A4 = A4 * B0 + A5 * B4 + A6 * B8 + A7 * B12
711 A5 = A4 * B1 + A5 * B5 + A6 * B9 + A7 * B13
712 A6 = A4 * B2 + A5 * B6 + A6 * B10+ A7 * B14
713 A7 = A4 * B3 + A5 * B7 + A6 * B11+ A7 * B15
714
715 A8 = A8 * B0 + A9 * B4 + A10* B8 + A11* B12
716 A9 = A8 * B1 + A9 * B5 + A10* B9 + A11* B13
717 A10= A8 * B2 + A9 * B6 + A10* B10+ A11* B14
718 A11= A8 * B3 + A9 * B7 + A10* B11+ A11* B15
719
720 A12= A12* B0 + A13* B4 + A14* B8 + A15* B12
721 A13= A12* B1 + A13* B5 + A14* B9 + A15* B13
722 A14= A12* B2 + A13* B6 + A14* B10+ A15* B14
723 A15= A12* B3 + A13* B7 + A14* B11+ A15* B15
724 */
725
m4x4_premultiply_by_m4x4(m4x4_t dst,const m4x4_t src)726 void m4x4_premultiply_by_m4x4(m4x4_t dst, const m4x4_t src)
727 {
728 vec_t dst0, dst1, dst2, dst3;
729
730 #if 1
731
732 dst0 = dst[0] * src[0] + dst[1] * src[4] + dst[2] * src[8] + dst[3] * src[12];
733 dst1 = dst[0] * src[1] + dst[1] * src[5] + dst[2] * src[9] + dst[3] * src[13];
734 dst2 = dst[0] * src[2] + dst[1] * src[6] + dst[2] * src[10]+ dst[3] * src[14];
735 dst3 = dst[0] * src[3] + dst[1] * src[7] + dst[2] * src[11]+ dst[3] * src[15];
736 dst[0] = dst0; dst[1] = dst1; dst[2] = dst2; dst[3]= dst3;
737
738 dst0 = dst[4] * src[0] + dst[5] * src[4] + dst[6] * src[8] + dst[7] * src[12];
739 dst1 = dst[4] * src[1] + dst[5] * src[5] + dst[6] * src[9] + dst[7] * src[13];
740 dst2 = dst[4] * src[2] + dst[5] * src[6] + dst[6] * src[10]+ dst[7] * src[14];
741 dst3 = dst[4] * src[3] + dst[5] * src[7] + dst[6] * src[11]+ dst[7] * src[15];
742 dst[4] = dst0; dst[5] = dst1; dst[6] = dst2; dst[7]= dst3;
743
744 dst0 = dst[8] * src[0] + dst[9] * src[4] + dst[10]* src[8] + dst[11]* src[12];
745 dst1 = dst[8] * src[1] + dst[9] * src[5] + dst[10]* src[9] + dst[11]* src[13];
746 dst2 = dst[8] * src[2] + dst[9] * src[6] + dst[10]* src[10]+ dst[11]* src[14];
747 dst3 = dst[8] * src[3] + dst[9] * src[7] + dst[10]* src[11]+ dst[11]* src[15];
748 dst[8] = dst0; dst[9] = dst1; dst[10] = dst2; dst[11]= dst3;
749
750 dst0 = dst[12]* src[0] + dst[13]* src[4] + dst[14]* src[8] + dst[15]* src[12];
751 dst1 = dst[12]* src[1] + dst[13]* src[5] + dst[14]* src[9] + dst[15]* src[13];
752 dst2 = dst[12]* src[2] + dst[13]* src[6] + dst[14]* src[10]+ dst[15]* src[14];
753 dst3 = dst[12]* src[3] + dst[13]* src[7] + dst[14]* src[11]+ dst[15]* src[15];
754 dst[12] = dst0; dst[13] = dst1; dst[14] = dst2; dst[15]= dst3;
755
756 #else
757
758 vec_t* p = dst;
759 for(int i=0;i<4;i++)
760 {
761 dst1 = src[0] * p[0];
762 dst2 = src[1] * p[0];
763 dst3 = src[2] * p[0];
764 dst4 = src[3] * p[0];
765 dst1 += src[4] * p[1];
766 dst2 += src[5] * p[1];
767 dst3 += src[6] * p[1];
768 dst4 += src[7] * p[1];
769 dst1 += src[8] * p[2];
770 dst2 += src[9] * p[2];
771 dst4 += src[11] * p[2];
772 dst3 += src[10] * p[2];
773 dst1 += src[12] * p[3];
774 dst2 += src[13] * p[3];
775 dst3 += src[14] * p[3];
776 dst4 += src[15] * p[3];
777
778 *p++ = dst1;
779 *p++ = dst2;
780 *p++ = dst3;
781 *p++ = dst4;
782 }
783
784 #endif
785 }
786
m4x4_orthogonal_multiply_by_m4x4(m4x4_t dst,const m4x4_t src)787 void m4x4_orthogonal_multiply_by_m4x4(m4x4_t dst, const m4x4_t src)
788 {
789 vec_t dst0, dst1, dst2, dst3;
790
791 dst0 = src[0] * dst[0] + src[1] * dst[4] + src[2] * dst[8];
792 dst1 = src[4] * dst[0] + src[5] * dst[4] + src[6] * dst[8];
793 dst2 = src[8] * dst[0] + src[9] * dst[4] + src[10]* dst[8];
794 dst3 = src[12]* dst[0] + src[13]* dst[4] + src[14]* dst[8] + dst[12];
795 dst[0] = dst0; dst[4] = dst1; dst[8] = dst2; dst[12]= dst3;
796
797 dst0 = src[0] * dst[1] + src[1] * dst[5] + src[2] * dst[9];
798 dst1 = src[4] * dst[1] + src[5] * dst[5] + src[6] * dst[9];
799 dst2 = src[8] * dst[1] + src[9] * dst[5] + src[10]* dst[9];
800 dst3 = src[12]* dst[1] + src[13]* dst[5] + src[14]* dst[9] + dst[13];
801 dst[1] = dst0; dst[5] = dst1; dst[9] = dst2; dst[13]= dst3;
802
803 dst0 = src[0] * dst[2] + src[1] * dst[6] + src[2] * dst[10];
804 dst1 = src[4] * dst[2] + src[5] * dst[6] + src[6] * dst[10];
805 dst2 = src[8] * dst[2] + src[9] * dst[6] + src[10]* dst[10];
806 dst3 = src[12]* dst[2] + src[13]* dst[6] + src[14]* dst[10]+ dst[14];
807 dst[2] = dst0; dst[6] = dst1; dst[10]= dst2; dst[14]= dst3;
808 }
809
m4x4_orthogonal_premultiply_by_m4x4(m4x4_t dst,const m4x4_t src)810 void m4x4_orthogonal_premultiply_by_m4x4(m4x4_t dst, const m4x4_t src)
811 {
812 vec_t dst0, dst1, dst2;
813
814 dst0 = dst[0] * src[0] + dst[1] * src[4] + dst[2] * src[8];
815 dst1 = dst[0] * src[1] + dst[1] * src[5] + dst[2] * src[9];
816 dst2 = dst[0] * src[2] + dst[1] * src[6] + dst[2] * src[10];
817 dst[0] = dst0; dst[1] = dst1; dst[2] = dst2;
818
819 dst0 = dst[4] * src[0] + dst[5] * src[4] + dst[6] * src[8];
820 dst1 = dst[4] * src[1] + dst[5] * src[5] + dst[6] * src[9];
821 dst2 = dst[4] * src[2] + dst[5] * src[6] + dst[6] * src[10];
822 dst[4] = dst0; dst[5] = dst1; dst[6] = dst2;
823
824 dst0 = dst[8] * src[0] + dst[9] * src[4] + dst[10]* src[8];
825 dst1 = dst[8] * src[1] + dst[9] * src[5] + dst[10]* src[9];
826 dst2 = dst[8] * src[2] + dst[9] * src[6] + dst[10]* src[10];
827 dst[8] = dst0; dst[9] = dst1; dst[10] = dst2;
828
829 dst0 = dst[12]* src[0] + dst[13]* src[4] + dst[14]* src[8] + dst[15]* src[12];
830 dst1 = dst[12]* src[1] + dst[13]* src[5] + dst[14]* src[9] + dst[15]* src[13];
831 dst2 = dst[12]* src[2] + dst[13]* src[6] + dst[14]* src[10]+ dst[15]* src[14];
832 dst[12] = dst0; dst[13] = dst1; dst[14] = dst2;
833 }
834
m4x4_transform_point(const m4x4_t matrix,vec3_t point)835 void m4x4_transform_point(const m4x4_t matrix, vec3_t point)
836 {
837 float out1, out2, out3;
838
839 out1 = matrix[0] * point[0] + matrix[4] * point[1] + matrix[8] * point[2] + matrix[12];
840 out2 = matrix[1] * point[0] + matrix[5] * point[1] + matrix[9] * point[2] + matrix[13];
841 out3 = matrix[2] * point[0] + matrix[6] * point[1] + matrix[10] * point[2] + matrix[14];
842
843 point[0] = out1;
844 point[1] = out2;
845 point[2] = out3;
846 }
847
m4x4_transform_normal(const m4x4_t matrix,vec3_t normal)848 void m4x4_transform_normal(const m4x4_t matrix, vec3_t normal)
849 {
850 float out1, out2, out3;
851
852 out1 = matrix[0] * normal[0] + matrix[4] * normal[1] + matrix[8] * normal[2];
853 out2 = matrix[1] * normal[0] + matrix[5] * normal[1] + matrix[9] * normal[2];
854 out3 = matrix[2] * normal[0] + matrix[6] * normal[1] + matrix[10] * normal[2];
855
856 normal[0] = out1;
857 normal[1] = out2;
858 normal[2] = out3;
859 }
860
m4x4_transform_vec4(const m4x4_t matrix,vec4_t vector)861 void m4x4_transform_vec4(const m4x4_t matrix, vec4_t vector)
862 {
863 float out1, out2, out3, out4;
864
865 out1 = matrix[0] * vector[0] + matrix[4] * vector[1] + matrix[8] * vector[2] + matrix[12] * vector[3];
866 out2 = matrix[1] * vector[0] + matrix[5] * vector[1] + matrix[9] * vector[2] + matrix[13] * vector[3];
867 out3 = matrix[2] * vector[0] + matrix[6] * vector[1] + matrix[10] * vector[2] + matrix[14] * vector[3];
868 out4 = matrix[3] * vector[0] + matrix[7] * vector[1] + matrix[11] * vector[2] + matrix[15] * vector[3];
869
870 vector[0] = out1;
871 vector[1] = out2;
872 vector[2] = out3;
873 vector[3] = out4;
874 }
875
876 #define CLIP_X_LT_W(p) ((p)[0] < (p)[3])
877 #define CLIP_X_GT_W(p) ((p)[0] > -(p)[3])
878 #define CLIP_Y_LT_W(p) ((p)[1] < (p)[3])
879 #define CLIP_Y_GT_W(p) ((p)[1] > -(p)[3])
880 #define CLIP_Z_LT_W(p) ((p)[2] < (p)[3])
881 #define CLIP_Z_GT_W(p) ((p)[2] > -(p)[3])
882
homogenous_clip_point(const vec4_t clipped)883 clipmask_t homogenous_clip_point(const vec4_t clipped)
884 {
885 clipmask_t result = CLIP_FAIL;
886 if(CLIP_X_LT_W(clipped)) result &= ~CLIP_LT_X; // X < W
887 if(CLIP_X_GT_W(clipped)) result &= ~CLIP_GT_X; // X > -W
888 if(CLIP_Y_LT_W(clipped)) result &= ~CLIP_LT_Y; // Y < W
889 if(CLIP_Y_GT_W(clipped)) result &= ~CLIP_GT_Y; // Y > -W
890 if(CLIP_Z_LT_W(clipped)) result &= ~CLIP_LT_Z; // Z < W
891 if(CLIP_Z_GT_W(clipped)) result &= ~CLIP_GT_Z; // Z > -W
892 return result;
893 }
894
m4x4_clip_point(const m4x4_t matrix,const vec3_t point,vec4_t clipped)895 clipmask_t m4x4_clip_point(const m4x4_t matrix, const vec3_t point, vec4_t clipped)
896 {
897 clipped[0] = point[0];
898 clipped[1] = point[1];
899 clipped[2] = point[2];
900 clipped[3] = 1;
901 m4x4_transform_vec4(matrix, clipped);
902 return homogenous_clip_point(clipped);
903 }
904
905
homogenous_clip_triangle(vec4_t clipped[9])906 unsigned int homogenous_clip_triangle(vec4_t clipped[9])
907 {
908 vec4_t buffer[9];
909 unsigned int rcount = 3;
910 unsigned int wcount = 0;
911 vec_t const* rptr = clipped[0];
912 vec_t* wptr = buffer[0];
913 const vec_t* p0;
914 const vec_t* p1;
915 unsigned char b0, b1;
916
917 unsigned int i;
918 double scale;
919
920 p0 = rptr;
921 b0 = CLIP_X_LT_W(p0);
922 for(i=0; i<rcount; ++i)
923 {
924 p1 = (i+1 != rcount) ? p0 + 4 : rptr;
925 b1 = CLIP_X_LT_W(p1);
926 if(b0 ^ b1)
927 {
928 wptr[0] = p1[0] - p0[0];
929 wptr[1] = p1[1] - p0[1];
930 wptr[2] = p1[2] - p0[2];
931 wptr[3] = p1[3] - p0[3];
932
933 scale = (p0[0] - p0[3]) / (wptr[3] - wptr[0]);
934
935 wptr[0] = (vec_t)(p0[0] + scale*(wptr[0]));
936 wptr[1] = (vec_t)(p0[1] + scale*(wptr[1]));
937 wptr[2] = (vec_t)(p0[2] + scale*(wptr[2]));
938 wptr[3] = (vec_t)(p0[3] + scale*(wptr[3]));
939
940 wptr += 4;
941 ++wcount;
942 }
943
944 if(b1)
945 {
946 wptr[0] = p1[0];
947 wptr[1] = p1[1];
948 wptr[2] = p1[2];
949 wptr[3] = p1[3];
950
951 wptr += 4;
952 ++wcount;
953 }
954
955 p0 = p1;
956 b0 = b1;
957 }
958
959 rcount = wcount;
960 wcount = 0;
961 rptr = buffer[0];
962 wptr = clipped[0];
963 p0 = rptr;
964 b0 = CLIP_X_GT_W(p0);
965
966 for(i=0; i<rcount; ++i)
967 {
968 p1 = (i+1 != rcount) ? p0 + 4 : rptr;
969 b1 = CLIP_X_GT_W(p1);
970 if(b0 ^ b1)
971 {
972 wptr[0] = p1[0] - p0[0];
973 wptr[1] = p1[1] - p0[1];
974 wptr[2] = p1[2] - p0[2];
975 wptr[3] = p1[3] - p0[3];
976
977 scale = (p0[0] + p0[3]) / (-wptr[3] - wptr[0]);
978
979 wptr[0] = (vec_t)(p0[0] + scale*(wptr[0]));
980 wptr[1] = (vec_t)(p0[1] + scale*(wptr[1]));
981 wptr[2] = (vec_t)(p0[2] + scale*(wptr[2]));
982 wptr[3] = (vec_t)(p0[3] + scale*(wptr[3]));
983
984 wptr += 4;
985 ++wcount;
986 }
987
988 if(b1)
989 {
990 wptr[0] = p1[0];
991 wptr[1] = p1[1];
992 wptr[2] = p1[2];
993 wptr[3] = p1[3];
994
995 wptr += 4;
996 ++wcount;
997 }
998
999 p0 = p1;
1000 b0 = b1;
1001 }
1002
1003 rcount = wcount;
1004 wcount = 0;
1005 rptr = clipped[0];
1006 wptr = buffer[0];
1007 p0 = rptr;
1008 b0 = CLIP_Y_LT_W(p0);
1009
1010 for(i=0; i<rcount; ++i)
1011 {
1012 p1 = (i+1 != rcount) ? p0 + 4 : rptr;
1013 b1 = CLIP_Y_LT_W(p1);
1014 if(b0 ^ b1)
1015 {
1016 wptr[0] = p1[0] - p0[0];
1017 wptr[1] = p1[1] - p0[1];
1018 wptr[2] = p1[2] - p0[2];
1019 wptr[3] = p1[3] - p0[3];
1020
1021 scale = (p0[1] - p0[3]) / (wptr[3] - wptr[1]);
1022
1023 wptr[0] = (vec_t)(p0[0] + scale*(wptr[0]));
1024 wptr[1] = (vec_t)(p0[1] + scale*(wptr[1]));
1025 wptr[2] = (vec_t)(p0[2] + scale*(wptr[2]));
1026 wptr[3] = (vec_t)(p0[3] + scale*(wptr[3]));
1027
1028 wptr += 4;
1029 ++wcount;
1030 }
1031
1032 if(b1)
1033 {
1034 wptr[0] = p1[0];
1035 wptr[1] = p1[1];
1036 wptr[2] = p1[2];
1037 wptr[3] = p1[3];
1038
1039 wptr += 4;
1040 ++wcount;
1041 }
1042
1043 p0 = p1;
1044 b0 = b1;
1045 }
1046
1047 rcount = wcount;
1048 wcount = 0;
1049 rptr = buffer[0];
1050 wptr = clipped[0];
1051 p0 = rptr;
1052 b0 = CLIP_Y_GT_W(p0);
1053
1054 for(i=0; i<rcount; ++i)
1055 {
1056 p1 = (i+1 != rcount) ? p0 + 4 : rptr;
1057 b1 = CLIP_Y_GT_W(p1);
1058 if(b0 ^ b1)
1059 {
1060 wptr[0] = p1[0] - p0[0];
1061 wptr[1] = p1[1] - p0[1];
1062 wptr[2] = p1[2] - p0[2];
1063 wptr[3] = p1[3] - p0[3];
1064
1065 scale = (p0[1] + p0[3]) / (-wptr[3] - wptr[1]);
1066
1067 wptr[0] = (vec_t)(p0[0] + scale*(wptr[0]));
1068 wptr[1] = (vec_t)(p0[1] + scale*(wptr[1]));
1069 wptr[2] = (vec_t)(p0[2] + scale*(wptr[2]));
1070 wptr[3] = (vec_t)(p0[3] + scale*(wptr[3]));
1071
1072 wptr += 4;
1073 ++wcount;
1074 }
1075
1076 if(b1)
1077 {
1078 wptr[0] = p1[0];
1079 wptr[1] = p1[1];
1080 wptr[2] = p1[2];
1081 wptr[3] = p1[3];
1082
1083 wptr += 4;
1084 ++wcount;
1085 }
1086
1087 p0 = p1;
1088 b0 = b1;
1089 }
1090
1091 rcount = wcount;
1092 wcount = 0;
1093 rptr = clipped[0];
1094 wptr = buffer[0];
1095 p0 = rptr;
1096 b0 = CLIP_Z_LT_W(p0);
1097
1098 for(i=0; i<rcount; ++i)
1099 {
1100 p1 = (i+1 != rcount) ? p0 + 4 : rptr;
1101 b1 = CLIP_Z_LT_W(p1);
1102 if(b0 ^ b1)
1103 {
1104 wptr[0] = p1[0] - p0[0];
1105 wptr[1] = p1[1] - p0[1];
1106 wptr[2] = p1[2] - p0[2];
1107 wptr[3] = p1[3] - p0[3];
1108
1109 scale = (p0[2] - p0[3]) / (wptr[3] - wptr[2]);
1110
1111 wptr[0] = (vec_t)(p0[0] + scale*(wptr[0]));
1112 wptr[1] = (vec_t)(p0[1] + scale*(wptr[1]));
1113 wptr[2] = (vec_t)(p0[2] + scale*(wptr[2]));
1114 wptr[3] = (vec_t)(p0[3] + scale*(wptr[3]));
1115
1116 wptr += 4;
1117 ++wcount;
1118 }
1119
1120 if(b1)
1121 {
1122 wptr[0] = p1[0];
1123 wptr[1] = p1[1];
1124 wptr[2] = p1[2];
1125 wptr[3] = p1[3];
1126
1127 wptr += 4;
1128 ++wcount;
1129 }
1130
1131 p0 = p1;
1132 b0 = b1;
1133 }
1134
1135 rcount = wcount;
1136 wcount = 0;
1137 rptr = buffer[0];
1138 wptr = clipped[0];
1139 p0 = rptr;
1140 b0 = CLIP_Z_GT_W(p0);
1141
1142 for(i=0; i<rcount; ++i)
1143 {
1144 p1 = (i+1 != rcount) ? p0 + 4 : rptr;
1145 b1 = CLIP_Z_GT_W(p1);
1146 if(b0 ^ b1)
1147 {
1148 wptr[0] = p1[0] - p0[0];
1149 wptr[1] = p1[1] - p0[1];
1150 wptr[2] = p1[2] - p0[2];
1151 wptr[3] = p1[3] - p0[3];
1152
1153 scale = (p0[2] + p0[3]) / (-wptr[3] - wptr[2]);
1154
1155 wptr[0] = (vec_t)(p0[0] + scale*(wptr[0]));
1156 wptr[1] = (vec_t)(p0[1] + scale*(wptr[1]));
1157 wptr[2] = (vec_t)(p0[2] + scale*(wptr[2]));
1158 wptr[3] = (vec_t)(p0[3] + scale*(wptr[3]));
1159
1160 wptr += 4;
1161 ++wcount;
1162 }
1163
1164 if(b1)
1165 {
1166 wptr[0] = p1[0];
1167 wptr[1] = p1[1];
1168 wptr[2] = p1[2];
1169 wptr[3] = p1[3];
1170
1171 wptr += 4;
1172 ++wcount;
1173 }
1174
1175 p0 = p1;
1176 b0 = b1;
1177 }
1178
1179 return wcount;
1180 }
1181
m4x4_clip_triangle(const m4x4_t matrix,const vec3_t p0,const vec3_t p1,const vec3_t p2,vec4_t clipped[9])1182 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])
1183 {
1184 clipped[0][0] = p0[0];
1185 clipped[0][1] = p0[1];
1186 clipped[0][2] = p0[2];
1187 clipped[0][3] = 1;
1188 clipped[1][0] = p1[0];
1189 clipped[1][1] = p1[1];
1190 clipped[1][2] = p1[2];
1191 clipped[1][3] = 1;
1192 clipped[2][0] = p2[0];
1193 clipped[2][1] = p2[1];
1194 clipped[2][2] = p2[2];
1195 clipped[2][3] = 1;
1196
1197 m4x4_transform_vec4(matrix, clipped[0]);
1198 m4x4_transform_vec4(matrix, clipped[1]);
1199 m4x4_transform_vec4(matrix, clipped[2]);
1200
1201 return homogenous_clip_triangle(clipped);
1202 }
1203
homogenous_clip_line(vec4_t clipped[2])1204 unsigned int homogenous_clip_line(vec4_t clipped[2])
1205 {
1206 vec4_t clip;
1207 double scale;
1208 const vec_t* const p0 = clipped[0];
1209 const vec_t* const p1 = clipped[1];
1210
1211 // early out
1212 {
1213 clipmask_t mask0 = homogenous_clip_point(clipped[0]);
1214 clipmask_t mask1 = homogenous_clip_point(clipped[1]);
1215
1216 if((mask0 | mask1) == CLIP_PASS) // both points passed all planes
1217 return 2;
1218
1219 if(mask0 & mask1) // both points failed any one plane
1220 return 0;
1221 }
1222
1223 {
1224 const unsigned int index = CLIP_X_LT_W(p0);
1225 if(index ^ CLIP_X_LT_W(p1))
1226 {
1227 clip[0] = p1[0] - p0[0];
1228 clip[1] = p1[1] - p0[1];
1229 clip[2] = p1[2] - p0[2];
1230 clip[3] = p1[3] - p0[3];
1231
1232 scale = (p0[0] - p0[3]) / (clip[3] - clip[0]);
1233
1234 clip[0] = (vec_t)(p0[0] + scale*(clip[0]));
1235 clip[1] = (vec_t)(p0[1] + scale*(clip[1]));
1236 clip[2] = (vec_t)(p0[2] + scale*(clip[2]));
1237 clip[3] = (vec_t)(p0[3] + scale*(clip[3]));
1238
1239 clipped[index][0] = clip[0];
1240 clipped[index][1] = clip[1];
1241 clipped[index][2] = clip[2];
1242 clipped[index][3] = clip[3];
1243 }
1244 else if(index == 0)
1245 return 0;
1246 }
1247
1248 {
1249 const unsigned int index = CLIP_X_GT_W(p0);
1250 if(index ^ CLIP_X_GT_W(p1))
1251 {
1252 clip[0] = p1[0] - p0[0];
1253 clip[1] = p1[1] - p0[1];
1254 clip[2] = p1[2] - p0[2];
1255 clip[3] = p1[3] - p0[3];
1256
1257 scale = (p0[0] + p0[3]) / (-clip[3] - clip[0]);
1258
1259 clip[0] = (vec_t)(p0[0] + scale*(clip[0]));
1260 clip[1] = (vec_t)(p0[1] + scale*(clip[1]));
1261 clip[2] = (vec_t)(p0[2] + scale*(clip[2]));
1262 clip[3] = (vec_t)(p0[3] + scale*(clip[3]));
1263
1264 clipped[index][0] = clip[0];
1265 clipped[index][1] = clip[1];
1266 clipped[index][2] = clip[2];
1267 clipped[index][3] = clip[3];
1268 }
1269 else if(index == 0)
1270 return 0;
1271 }
1272
1273 {
1274 const unsigned int index = CLIP_Y_LT_W(p0);
1275 if(index ^ CLIP_Y_LT_W(p1))
1276 {
1277 clip[0] = p1[0] - p0[0];
1278 clip[1] = p1[1] - p0[1];
1279 clip[2] = p1[2] - p0[2];
1280 clip[3] = p1[3] - p0[3];
1281
1282 scale = (p0[1] - p0[3]) / (clip[3] - clip[1]);
1283
1284 clip[0] = (vec_t)(p0[0] + scale*(clip[0]));
1285 clip[1] = (vec_t)(p0[1] + scale*(clip[1]));
1286 clip[2] = (vec_t)(p0[2] + scale*(clip[2]));
1287 clip[3] = (vec_t)(p0[3] + scale*(clip[3]));
1288
1289 clipped[index][0] = clip[0];
1290 clipped[index][1] = clip[1];
1291 clipped[index][2] = clip[2];
1292 clipped[index][3] = clip[3];
1293 }
1294 else if(index == 0)
1295 return 0;
1296 }
1297
1298 {
1299 const unsigned int index = CLIP_Y_GT_W(p0);
1300 if(index ^ CLIP_Y_GT_W(p1))
1301 {
1302 clip[0] = p1[0] - p0[0];
1303 clip[1] = p1[1] - p0[1];
1304 clip[2] = p1[2] - p0[2];
1305 clip[3] = p1[3] - p0[3];
1306
1307 scale = (p0[1] + p0[3]) / (-clip[3] - clip[1]);
1308
1309 clip[0] = (vec_t)(p0[0] + scale*(clip[0]));
1310 clip[1] = (vec_t)(p0[1] + scale*(clip[1]));
1311 clip[2] = (vec_t)(p0[2] + scale*(clip[2]));
1312 clip[3] = (vec_t)(p0[3] + scale*(clip[3]));
1313
1314 clipped[index][0] = clip[0];
1315 clipped[index][1] = clip[1];
1316 clipped[index][2] = clip[2];
1317 clipped[index][3] = clip[3];
1318 }
1319 else if(index == 0)
1320 return 0;
1321 }
1322
1323 {
1324 const unsigned int index = CLIP_Z_LT_W(p0);
1325 if(index ^ CLIP_Z_LT_W(p1))
1326 {
1327 clip[0] = p1[0] - p0[0];
1328 clip[1] = p1[1] - p0[1];
1329 clip[2] = p1[2] - p0[2];
1330 clip[3] = p1[3] - p0[3];
1331
1332 scale = (p0[2] - p0[3]) / (clip[3] - clip[2]);
1333
1334 clip[0] = (vec_t)(p0[0] + scale*(clip[0]));
1335 clip[1] = (vec_t)(p0[1] + scale*(clip[1]));
1336 clip[2] = (vec_t)(p0[2] + scale*(clip[2]));
1337 clip[3] = (vec_t)(p0[3] + scale*(clip[3]));
1338
1339 clipped[index][0] = clip[0];
1340 clipped[index][1] = clip[1];
1341 clipped[index][2] = clip[2];
1342 clipped[index][3] = clip[3];
1343 }
1344 else if(index == 0)
1345 return 0;
1346 }
1347
1348 {
1349 const unsigned int index = CLIP_Z_GT_W(p0);
1350 if(index ^ CLIP_Z_GT_W(p1))
1351 {
1352 clip[0] = p1[0] - p0[0];
1353 clip[1] = p1[1] - p0[1];
1354 clip[2] = p1[2] - p0[2];
1355 clip[3] = p1[3] - p0[3];
1356
1357 scale = (p0[2] + p0[3]) / (-clip[3] - clip[2]);
1358
1359 clip[0] = (vec_t)(p0[0] + scale*(clip[0]));
1360 clip[1] = (vec_t)(p0[1] + scale*(clip[1]));
1361 clip[2] = (vec_t)(p0[2] + scale*(clip[2]));
1362 clip[3] = (vec_t)(p0[3] + scale*(clip[3]));
1363
1364 clipped[index][0] = clip[0];
1365 clipped[index][1] = clip[1];
1366 clipped[index][2] = clip[2];
1367 clipped[index][3] = clip[3];
1368 }
1369 else if(index == 0)
1370 return 0;
1371 }
1372
1373 return 2;
1374 }
1375
m4x4_clip_line(const m4x4_t matrix,const vec3_t p0,const vec3_t p1,vec4_t clipped[2])1376 unsigned int m4x4_clip_line(const m4x4_t matrix, const vec3_t p0, const vec3_t p1, vec4_t clipped[2])
1377 {
1378 clipped[0][0] = p0[0];
1379 clipped[0][1] = p0[1];
1380 clipped[0][2] = p0[2];
1381 clipped[0][3] = 1;
1382 clipped[1][0] = p1[0];
1383 clipped[1][1] = p1[1];
1384 clipped[1][2] = p1[2];
1385 clipped[1][3] = 1;
1386
1387 m4x4_transform_vec4(matrix, clipped[0]);
1388 m4x4_transform_vec4(matrix, clipped[1]);
1389
1390 return homogenous_clip_line(clipped);
1391 }
1392
m4x4_transpose(m4x4_t matrix)1393 void m4x4_transpose(m4x4_t matrix)
1394 {
1395 int i, j;
1396 float temp, *p1, *p2;
1397
1398 for (i=1; i<4; i++) {
1399 for (j=0; j<i; j++) {
1400 p1 = matrix+(j*4+i);
1401 p2 = matrix+(i*4+j);
1402 temp = *p1;
1403 *p1=*p2;
1404 *p2=temp;
1405 }
1406 }
1407 }
1408
1409 /* adapted from Graphics Gems 2
1410 invert a 3d matrix (4x3) */
m4x4_orthogonal_invert(m4x4_t matrix)1411 int m4x4_orthogonal_invert(m4x4_t matrix)
1412 {
1413 m4x4_t temp;
1414 vec_t* src = temp;
1415
1416 m4x4_assign(src, matrix);
1417
1418 /* Calculate the determinant of upper left 3x3 submatrix and
1419 * determine if the matrix is singular.
1420 */
1421 {
1422 #if 0
1423 float pos = 0.0f;
1424 float neg = 0.0f;
1425 float det = src[0] * src[5] * src[10];
1426 if (det >= 0.0) pos += det; else neg += det;
1427
1428 det = src[1] * src[6] * src[8];
1429 if (det >= 0.0) pos += det; else neg += det;
1430
1431 det = src[2] * src[4] * src[9];
1432 if (det >= 0.0) pos += det; else neg += det;
1433
1434 det = -src[2] * src[5] * src[8];
1435 if (det >= 0.0) pos += det; else neg += det;
1436
1437 det = -src[1] * src[4] * src[10];
1438 if (det >= 0.0) pos += det; else neg += det;
1439
1440 det = -src[0] * src[6] * src[9];
1441 if (det >= 0.0) pos += det; else neg += det;
1442
1443 det = pos + neg;
1444 #elif 0
1445 float det
1446 = (src[0] * src[5] * src[10])
1447 + (src[1] * src[6] * src[8])
1448 + (src[2] * src[4] * src[9])
1449 - (src[2] * src[5] * src[8])
1450 - (src[1] * src[4] * src[10])
1451 - (src[0] * src[6] * src[9]);
1452 #else
1453 float det
1454 = src[0] * ( src[5]*src[10] - src[9]*src[6] )
1455 - src[1] * ( src[4]*src[10] - src[8]*src[6] )
1456 + src[2] * ( src[4]*src[9] - src[8]*src[5] );
1457
1458 #endif
1459
1460 if (det*det < 1e-25)
1461 return 1;
1462
1463 det = 1.0f / det;
1464 matrix[0] = ( (src[5]*src[10]- src[6]*src[9] )*det);
1465 matrix[1] = (- (src[1]*src[10]- src[2]*src[9] )*det);
1466 matrix[2] = ( (src[1]*src[6] - src[2]*src[5] )*det);
1467 matrix[4] = (- (src[4]*src[10]- src[6]*src[8] )*det);
1468 matrix[5] = ( (src[0]*src[10]- src[2]*src[8] )*det);
1469 matrix[6] = (- (src[0]*src[6] - src[2]*src[4] )*det);
1470 matrix[8] = ( (src[4]*src[9] - src[5]*src[8] )*det);
1471 matrix[9] = (- (src[0]*src[9] - src[1]*src[8] )*det);
1472 matrix[10]= ( (src[0]*src[5] - src[1]*src[4] )*det);
1473 }
1474
1475 /* Do the translation part */
1476 matrix[12] = - (src[12] * matrix[0] +
1477 src[13] * matrix[4] +
1478 src[14] * matrix[8]);
1479 matrix[13] = - (src[12] * matrix[1] +
1480 src[13] * matrix[5] +
1481 src[14] * matrix[9]);
1482 matrix[14] = - (src[12] * matrix[2] +
1483 src[13] * matrix[6] +
1484 src[14] * matrix[10]);
1485
1486 return 0;
1487 }
1488
quat_identity(vec4_t quat)1489 void quat_identity(vec4_t quat)
1490 {
1491 quat[0] = quat[1] = quat[2] = 0;
1492 quat[3] = 1;
1493 }
1494
quat_multiply_by_quat(vec4_t quat,const vec4_t other)1495 void quat_multiply_by_quat(vec4_t quat, const vec4_t other)
1496 {
1497 const vec_t x = quat[3]*other[0] + quat[0]*other[3] + quat[1]*other[2] - quat[2]*other[1];
1498 const vec_t y = quat[3]*other[1] + quat[1]*other[3] + quat[2]*other[0] - quat[0]*other[2];
1499 const vec_t z = quat[3]*other[2] + quat[2]*other[3] + quat[0]*other[1] - quat[1]*other[0];
1500 const vec_t w = quat[3]*other[3] - quat[0]*other[0] - quat[1]*other[1] - quat[2]*other[2];
1501 quat[0] = x;
1502 quat[1] = y;
1503 quat[2] = z;
1504 quat[3] = w;
1505 }
1506
quat_conjugate(vec4_t quat)1507 void quat_conjugate(vec4_t quat)
1508 {
1509 VectorNegate(quat, quat);
1510 }
1511
1512 //! quaternion from two unit vectors
quat_for_unit_vectors(vec4_t quat,const vec3_t from,const vec3_t to)1513 void quat_for_unit_vectors(vec4_t quat, const vec3_t from, const vec3_t to)
1514 {
1515 CrossProduct(from, to, quat);
1516 quat[3] = DotProduct(from, to);
1517 }
1518
quat_normalise(vec4_t quat)1519 void quat_normalise(vec4_t quat)
1520 {
1521 const vec_t n = 1 / ( quat[0] * quat[0] + quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3] );
1522 quat[0] *= n;
1523 quat[1] *= n;
1524 quat[2] *= n;
1525 quat[3] *= n;
1526 }
1527
quat_for_axisangle(vec4_t quat,const vec3_t axis,double angle)1528 void quat_for_axisangle(vec4_t quat, const vec3_t axis, double angle)
1529 {
1530 angle *= 0.5;
1531
1532 quat[3] = (float)sin(angle);
1533
1534 quat[0] = axis[0] * quat[3];
1535 quat[1] = axis[1] * quat[3];
1536 quat[2] = axis[2] * quat[3];
1537 quat[3] = (float)cos(angle);
1538 }
1539
m3x3_multiply_by_m3x3(m3x3_t matrix,const m3x3_t matrix_src)1540 void m3x3_multiply_by_m3x3(m3x3_t matrix, const m3x3_t matrix_src)
1541 {
1542 float *pDest = matrix;
1543 float out1, out2, out3;
1544 int i;
1545
1546 for(i=0;i<3;i++)
1547 {
1548 out1 = matrix_src[0] * pDest[0];
1549 out1 += matrix_src[1] * pDest[3];
1550 out1 += matrix_src[2] * pDest[6];
1551 out2 = matrix_src[3] * pDest[0];
1552 out2 += matrix_src[4] * pDest[3];
1553 out2 += matrix_src[5] * pDest[6];
1554 out3 = matrix_src[6] * pDest[0];
1555 out3 += matrix_src[7] * pDest[3];
1556 out3 += matrix_src[8] * pDest[6];
1557
1558 pDest[0] = out1;
1559 pDest[3] = out2;
1560 pDest[6] = out3;
1561
1562 pDest++;
1563 }
1564 }
1565
m3x3_transform_vec3(const m3x3_t matrix,vec3_t vector)1566 void m3x3_transform_vec3(const m3x3_t matrix, vec3_t vector)
1567 {
1568 float out1, out2, out3;
1569
1570 out1 = matrix[0] * vector[0];
1571 out1 += matrix[3] * vector[1];
1572 out1 += matrix[6] * vector[2];
1573 out2 = matrix[1] * vector[0];
1574 out2 += matrix[4] * vector[1];
1575 out2 += matrix[7] * vector[2];
1576 out3 = matrix[2] * vector[0];
1577 out3 += matrix[5] * vector[1];
1578 out3 += matrix[8] * vector[2];
1579
1580 vector[0] = out1;
1581 vector[1] = out2;
1582 vector[2] = out3;
1583 }
1584
m3_det(m3x3_t mat)1585 float m3_det( m3x3_t mat )
1586 {
1587 float det;
1588
1589 det = mat[0] * ( mat[4]*mat[8] - mat[7]*mat[5] )
1590 - mat[1] * ( mat[3]*mat[8] - mat[6]*mat[5] )
1591 + mat[2] * ( mat[3]*mat[7] - mat[6]*mat[4] );
1592
1593 return( det );
1594 }
1595
m3_inverse(m3x3_t mr,m3x3_t ma)1596 int m3_inverse( m3x3_t mr, m3x3_t ma )
1597 {
1598 float det = m3_det( ma );
1599
1600 if (det == 0 )
1601 {
1602 return 1;
1603 }
1604
1605
1606 mr[0] = ma[4]*ma[8] - ma[5]*ma[7] / det;
1607 mr[1] = -( ma[1]*ma[8] - ma[7]*ma[2] ) / det;
1608 mr[2] = ma[1]*ma[5] - ma[4]*ma[2] / det;
1609
1610 mr[3] = -( ma[3]*ma[8] - ma[5]*ma[6] ) / det;
1611 mr[4] = ma[0]*ma[8] - ma[6]*ma[2] / det;
1612 mr[5] = -( ma[0]*ma[5] - ma[3]*ma[2] ) / det;
1613
1614 mr[6] = ma[3]*ma[7] - ma[6]*ma[4] / det;
1615 mr[7] = -( ma[0]*ma[7] - ma[6]*ma[1] ) / det;
1616 mr[8] = ma[0]*ma[4] - ma[1]*ma[3] / det;
1617
1618 return 0;
1619 }
1620
m4_submat(m4x4_t mr,m3x3_t mb,int i,int j)1621 void m4_submat( m4x4_t mr, m3x3_t mb, int i, int j )
1622 {
1623 int ti, tj, idst, jdst;
1624
1625 for ( ti = 0; ti < 4; ti++ )
1626 {
1627 if ( ti < i )
1628 idst = ti;
1629 else
1630 if ( ti > i )
1631 idst = ti-1;
1632
1633 for ( tj = 0; tj < 4; tj++ )
1634 {
1635 if ( tj < j )
1636 jdst = tj;
1637 else
1638 if ( tj > j )
1639 jdst = tj-1;
1640
1641 if ( ti != i && tj != j )
1642 mb[idst*3 + jdst] = mr[ti*4 + tj ];
1643 }
1644 }
1645 }
1646
m4_det(m4x4_t mr)1647 float m4_det( m4x4_t mr )
1648 {
1649 float det, result = 0, i = 1;
1650 m3x3_t msub3;
1651 int n;
1652
1653 for ( n = 0; n < 4; n++, i *= -1 )
1654 {
1655 m4_submat( mr, msub3, 0, n );
1656
1657 det = m3_det( msub3 );
1658 result += mr[n] * det * i;
1659 }
1660
1661 return result;
1662 }
1663
m4x4_invert(m4x4_t matrix)1664 int m4x4_invert(m4x4_t matrix)
1665 {
1666 float mdet = m4_det( matrix );
1667 m3x3_t mtemp;
1668 int i, j, sign;
1669 m4x4_t m4x4_temp;
1670
1671 #if 0
1672 if ( fabs( mdet ) < 0.0000000001 )
1673 return 1;
1674 #endif
1675
1676 m4x4_assign(m4x4_temp, matrix);
1677
1678 for ( i = 0; i < 4; i++ )
1679 for ( j = 0; j < 4; j++ )
1680 {
1681 sign = 1 - ( (i +j) % 2 ) * 2;
1682
1683 m4_submat( m4x4_temp, mtemp, i, j );
1684
1685 matrix[i+j*4] = ( m3_det( mtemp ) * sign ) / mdet; /* FIXME: try using * inverse det and see if speed/accuracy are good enough */
1686 }
1687
1688 return 0;
1689 }
1690 #if 0
1691 void m4x4_solve_ge(m4x4_t matrix, vec4_t x)
1692 {
1693 int indx[4];
1694 int c,r;
1695 int i;
1696 int best;
1697 float scale[4];
1698 float f, pivot;
1699 float aug[4];
1700 float recip, ratio;
1701 float* p;
1702
1703 for(r=0; r<4; r++)
1704 {
1705 aug[r] = 0;
1706 indx[r] = r;
1707 }
1708
1709 for (r=0; r<4; r++)
1710 {
1711 scale[r] = 0;
1712 for (c=0; c<4; c++, p++)
1713 {
1714 if (fabs(*p) > scale[r])
1715 {
1716 scale[r] = (float)fabs(*p);
1717 }
1718 }
1719 }
1720
1721 for (c=0; c<3; c++)
1722 {
1723 pivot = 0;
1724 for (r=c; r<4; r++)
1725 {
1726 f = (float)fabs(matrix[(indx[r]<<2)+c]) / scale[indx[r]];
1727 if (f > pivot)
1728 {
1729 pivot = f;
1730 best = r;
1731 }
1732 }
1733
1734 i = indx[c];
1735 indx[c] = indx[best];
1736 indx[best] = i;
1737
1738 recip = 1 / matrix[(indx[c]<<2)+c];
1739
1740 for (r=c+1; r<4; r++)
1741 {
1742 p = matrix + (indx[r]<<2);
1743 ratio = p[c] * recip;
1744
1745 for (i=c+1; i<4; i++)
1746 p[i] -= ratio * matrix[(indx[c]<<2)+i];
1747 aug[indx[r]] -= ratio * aug[indx[c]];
1748 }
1749 }
1750
1751 x[indx[3]] = aug[indx[3]] / matrix[(indx[3]<<2)+3];
1752 for(r=2; r>=0; r--)
1753 {
1754 f = aug[indx[r]];
1755 p = matrix + (indx[r]<<2);
1756 recip = 1 / p[r];
1757 for(c=(r+1); c<4; c++)
1758 {
1759 f -= (p[c] * x[indx[c]]);
1760 }
1761 x[indx[r]] = f * recip;
1762 }
1763 }
1764 #endif
1765
1766 #define N 3
1767
matrix_solve_ge(vec_t * matrix,vec_t * aug,vec3_t x)1768 int matrix_solve_ge(vec_t* matrix, vec_t* aug, vec3_t x)
1769 {
1770 int indx[N];
1771 int c,r;
1772 int i;
1773 int best;
1774 float scale[N];
1775 float f, pivot;
1776 float ratio;
1777 float* p;
1778
1779 for(r=0; r<N; r++)
1780 {
1781 indx[r] = r;
1782 }
1783
1784 for (r=0; r<N; r++)
1785 {
1786 p = matrix+r;
1787 scale[r] = 0;
1788 for (c=0; c<N; c++, p++)
1789 {
1790 if (fabs(*p) > scale[r])
1791 {
1792 scale[r] = (float)fabs(*p);
1793 }
1794 }
1795 }
1796
1797 for (c=0; c<N; c++)
1798 {
1799 pivot = 0;
1800 best = -1;
1801 for (r=c; r<N; r++)
1802 {
1803 f = (float)fabs(matrix[(indx[r]*N)+c]) / scale[indx[r]];
1804 if (f > pivot)
1805 {
1806 pivot = f;
1807 best = r;
1808 }
1809 }
1810
1811 if(best == -1) return 1;
1812
1813 i = indx[c];
1814 indx[c] = indx[best];
1815 indx[best] = i;
1816
1817 for (r=c+1; r<N; r++)
1818 {
1819 p = matrix + (indx[r]*N);
1820 ratio = p[c] / matrix[(indx[c]*N)+c];
1821
1822 for (i=c+1; i<N; i++) p[i] -= ratio * matrix[(indx[c]*N)+i];
1823 aug[indx[r]] -= ratio * aug[indx[c]];
1824 }
1825 }
1826
1827 x[N-1] = aug[indx[N-1]] / matrix[(indx[N-1]*N)+N-1];
1828 for(r=1; r>=0; r--)
1829 {
1830 f = aug[indx[r]];
1831 p = matrix + (indx[r]*N);
1832 for(c=(r+1); c<N; c++) f -= (p[c] * x[c]);
1833 x[r] = f / p[r];
1834 }
1835 return 0;
1836 }
1837
1838 #ifdef YOU_WANT_IT_TO_BORK
1839 /* Gaussian elimination */
1840 for(i=0;i<4;i++)
1841 {
1842 for(j=(i+1);j<4;j++)
1843 {
1844 ratio = matrix[j][i] / matrix[i][i];
1845 for(count=i;count<n;count++) {
1846 matrix[j][count] -= (ratio * matrix[i][count]);
1847 }
1848 b[j] -= (ratio * b[i]);
1849 }
1850 }
1851
1852 /* Back substitution */
1853 x[n-1] = b[n-1] / matrix[n-1][n-1];
1854 for(i=(n-2);i>=0;i--)
1855 {
1856 temp = b[i];
1857 for(j=(i+1);j<n;j++)
1858 {
1859 temp -= (matrix[i][j] * x[j]);
1860 }
1861 x[i] = temp / matrix[i][i];
1862 }
1863 #endif
1864
plane_intersect_planes(const vec4_t plane1,const vec4_t plane2,const vec4_t plane3,vec3_t intersection)1865 int plane_intersect_planes(const vec4_t plane1, const vec4_t plane2, const vec4_t plane3, vec3_t intersection)
1866 {
1867 m3x3_t planes;
1868 vec3_t b;
1869 VectorCopy(plane1, planes+0);
1870 b[0] = plane1[3];
1871 VectorCopy(plane2, planes+3);
1872 b[1] = plane2[3];
1873 VectorCopy(plane3, planes+6);
1874 b[2] = plane3[3];
1875
1876 return matrix_solve_ge(planes, b, intersection);
1877 }
1878