1 /*
2 * Copyright 2011-2012 Arx Libertatis Team (see the AUTHORS file)
3 *
4 * This file is part of Arx Libertatis.
5 *
6 * Arx Libertatis is free software: you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation, either version 3 of the License, or
9 * (at your option) any later version.
10 *
11 * Arx Libertatis is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with Arx Libertatis. If not, see <http://www.gnu.org/licenses/>.
18 */
19 /* Based on:
20 ===========================================================================
21 ARX FATALIS GPL Source Code
22 Copyright (C) 1999-2010 Arkane Studios SA, a ZeniMax Media company.
23
24 This file is part of the Arx Fatalis GPL Source Code ('Arx Fatalis Source Code').
25
26 Arx Fatalis Source Code is free software: you can redistribute it and/or modify it under the terms of the GNU General Public
27 License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
28
29 Arx Fatalis Source Code is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied
30 warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
31
32 You should have received a copy of the GNU General Public License along with Arx Fatalis Source Code. If not, see
33 <http://www.gnu.org/licenses/>.
34
35 In addition, the Arx Fatalis Source Code is also subject to certain additional terms. You should have received a copy of these
36 additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Arx
37 Fatalis Source Code. If not, please request a copy in writing from Arkane Studios at the address below.
38
39 If you have questions concerning this license or the applicable additional terms, you may contact in writing Arkane Studios, c/o
40 ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
41 ===========================================================================
42 */
43 // Code: Cyril Meynier
44 //
45 // Copyright (c) 1999 ARKANE Studios SA. All rights reserved
46
47 #include "graphics/Math.h"
48
49 #include <algorithm>
50
51 #include "graphics/GraphicsTypes.h"
52
53 using std::min;
54 using std::max;
55
56 /* Triangle/triangle intersection test routine,
57 * int tri_tri_intersect(float V0[3],float V1[3],float V2[3],
58 * float U0[3],float U1[3],float U2[3])
59 *
60 * parameters: vertices of triangle 1: V0,V1,V2
61 * vertices of triangle 2: U0,U1,U2
62 * result : returns 1 if the triangles intersect, otherwise 0
63 *
64 */
65
66 #define OPTIM_COMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,A,B,C,X0,X1) \
67 { \
68 if(D0D1>0.0f) \
69 { \
70 /* here we know that D0D2<=0.0 */ \
71 /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
72 A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \
73 } \
74 else if(D0D2>0.0f)\
75 { \
76 /* here we know that d0d1<=0.0 */ \
77 A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \
78 } \
79 else if(D1*D2>0.0f || D0!=0.0f) \
80 { \
81 /* here we know that d0d1<=0.0 or that D0!=0.0 */ \
82 A=VV0; B=(VV1-VV0)*D0; C=(VV2-VV0)*D0; X0=D0-D1; X1=D0-D2; \
83 } \
84 else if(D1!=0.0f) \
85 { \
86 A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \
87 } \
88 else if(D2!=0.0f) \
89 { \
90 A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \
91 } \
92 else \
93 { \
94 /* triangles are coplanar */ \
95 return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \
96 } \
97 }
98
99
100
101 /* this edge to edge test is based on Franlin Antonio's gem:
102 "Faster Line Segment Intersection", in Graphics Gems III,
103 pp. 199-202 */
104 #define EDGE_EDGE_TEST(V0,U0,U1) \
105 Bx=U0[i0]-U1[i0]; \
106 By=U0[i1]-U1[i1]; \
107 Cx=V0[i0]-U0[i0]; \
108 Cy=V0[i1]-U0[i1]; \
109 f=Ay*Bx-Ax*By; \
110 d=By*Cx-Bx*Cy; \
111 if((f>0 && d>=0 && d<=f) || (f<0 && d<=0 && d>=f)) \
112 { \
113 e=Ax*Cy-Ay*Cx; \
114 if(f>0) \
115 { \
116 if(e>=0 && e<=f) return 1; \
117 } \
118 else \
119 { \
120 if(e<=0 && e>=f) return 1; \
121 } \
122 }
123
124 #define EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2) \
125 { \
126 float Ax,Ay,Bx,By,Cx,Cy,e,d,f; \
127 Ax=V1[i0]-V0[i0]; \
128 Ay=V1[i1]-V0[i1]; \
129 /* test edge U0,U1 against V0,V1 */ \
130 EDGE_EDGE_TEST(V0,U0,U1); \
131 /* test edge U1,U2 against V0,V1 */ \
132 EDGE_EDGE_TEST(V0,U1,U2); \
133 /* test edge U2,U1 against V0,V1 */ \
134 EDGE_EDGE_TEST(V0,U2,U0); \
135 }
136
137 #define POINT_IN_TRI(V0,U0,U1,U2) \
138 { \
139 float a,b,c,d0,d1,d2; \
140 /* is T1 completly inside T2? */ \
141 /* check if V0 is inside tri(U0,U1,U2) */ \
142 a=U1[i1]-U0[i1]; \
143 b=-(U1[i0]-U0[i0]); \
144 c=-a*U0[i0]-b*U0[i1]; \
145 d0=a*V0[i0]+b*V0[i1]+c; \
146 \
147 a=U2[i1]-U1[i1]; \
148 b=-(U2[i0]-U1[i0]); \
149 c=-a*U1[i0]-b*U1[i1]; \
150 d1=a*V0[i0]+b*V0[i1]+c; \
151 \
152 a=U0[i1]-U2[i1]; \
153 b=-(U0[i0]-U2[i0]); \
154 c=-a*U2[i0]-b*U2[i1]; \
155 d2=a*V0[i0]+b*V0[i1]+c; \
156 if(d0*d1>0.0) \
157 { \
158 if(d0*d2>0.0) return 1; \
159 } \
160 }
161
coplanar_tri_tri(const float N[3],const float V0[3],const float V1[3],const float V2[3],const float U0[3],const float U1[3],const float U2[3])162 int coplanar_tri_tri(const float N[3], const float V0[3], const float V1[3], const float V2[3],
163 const float U0[3], const float U1[3], const float U2[3])
164 {
165 float A[3];
166 short i0, i1;
167 /* first project onto an axis-aligned plane, that maximizes the area */
168 /* of the triangles, compute indices: i0,i1. */
169 A[0] = EEfabs(N[0]);
170 A[1] = EEfabs(N[1]);
171 A[2] = EEfabs(N[2]);
172
173 if (A[0] > A[1])
174 {
175 if (A[0] > A[2])
176 {
177 i0 = 1; /* A[0] is greatest */
178 i1 = 2;
179 }
180 else
181 {
182 i0 = 0; /* A[2] is greatest */
183 i1 = 1;
184 }
185 }
186 else /* A[0]<=A[1] */
187 {
188 if (A[2] > A[1])
189 {
190 i0 = 0; /* A[2] is greatest */
191 i1 = 1;
192 }
193 else
194 {
195 i0 = 0; /* A[1] is greatest */
196 i1 = 2;
197 }
198 }
199
200 /* test all edges of triangle 1 against the edges of triangle 2 */
201 EDGE_AGAINST_TRI_EDGES(V0, V1, U0, U1, U2);
202 EDGE_AGAINST_TRI_EDGES(V1, V2, U0, U1, U2);
203 EDGE_AGAINST_TRI_EDGES(V2, V0, U0, U1, U2);
204
205 return 0;
206 }
207
208 #define CROSS(dest,v1,v2) \
209 dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
210 dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
211 dest[2]=v1[0]*v2[1]-v1[1]*v2[0];
212
213 #define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])
214
215 #define SUB(dest,v1,v2) \
216 dest[0]=v1[0]-v2[0]; \
217 dest[1]=v1[1]-v2[1]; \
218 dest[2]=v1[2]-v2[2];
219
220 /* sort so that a<=b */
221 #define SORT(a,b) \
222 if(a>b) \
223 { \
224 float c; \
225 c=a; \
226 a=b; \
227 b=c; \
228 }
229
230 //***********************************************************************************************
231 // Computes Intersection of 2 triangles
232 //-----------------------------------------------------------------------------------------------
233 // VERIFIED (Cyril 2001/10/19)
234 // OPTIMIZED (Cyril 2001/10/19) removed divisions, need some more optims perhaps...
235 //***********************************************************************************************
tri_tri_intersect(const EERIE_TRI * VV,const EERIE_TRI * UU)236 int tri_tri_intersect(const EERIE_TRI * VV, const EERIE_TRI * UU)
237 {
238 float E1[3], E2[3];
239 float N1[3], N2[3], d1, d2;
240 float du0, du1, du2, dv0, dv1, dv2;
241 float D[3];
242 float isect1[2], isect2[2];
243 float du0du1, du0du2, dv0dv1, dv0dv2;
244 short index;
245 float vp0, vp1, vp2;
246 float up0, up1, up2;
247 float bb, cc, max;
248 float a, b, c, x0, x1;
249 float d, e, f, y0, y1;
250
251 float xx, yy, xxyy, tmp;
252
253 const float * V0;
254 const float * V1;
255 const float * V2;
256 const float * U0;
257 const float * U1;
258 const float * U2;
259 V0 = VV->v[0].elem;
260 V1 = VV->v[1].elem;
261 V2 = VV->v[2].elem;
262
263 U0 = UU->v[0].elem;
264 U1 = UU->v[1].elem;
265 U2 = UU->v[2].elem;
266
267 /* compute plane equation of triangle(V0,V1,V2) */
268 SUB(E1, V1, V0);
269 SUB(E2, V2, V0);
270 CROSS(N1, E1, E2);
271 d1 = -DOT(N1, V0);
272 /* plane equation 1: N1.X+d1=0 */
273
274 /* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/
275 du0 = DOT(N1, U0) + d1;
276 du1 = DOT(N1, U1) + d1;
277 du2 = DOT(N1, U2) + d1;
278
279 /* coplanarity robustness check */
280 du0du1 = du0 * du1;
281 du0du2 = du0 * du2;
282
283 if (du0du1 > 0.0f && du0du2 > 0.0f) /* same sign on all of them + not equal 0 ? */
284 return 0; /* no intersection occurs */
285
286 /* compute plane of triangle (U0,U1,U2) */
287 SUB(E1, U1, U0);
288 SUB(E2, U2, U0);
289 CROSS(N2, E1, E2);
290 d2 = -DOT(N2, U0);
291 /* plane equation 2: N2.X+d2=0 */
292
293 /* put V0,V1,V2 into plane equation 2 */
294 dv0 = DOT(N2, V0) + d2;
295 dv1 = DOT(N2, V1) + d2;
296 dv2 = DOT(N2, V2) + d2;
297
298 dv0dv1 = dv0 * dv1;
299 dv0dv2 = dv0 * dv2;
300
301 if (dv0dv1 > 0.0f && dv0dv2 > 0.0f) // same sign on all of them + not equal 0 ?
302 return 0; // no intersection occurs
303
304 // compute direction of intersection line
305 CROSS(D, N1, N2);
306
307 // compute and index to the largest component of D
308 max = (float)EEfabs(D[0]);
309 index = 0;
310 bb = (float)EEfabs(D[1]);
311 cc = (float)EEfabs(D[2]);
312
313 if (bb > max) max = bb, index = 1;
314
315 if (cc > max) index = 2;
316
317 // this is the simplified projection onto L
318 vp0 = V0[index];
319 vp1 = V1[index];
320 vp2 = V2[index];
321
322 up0 = U0[index];
323 up1 = U1[index];
324 up2 = U2[index];
325
326 // compute interval for triangle 1
327 OPTIM_COMPUTE_INTERVALS(vp0, vp1, vp2, dv0, dv1, dv2, dv0dv1, dv0dv2, a, b, c, x0, x1);
328
329 // compute interval for triangle 2
330 OPTIM_COMPUTE_INTERVALS(up0, up1, up2, du0, du1, du2, du0du1, du0du2, d, e, f, y0, y1);
331
332 xx = x0 * x1;
333 yy = y0 * y1;
334 xxyy = xx * yy;
335
336 tmp = a * xxyy;
337 isect1[0] = tmp + b * x1 * yy;
338 isect1[1] = tmp + c * x0 * yy;
339
340 tmp = d * xxyy;
341 isect2[0] = tmp + e * xx * y1;
342 isect2[1] = tmp + f * xx * y0;
343
344 SORT(isect1[0], isect1[1]);
345 SORT(isect2[0], isect2[1]);
346
347 if (isect1[1] < isect2[0] || isect2[1] < isect1[0]) return 0;
348
349 return 1;
350 }
351
352 #undef CROSS
353 #undef DOT
354 #undef SUB
355
356 // Computes Bounding Box for a triangle
Triangle_ComputeBoundingBox(EERIE_3D_BBOX * bb,const EERIE_TRI * v)357 static inline void Triangle_ComputeBoundingBox(EERIE_3D_BBOX * bb, const EERIE_TRI * v) {
358 bb->min.x = min(v->v[0].x, v->v[1].x);
359 bb->min.x = min(bb->min.x, v->v[2].x);
360
361 bb->max.x = max(v->v[0].x, v->v[1].x);
362 bb->max.x = max(bb->max.x, v->v[2].x);
363
364 bb->min.y = min(v->v[0].y, v->v[1].y);
365 bb->min.y = min(bb->min.y, v->v[2].y);
366
367 bb->max.y = max(v->v[0].y, v->v[1].y);
368 bb->max.y = max(bb->max.y, v->v[2].y);
369
370 bb->min.z = min(v->v[0].z, v->v[1].z);
371 bb->min.z = min(bb->min.z, v->v[2].z);
372
373 bb->max.z = max(v->v[0].z, v->v[1].z);
374 bb->max.z = max(bb->max.z, v->v[2].z);
375 }
376
Triangles_Intersect(const EERIE_TRI * v,const EERIE_TRI * u)377 bool Triangles_Intersect(const EERIE_TRI * v, const EERIE_TRI * u)
378 {
379 EERIE_3D_BBOX bb1, bb2;
380 Triangle_ComputeBoundingBox(&bb1, v);
381 Triangle_ComputeBoundingBox(&bb2, u);
382
383 if (bb1.max.y < bb2.min.y) return false;
384
385 if (bb1.min.y > bb2.max.y) return false;
386
387 if (bb1.max.x < bb2.min.x) return false;
388
389 if (bb1.min.x > bb2.max.x) return false;
390
391 if (bb1.max.z < bb2.min.z) return false;
392
393 if (bb1.min.z > bb2.max.z) return false;
394
395 if (tri_tri_intersect(v, u))
396 return true;
397
398 return false;
399 }
400
401 ///////////////////////////////////////////////////////////////////////////////////
402
403 #define X 0
404 #define Y 1
405 #define Z 2
406
407 #define FINDMINMAX(x0,x1,x2,min,max) \
408 min = max = x0; \
409 if(x1<min) min=x1;\
410 else if(x1>max) max=x1;\
411 if(x2<min) min=x2;\
412 else if(x2>max) max=x2;
413
414 /*======================== X-tests ========================*/
415 #define AXISTEST_X01(a, b, fa, fb) \
416 p0 = a*v0[Y] - b*v0[Z]; \
417 p2 = a*v2[Y] - b*v2[Z]; \
418 if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
419 rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z]; \
420 if(min>rad || max<-rad) return 0;
421
422 #define AXISTEST_X2(a, b, fa, fb) \
423 p0 = a*v0[Y] - b*v0[Z]; \
424 p1 = a*v1[Y] - b*v1[Z]; \
425 if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
426 rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z]; \
427 if(min>rad || max<-rad) return 0;
428
429 /*======================== Y-tests ========================*/
430 #define AXISTEST_Y02(a, b, fa, fb) \
431 p0 = -a*v0[X] + b*v0[Z]; \
432 p2 = -a*v2[X] + b*v2[Z]; \
433 if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
434 rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z]; \
435 if(min>rad || max<-rad) return 0;
436
437 #define AXISTEST_Y1(a, b, fa, fb) \
438 p0 = -a*v0[X] + b*v0[Z]; \
439 p1 = -a*v1[X] + b*v1[Z]; \
440 if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
441 rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z]; \
442 if(min>rad || max<-rad) return 0;
443
444 /*======================== Z-tests ========================*/
445
446 #define AXISTEST_Z12(a, b, fa, fb) \
447 p1 = a*v1[X] - b*v1[Y]; \
448 p2 = a*v2[X] - b*v2[Y]; \
449 if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;} \
450 rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y]; \
451 if(min>rad || max<-rad) return 0;
452
453 #define AXISTEST_Z0(a, b, fa, fb) \
454 p0 = a*v0[X] - b*v0[Y]; \
455 p1 = a*v1[X] - b*v1[Y]; \
456 if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
457 rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y]; \
458 if(min>rad || max<-rad) return 0;
459
460 //*******************************************************************************************
461 //*******************************************************************************************
462
463 // Cylinder y origin must be min Y of cylinder
464 // Cylinder height MUST be negative FROM origin (inverted Theo XYZ system Legacy)
CylinderInCylinder(const EERIE_CYLINDER * cyl1,const EERIE_CYLINDER * cyl2)465 bool CylinderInCylinder(const EERIE_CYLINDER * cyl1, const EERIE_CYLINDER * cyl2)
466 {
467 if (cyl1 == cyl2) return false;
468
469 float m1 = cyl1->origin.y; //tokeep: max(cyl1->origin.y,cyl1->origin.y+cyl1->height);
470 float m2 = cyl2->origin.y + cyl2->height; //tokeep: min(cyl2->origin.y,cyl2->origin.y+cyl2->height);
471
472 if (m2 > m1) return false;
473
474 m1 = cyl1->origin.y + cyl1->height; //tokeep: min(cyl1->origin.y,cyl1->origin.y+cyl1->height);
475 m2 = cyl2->origin.y; //tokeep: max(cyl2->origin.y,cyl2->origin.y+cyl2->height);
476
477 if (m1 > m2) return false;
478
479 m1 = cyl1->radius + cyl2->radius;
480
481 if(!fartherThan(Vec2f(cyl1->origin.x, cyl1->origin.z), Vec2f(cyl2->origin.x, cyl2->origin.z), m1)) {
482 return true;
483 }
484
485 return false;
486 }
487
488 // Sort of...
SphereInCylinder(const EERIE_CYLINDER * cyl1,const EERIE_SPHERE * s)489 bool SphereInCylinder(const EERIE_CYLINDER * cyl1, const EERIE_SPHERE * s)
490 {
491 float m1 = max(cyl1->origin.y, cyl1->origin.y + cyl1->height);
492 float m2 = s->origin.y - s->radius;
493
494 if (m2 > m1) return false;
495
496 m1 = min(cyl1->origin.y, cyl1->origin.y + cyl1->height);
497 m2 = s->origin.y + s->radius;
498
499 if (m1 > m2) return false;
500
501 if(!fartherThan(Vec2f(cyl1->origin.x, cyl1->origin.z), Vec2f(s->origin.x, s->origin.z), cyl1->radius + s->radius)) {
502 return true;
503 }
504
505 return false;
506 }
507
508 //--------------------------------------------------------------------------------------
509 // Quaternions Funcs
510 //--------------------------------------------------------------------------------------
511
512 //*************************************************************************************
513 // Multiply Quaternion 'q1' by Quaternion 'q2', returns result in Quaternion 'dest'
514 //*************************************************************************************
Quat_Multiply(EERIE_QUAT * dest,const EERIE_QUAT * q1,const EERIE_QUAT * q2)515 void Quat_Multiply(EERIE_QUAT * dest, const EERIE_QUAT * q1, const EERIE_QUAT * q2)
516 {
517 /*
518 Fast multiplication
519
520 There are some schemes available that reduces the number of internal
521 multiplications when doing quaternion multiplication. Here is one:
522
523 q = (a, b, c, d), p = (x, y, z, w)
524
525 tmp_00 = (d - c) * (z - w)
526 tmp_01 = (a + b) * (x + y)
527 tmp_02 = (a - b) * (z + w)
528 tmp_03 = (c + d) * (x - y)
529 tmp_04 = (d - b) * (y - z)
530 tmp_05 = (d + b) * (y + z)
531 tmp_06 = (a + c) * (x - w)
532 tmp_07 = (a - c) * (x + w)
533 tmp_08 = tmp_05 + tmp_06 + tmp_07
534 tmp_09 = 0.5 * (tmp_04 + tmp_08)
535
536 q * p = (tmp_00 + tmp_09 - tmp_05,
537 tmp_01 + tmp_09 - tmp_08,
538 tmp_02 + tmp_09 - tmp_07,
539 tmp_03 + tmp_09 - tmp_06)
540
541 With this method You get 7 less multiplications, but 15 more
542 additions/subtractions. Generally, this is still an improvement.
543 */
544
545 dest->x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y;
546 dest->y = q1->w * q2->y + q1->y * q2->w + q1->z * q2->x - q1->x * q2->z;
547 dest->z = q1->w * q2->z + q1->z * q2->w + q1->x * q2->y - q1->y * q2->x;
548 dest->w = q1->w * q2->w - q1->x * q2->x - q1->y * q2->y - q1->z * q2->z;
549 }
550
551 //*************************************************************************************
552 // Invert Multiply of a quaternion by another quaternion
553 //*************************************************************************************
Quat_Divide(EERIE_QUAT * dest,const EERIE_QUAT * q1,const EERIE_QUAT * q2)554 void Quat_Divide(EERIE_QUAT * dest, const EERIE_QUAT * q1, const EERIE_QUAT * q2)
555 {
556 dest->x = q1->w * q2->x - q1->x * q2->w - q1->y * q2->z + q1->z * q2->y;
557 dest->y = q1->w * q2->y - q1->y * q2->w - q1->z * q2->x + q1->x * q2->z;
558 dest->z = q1->w * q2->z - q1->z * q2->w - q1->x * q2->y + q1->y * q2->x;
559 dest->w = q1->w * q2->w + q1->x * q2->x + q1->y * q2->y + q1->z * q2->z;
560 }
561
562 // Invert-Transform of vertex by a quaternion
TransformInverseVertexQuat(const EERIE_QUAT * quat,const Vec3f * vertexin,Vec3f * vertexout)563 void TransformInverseVertexQuat(const EERIE_QUAT * quat, const Vec3f * vertexin,
564 Vec3f * vertexout) {
565
566 EERIE_QUAT rev_quat;
567 Quat_Copy(&rev_quat, quat);
568 Quat_Reverse(&rev_quat);
569
570 float x = vertexin->x;
571 float y = vertexin->y;
572 float z = vertexin->z;
573
574 float qx = rev_quat.x;
575 float qy = rev_quat.y;
576 float qz = rev_quat.z;
577 float qw = rev_quat.w;
578
579 float rx = x * qw - y * qz + z * qy;
580 float ry = y * qw - z * qx + x * qz;
581 float rz = z * qw - x * qy + y * qx;
582 float rw = x * qx + y * qy + z * qz;
583
584 vertexout->x = qw * rx + qx * rw + qy * rz - qz * ry;
585 vertexout->y = qw * ry + qy * rw + qz * rx - qx * rz;
586 vertexout->z = qw * rz + qz * rw + qx * ry - qy * rx;
587 }
588
589
Quat_Slerp(EERIE_QUAT * result,const EERIE_QUAT * from,EERIE_QUAT * to,float ratio)590 void Quat_Slerp(EERIE_QUAT * result, const EERIE_QUAT * from, EERIE_QUAT * to, float ratio)
591 {
592 float fCosTheta = from->x * to->x + from->y * to->y + from->z * to->z + from->w * to->w;
593
594 if (fCosTheta < 0.0f)
595 {
596 fCosTheta = -fCosTheta;
597 to->x = -to->x;
598 to->y = -to->y;
599 to->z = -to->z;
600 to->w = -to->w;
601 }
602
603 float fBeta = 1.f - ratio;
604
605 if (1.0f - fCosTheta > 0.001f)
606 {
607 float fTheta = acosf(fCosTheta);
608 float t = 1 / EEsin(fTheta);
609 fBeta = EEsin(fTheta * fBeta) * t ;
610 ratio = EEsin(fTheta * ratio) * t ;
611 }
612
613 result->x = fBeta * from->x + ratio * to->x;
614 result->y = fBeta * from->y + ratio * to->y;
615 result->z = fBeta * from->z + ratio * to->z;
616 result->w = fBeta * from->w + ratio * to->w;
617 }
618
619
620
621 //*************************************************************************************
622 // Inverts a Quaternion
623 //*************************************************************************************
Quat_Reverse(EERIE_QUAT * q)624 void Quat_Reverse(EERIE_QUAT * q)
625 {
626 EERIE_QUAT qw, qr;
627 Quat_Init(&qw);
628 Quat_Divide(&qr, q, &qw);
629 Quat_Copy(q, &qr);
630
631 }
632
633
634 //*************************************************************************************
635 // Converts euler angles to a unit quaternion.
636 //*************************************************************************************
QuatFromAngles(EERIE_QUAT * q,const Anglef * angle)637 void QuatFromAngles(EERIE_QUAT * q, const Anglef * angle)
638
639 {
640 float A, B;
641 A = angle->yaw * ( 1.0f / 2 );
642 B = angle->pitch * ( 1.0f / 2 );
643
644 float fSinYaw = sinf(A);
645 float fCosYaw = cosf(A);
646 float fSinPitch = sinf(B);
647 float fCosPitch = cosf(B);
648 A = angle->roll * ( 1.0f / 2 );
649 float fSinRoll = sinf(A);
650 float fCosRoll = cosf(A);
651 A = fCosRoll * fCosPitch;
652 B = fSinRoll * fSinPitch;
653 q->x = fSinRoll * fCosPitch * fCosYaw - fCosRoll * fSinPitch * fSinYaw;
654 q->y = fCosRoll * fSinPitch * fCosYaw + fSinRoll * fCosPitch * fSinYaw;
655 q->z = A * fSinYaw - B * fCosYaw;
656 q->w = A * fCosYaw + B * fSinYaw;
657
658 }
659
660 //*************************************************************************************
661 // Converts a unit quaternion into a rotation matrix.
662 //*************************************************************************************
663
MatrixFromQuat(EERIEMATRIX * m,const EERIE_QUAT * quat)664 void MatrixFromQuat(EERIEMATRIX * m, const EERIE_QUAT * quat)
665 {
666 float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
667
668 // calculate coefficients
669 x2 = quat->x + quat->x;
670 y2 = quat->y + quat->y;
671 z2 = quat->z + quat->z;
672 xx = quat->x * x2;
673 xy = quat->x * y2;
674 xz = quat->x * z2;
675 yy = quat->y * y2;
676 yz = quat->y * z2;
677 zz = quat->z * z2;
678 wx = quat->w * x2;
679 wy = quat->w * y2;
680 wz = quat->w * z2;
681
682 m->_11 = 1.0F - (yy + zz);
683 m->_21 = xy - wz;
684 m->_31 = xz + wy;
685 m->_41 = 0.0F;
686
687 m->_12 = xy + wz;
688 m->_22 = 1.0F - (xx + zz);
689 m->_32 = yz - wx;
690 m->_42 = 0.0F;
691
692 m->_13 = xz - wy;
693 m->_23 = yz + wx;
694 m->_33 = 1.0F - (xx + yy);
695 m->_43 = 0.0F;
696 }
697
698 //*************************************************************************************
699 // Converts a rotation matrix into a unit quaternion.
700 //*************************************************************************************
QuatFromMatrix(EERIE_QUAT & quat,EERIEMATRIX & mat)701 void QuatFromMatrix(EERIE_QUAT & quat, EERIEMATRIX & mat)
702 {
703 float m[4][4];
704 m[0][0] = mat._11;
705 m[0][1] = mat._12;
706 m[0][2] = mat._13;
707 m[0][3] = mat._14;
708 m[1][0] = mat._21;
709 m[1][1] = mat._22;
710 m[1][2] = mat._23;
711 m[1][3] = mat._24;
712 m[2][0] = mat._31;
713 m[2][1] = mat._32;
714 m[2][2] = mat._33;
715 m[2][3] = mat._34;
716 m[3][0] = mat._41;
717 m[3][1] = mat._42;
718 m[3][2] = mat._43;
719 m[3][3] = mat._44;
720 float tr, s, q[4];
721
722 int nxt[3] = {1, 2, 0};
723
724 tr = m[0][0] + m[1][1] + m[2][2];
725
726 // check the diagonal
727 if (tr > 0.0f)
728 {
729 s = sqrt(tr + 1.0f);
730 quat.w = s * ( 1.0f / 2 );
731 s = 0.5f / s;
732 quat.x = (m[1][2] - m[2][1]) * s;
733 quat.y = (m[2][0] - m[0][2]) * s;
734 quat.z = (m[0][1] - m[1][0]) * s;
735 }
736 else
737 {
738 // diagonal is negative
739 int i = 0;
740
741 if (m[1][1] > m[0][0]) i = 1;
742
743 if (m[2][2] > m[i][i]) i = 2;
744
745 int j = nxt[i];
746 int k = nxt[j];
747
748 s = sqrt((m[i][i] - (m[j][j] + m[k][k])) + 1.0f);
749
750 q[i] = s * 0.5f;
751
752 if (s != 0.0) s = 0.5f / s;
753
754 q[3] = (m[j][k] - m[k][j]) * s;
755 q[j] = (m[i][j] + m[j][i]) * s;
756 q[k] = (m[i][k] + m[k][i]) * s;
757
758
759 quat.x = q[0];
760 quat.y = q[1];
761 quat.z = q[2];
762 quat.w = q[3];
763 }
764 }
765
766 //--------------------------------------------------------------------------------------
767 // VECTORS Functions
768 //--------------------------------------------------------------------------------------
769
770 // Rotates a Vector around X. angle is given in degrees
VRotateX(Vec3f * out,const float angle)771 void VRotateX(Vec3f * out, const float angle) {
772 Vec3f in = *out;
773 float s = radians(angle);
774 float c = EEcos(s);
775 s = EEsin(s);
776 *out = Vec3f(in.x, (in.y * c) + (in.z * s), (in.z * c) - (in.y * s));
777 }
778
779 // Rotates a Vector around Y. angle is given in degrees
VRotateY(Vec3f * out,const float angle)780 void VRotateY(Vec3f * out, const float angle) {
781 Vec3f in = *out;
782 float s = radians(angle);
783 float c = EEcos(s);
784 s = EEsin(s);
785 *out = Vec3f((in.x * c) + (in.z * s), in.y, (in.z * c) - (in.x * s));
786 }
787
788 // Rotates a Vector around Z. angle is given in degrees
VRotateZ(Vec3f * out,const float angle)789 void VRotateZ(Vec3f * out, const float angle) {
790 Vec3f in = *out;
791 float s = radians(angle);
792 float c = EEcos(s);
793 s = EEsin(s);
794 *out = Vec3f((in.x * c) + (in.y * s), (in.y * c) - (in.x * s), in.z);
795 }
796
797 // Rotates a Vector around Y. angle is given in degrees
Vector_RotateY(Vec3f * dest,const Vec3f * src,const float angle)798 void Vector_RotateY(Vec3f * dest, const Vec3f * src, const float angle) {
799 float s = radians(angle);
800 float c = EEcos(s);
801 s = EEsin(s);
802 *dest = Vec3f((src->x * c) + (src->z * s), src->y, (src->z * c) - (src->x * s));
803 }
804
805 // Rotates a Vector around Z. angle is given in degrees
Vector_RotateZ(Vec3f * dest,const Vec3f * src,const float angle)806 void Vector_RotateZ(Vec3f * dest, const Vec3f * src, const float angle) {
807 float s = radians(angle);
808 float c = EEcos(s);
809 s = EEsin(s);
810 *dest = Vec3f((src->x * c) + (src->y * s), (src->y * c) - (src->x * s), src->z);
811 }
812
813 //A x B = <Ay*Bz - Az*By, Az*Bx - Ax*Bz, Ax*By - Ay*Bx>
CalcFaceNormal(EERIEPOLY * ep,const TexturedVertex * v)814 void CalcFaceNormal(EERIEPOLY * ep, const TexturedVertex * v) {
815
816 float Ax, Ay, Az, Bx, By, Bz;
817 Ax = v[1].p.x - v[0].p.x;
818 Ay = v[1].p.y - v[0].p.y;
819 Az = v[1].p.z - v[0].p.z;
820
821 Bx = v[2].p.x - v[0].p.x;
822 By = v[2].p.y - v[0].p.y;
823 Bz = v[2].p.z - v[0].p.z;
824
825 ep->norm = Vec3f(Ay * Bz - Az * By, Az * Bx - Ax * Bz, Ax * By - Ay * Bx);
826 fnormalize(ep->norm);
827 }
828
CalcObjFaceNormal(const Vec3f * v0,const Vec3f * v1,const Vec3f * v2,EERIE_FACE * ef)829 void CalcObjFaceNormal(const Vec3f * v0, const Vec3f * v1, const Vec3f * v2,
830 EERIE_FACE * ef) {
831
832 float Ax, Ay, Az, Bx, By, Bz;
833 Ax = v1->x - v0->x;
834 Ay = v1->y - v0->y;
835 Az = v1->z - v0->z;
836 Bx = v2->x - v0->x;
837 By = v2->y - v0->y;
838 Bz = v2->z - v0->z;
839
840 ef->norm = Vec3f(Ay * Bz - Az * By, Az * Bx - Ax * Bz, Ax * By - Ay * Bx);
841 ef->norm.normalize();
842 }
843
MatrixReset(EERIEMATRIX * mat)844 void MatrixReset(EERIEMATRIX * mat) {
845 memset(mat, 0, sizeof(EERIEMATRIX));
846 }
847
MatrixSetByVectors(EERIEMATRIX * m,const Vec3f * d,const Vec3f * u)848 void MatrixSetByVectors(EERIEMATRIX * m, const Vec3f * d, const Vec3f * u)
849 {
850 float t;
851 Vec3f D, U, R;
852 D = d->getNormalized();
853 U = *u;
854 t = U.x * D.x + U.y * D.y + U.z * D.z;
855 U.x -= D.x * t;
856 U.y -= D.y * t;
857 U.z -= D.y * t; // TODO is this really supposed to be D.y?
858 U.normalize();
859 R = cross(U, D);
860 m->_11 = R.x;
861 m->_12 = R.y;
862 m->_21 = U.x;
863 m->_22 = U.y;
864 m->_31 = D.x;
865 m->_32 = D.y;
866 m->_33 = D.z;
867 m->_13 = R.z;
868 m->_23 = U.z;
869 }
870
GenerateMatrixUsingVector(EERIEMATRIX * matrix,const Vec3f * vect,float rollDegrees)871 void GenerateMatrixUsingVector(EERIEMATRIX * matrix, const Vec3f * vect, float rollDegrees)
872 {
873 // Get our direction vector (the Z vector component of the matrix)
874 // and make sure it's normalized into a unit vector
875 Vec3f zAxis = vect->getNormalized();
876
877 // Build the Y vector of the matrix (handle the degenerate case
878 // in the way that 3DS does) -- This is not the true vector, only
879 // a reference vector.
880 Vec3f yAxis;
881
882 if (!zAxis.x && !zAxis.z)
883 yAxis = Vec3f(-zAxis.y, 0.f, 0.f);
884 else
885 yAxis = Vec3f(0.f, 1.f, 0.f);
886
887 // Build the X axis vector based on the two existing vectors
888 Vec3f xAxis = cross(yAxis, zAxis).getNormalized();
889
890 // Correct the Y reference vector
891 yAxis = cross(xAxis, zAxis).getNormalized();
892 yAxis = -yAxis;
893
894 // Generate rotation matrix without roll included
895 EERIEMATRIX rot, roll;
896 MatrixReset(&rot);
897 MatrixReset(&roll);
898 rot._11 = yAxis.x;
899 rot._12 = yAxis.y;
900 rot._13 = yAxis.z;
901 rot._21 = zAxis.x;
902 rot._22 = zAxis.y;
903 rot._23 = zAxis.z;
904 rot._31 = xAxis.x;
905 rot._32 = xAxis.y;
906 rot._33 = xAxis.z;
907
908 // Generate the Z rotation matrix for roll
909 roll._33 = 1.f;
910 roll._44 = 1.f;
911 roll._11 = EEcos(radians(rollDegrees));
912 roll._12 = -EEsin(radians(rollDegrees));
913 roll._21 = EEsin(radians(rollDegrees));
914 roll._22 = EEcos(radians(rollDegrees));
915
916 // Concatinate them for a complete rotation matrix that includes
917 // all rotations
918 MatrixMultiply(matrix, &rot, &roll);
919 }
920
921
922 //-----------------------------------------------------------------------------
923 // MatrixMultiply()
924 // Does the matrix operation: [Q] = [A] * [B]. Note that the order of
925 // this operation was changed from the previous version of the DXSDK.
926 //-----------------------------------------------------------------------------
MatrixMultiply(EERIEMATRIX * q,const EERIEMATRIX * a,const EERIEMATRIX * b)927 void MatrixMultiply(EERIEMATRIX * q, const EERIEMATRIX * a, const EERIEMATRIX * b)
928 {
929 const float * pA = &a->_11;
930 const float * pB = &b->_11;
931 float pM[16];
932
933 memset(pM, 0, sizeof(EERIEMATRIX));
934
935 for (size_t i = 0; i < 4; i++)
936 for (size_t j = 0; j < 4; j++)
937 for (size_t k = 0; k < 4; k++)
938 pM[4*i+j] += pA[4*i+k] * pB[4*k+j];
939
940 memcpy(q, pM, sizeof(EERIEMATRIX));
941 }
942
943 // Desc: Multiplies a vector by a matrix
VectorMatrixMultiply(Vec3f * vDest,const Vec3f * vSrc,const EERIEMATRIX * mat)944 void VectorMatrixMultiply(Vec3f * vDest, const Vec3f * vSrc, const EERIEMATRIX * mat) {
945 float x = vSrc->x * mat->_11 + vSrc->y * mat->_21 + vSrc->z * mat->_31 + mat->_41;
946 float y = vSrc->x * mat->_12 + vSrc->y * mat->_22 + vSrc->z * mat->_32 + mat->_42;
947 float z = vSrc->x * mat->_13 + vSrc->y * mat->_23 + vSrc->z * mat->_33 + mat->_43;
948 *vDest = Vec3f(x, y, z);
949 }
950
951 #undef X
952 #undef Y
953 #undef Z
954