1 /*****************************************************************************
2 * @file
3 * @author Jim E. Brooks http://www.jimbrooks.org
4 * @brief Proven math/matrix code written in 2004..2006 (gfx_2006).
5 *
6 * This old code was hacked in order to compile.
7 *
8 *//*
9 * LEGAL: COPYRIGHT (C) 2004 JIM E. BROOKS
10 * THIS SOURCE CODE IS RELEASED UNDER THE TERMS
11 * OF THE GNU GENERAL PUBLIC LICENSE VERSION 2 (GPL 2).
12 *****************************************************************************/
13
14 #ifndef MATH_MATRIX_2006_HH
15 #define MATH_MATRIX_2006_HH
16
17 namespace gfx_2006 {
18
19 using math::Vector2;
20 using math::Vector3;
21 using math::LocalVertex;
22 using math::WorldVertex;
23 using math::EyeVertex;
24 using math::NormalVertex;
25
26 ////////////////////////////////////////////////////////////////////////////////
27 ////////////////////////////// M A C R O S ///////////////////////////////////
28 ////////////////////////////////////////////////////////////////////////////////
29
30 ////////////////////////////////////////////////////////////////////////////////
31 /////////////////////////////// T R I G /////////////////////////////////////
32 ////////////////////////////////////////////////////////////////////////////////
33
34 // fpx is used in trig functions since they're used in matrix functions
35 // which need to maintain precision.
36
37 // 180' = PI
38 // 360' = 2 * PI
39 const fpx PI = 3.14159265358979323846;
40 const fpx PI2 = 6.28318530717958647692; // pi*2
41
42 #if SPEED
43 typedef fp Radian; // aren't distinct C++ types (useless for overloading functions)
44 typedef fp Degree;
45 #else
46 typedef fpx Radian;
47 typedef fpx Degree;
48 #endif
49
50 const Radian RADIAN_180 = math::PI; // 180' in radians (half circle)
51 const Radian RADIAN_360 = math::PI2; // 360' in radians (full circle)
52
53 // Compare degrees/radians while ignoring sign.
54 #if 0 // [2007]
55 #define IF_DEG_GT( DEG1, DEG2 ) (ABS(DEG1) > ABS(DEG2))
56 #define IF_DEG_LT( DEG1, DEG2 ) (ABS(DEG1) < ABS(DEG2))
57 #define IF_RAD_GT( RAD1, RAD2 ) (ABS(RAD1) > ABS(RAD2))
58 #define IF_RAD_LT( RAD1, RAD2 ) (ABS(RAD1) < ABS(RAD2))
59 #endif
60
61 /*****************************************************************************
62 * sincos().
63 *****************************************************************************/
64 template<typename FP>
65 void
SinCos(Radian rad,FP & si,FP & co)66 SinCos( Radian rad, FP& si, FP& co )
67 {
68 // This x87 asm code in this C++ template returning args by reference
69 // seems perilous but was tested by gfx/tests/math.cc.
70 #if defined(__GNUC__) && defined(CPU_X86)
71 asm("fsincos" : "=t" (co), "=u" (si) : "0" (rad));
72 #elif defined(__GNUC__) && _GLIBCXX_HAVE_SINCOS
73 sincos( rad, &si, &co ); // GNU extension
74 #else
75 si = sin( rad );
76 co = cos( rad );
77 #endif
78 }
79
80 /*****************************************************************************
81 * Truncate a degree.
82 *****************************************************************************/
83 INLINE Degree
TruncateDeg(Degree deg)84 TruncateDeg( Degree deg )
85 {
86 if ( EX(deg < 360.0) )
87 return deg;
88 else
89 return deg - 360.0;
90 }
91
92 /*****************************************************************************
93 * Convert a (360) degree into a radian.
94 * 360' = 2 * PI
95 *****************************************************************************/
96 INTERN const fpx degradRatio = (1.0 / 360.0) * (math::PI2);
97 #define DEG2RAD_2006( DEG ) ((DEG) * (degradRatio))
98 INLINE Radian
deg2rad(Degree deg)99 deg2rad( Degree deg )
100 {
101 return DEG2RAD_2006( deg );
102 //return deg / 360.0 * (2.0 * math::PI); // x87 FDIV is slower than FMUL
103 }
104
105 /*****************************************************************************
106 * Convert a radian into a (360) degree.
107 * 360' = 2 * PI
108 *****************************************************************************/
109 INTERN const fpx raddegRatio = (1.0 / math::PI2) * 360.0;
110 #define RAD2DEG_2006( RAD ) ((RAD) * (raddegRatio))
111 INLINE Degree
rad2deg(Radian rad)112 rad2deg( Radian rad )
113 {
114 return RAD2DEG_2006( rad );
115 //return rad / (2.0 * math::PI) * 360.0; // x87 FDIV is slower than FMUL
116 }
117
118 /*****************************************************************************
119 * 360' sin().
120 *****************************************************************************/
121 INLINE fpx
sin_deg(Degree deg)122 sin_deg( Degree deg )
123 {
124 return sin( deg2rad( deg ) );
125 }
126
127 /*****************************************************************************
128 * 360' cos().
129 *****************************************************************************/
130 INLINE fpx
cos_deg(Degree deg)131 cos_deg( Degree deg )
132 {
133 return cos( deg2rad( deg ) );
134 }
135
136 ////////////////////////////////////////////////////////////////////////////////
137 ///////////////////////////// M A T R I X ///////////////////////////////////
138 ////////////////////////////////////////////////////////////////////////////////
139
140 // A gfx matrix has 12 elements to minimize space (W elements aren't needed).
141 // A 16 element matrix remains a compile option.
142 #define MATRIX_ELEMS_2006 12
143 //#define MATRIX_ELEMS_2006 16
144
145 /// Matrix type.
146 #if DEBUG == 2
147 // For debugging, this class does bounds-checking.
148 template<typename FP>
149 struct MatrixTemplate
150 {
operator []gfx_2006::MatrixTemplate151 FP& operator[]( uint i ) { ASSERT( i < MATRIX_ELEMS_2006 ); return m[i]; }
operator []gfx_2006::MatrixTemplate152 const FP& operator[]( uint i ) const { ASSERT( i < MATRIX_ELEMS_2006 ); return m[i]; }
153 FP m[MATRIX_ELEMS_2006];
154 };
155 typedef MatrixTemplate<fp> Matrix;
156 typedef MatrixTemplate<fpx> MatrixExt;
157 #else
158 typedef fp Matrix[MATRIX_ELEMS_2006]; // Matrix : single-precision matrix
159 typedef fpx MatrixExt[MATRIX_ELEMS_2006]; // MatrixExt : extended-precision matrix
160 #endif
161
162 /// Row-major or column-major.
163 /// If row-major, to go to row i, multiply i by amount of columns.
164 #ifndef MATRIX_ROW_MAJOR_2006
165 #define MATRIX_ROW_MAJOR_2006 0
166 #endif
167
168 /// Matrix element offsets.
169 #if MATRIX_ELEMS_2006 == 12
170 #if MATRIX_ROW_MAJOR_2006
171 enum { Xx, Xy, Xz, Ox,
172 Yx, Yy, Yz, Oy,
173 Zx, Zy, Zz, Oz };
174 #else
175 enum { Xx, Yx, Zx,
176 Xy, Yy, Zy,
177 Xz, Yz, Zz,
178 Ox, Oy, Oz };
179 #endif // MATRIX_ROW_MAJOR_2006
180 #elif MATRIX_ELEMS_2006 == 16
181 #if MATRIX_ROW_MAJOR_2006
182 enum { Xx, Xy, Xz, Ox,
183 Yx, Yy, Yz, Oy,
184 Zx, Zy, Zz, Oz,
185 Xw, Yw, Zw, Ow }; // W elements are strongly deprecated
186 #else
187 enum { Xx, Yx, Zx, Xw,
188 Xy, Yy, Zy, Yw,
189 Xz, Yz, Zz, Zw,
190 Ox, Oy, Oz, Ow };
191 #endif // MATRIX_ROW_MAJOR_2006
192 #endif
193
194 /// Macros to access matrix elements (depends on row/column major order).
195 /// These macros should be used instead of writing hardcoded index computations.
196 #if MATRIX_ELEMS_2006 == 12
197 #if MATRIX_ROW_MAJOR_2006
198 # define MIX( I, J ) ( (I)*4 + (J) )
199 #else
200 # define MIX( I, J ) ( (J)*3 + (I) )
201 #endif
202 #if MATRIX_ROW_MAJOR_2006
203 # define MIXR( I, J ) ( (J)*4 + (I) )
204 #else
205 # define MIXR( I, J ) ( (I)*3 + (J) )
206 #endif
207 #elif MATRIX_ELEMS_2006 == 16
208 #if MATRIX_ROW_MAJOR_2006
209 # define MIX( I, J ) ( (I)*4 + (J) )
210 #else
211 # define MIX( I, J ) ( (J)*4 + (I) )
212 #endif
213 #if MATRIX_ROW_MAJOR_2006
214 # define MIXR( I, J ) ( (J)*4 + (I) )
215 #else
216 # define MIXR( I, J ) ( (I)*4 + (J) )
217 #endif
218 #endif
219
220 // It served its purpose.
221 #undef MATRIX_ELEMS_2006
222
223 /*****************************************************************************
224 * Load matrix with identity mapping.
225 *****************************************************************************/
226 template<typename MATRIX>
227 void
IdentityMatrix(MATRIX & m)228 IdentityMatrix( MATRIX& m /*OUT*/ )
229 {
230 m[Xx] = 1.0; m[Xy] = 0.0; m[Xz] = 0.0;
231 m[Yx] = 0.0; m[Yy] = 1.0; m[Yz] = 0.0;
232 m[Zx] = 0.0; m[Zy] = 0.0; m[Zz] = 1.0;
233 m[Ox] = 0.0; m[Oy] = 0.0; m[Oz] = 0.0;
234 }
235
236 /*****************************************************************************
237 * Copy matrix.
238 *****************************************************************************/
239 template<typename MATRIX_DEST, typename MATRIX_SRC>
240 void
CopyMatrix(MATRIX_DEST & dest,const MATRIX_SRC & src)241 CopyMatrix( MATRIX_DEST& dest, const MATRIX_SRC& src )
242 {
243 dest[Xx] = src[Xx]; dest[Xy] = src[Xy]; dest[Xz] = src[Xz];
244 dest[Yx] = src[Yx]; dest[Yy] = src[Yy]; dest[Yz] = src[Yz];
245 dest[Zx] = src[Zx]; dest[Zy] = src[Zy]; dest[Zz] = src[Zz];
246 dest[Ox] = src[Ox]; dest[Oy] = src[Oy]; dest[Oz] = src[Oz];
247 }
248
249 /*****************************************************************************
250 * Multiply two matrixs.
251 *
252 * @verbatim
253 *
254 * Matrix multiplication can be used to combine matrix transformations.
255 *
256 * For example, the two transformations of a local vertex to an eye vertex:
257 * vl * wm[] = vw // local --> world
258 * vw * em[] = ve // world --> eye
259 *
260 * Is mathematically equivalent to:
261 * vl * wm[] * em[] = ve
262 *
263 * Combining the two matrixs into one using multiplication:
264 * wm[] * em[] = wem[]
265 *
266 * Allows a faster single vertex transformation:
267 * vl * wem[] = ve
268 *
269 * @endverbatim
270 *****************************************************************************/
271 #if 0
272 // Reference function:
273 template<typename FPP,typename FPM,typename FPN>
274 void
275 MultiplyMatrix( FPP p[16] /*OUT*/, const FPM m[16], const FPN n[16] )
276 {
277 for ( uint i = 0; i < 4; ++i )
278 for ( uint j = 0; j < 4; ++j )
279 {
280 FPP e = 0.0;
281 for ( uint k = 0; k < 4; ++k )
282 {
283 #if MATRIX_ROW_MAJOR_2006
284 e += m[k*4+j] * n[i*4+k];
285 #else
286 e += m[i*4+k] * n[k*4+j];
287 #endif
288 }
289 p[i*4+j] = e;
290 }
291 }
292 #endif // 0
293
294 // Actual optimized function:
295 template<typename MATRIX_P, typename MATRIX_M, typename MATRIX_N>
296 void
MultiplyMatrix(MATRIX_P & p,const MATRIX_M & m,const MATRIX_N & n)297 MultiplyMatrix( MATRIX_P& p /*OUT*/, const MATRIX_M& m, const MATRIX_N& n )
298 {
299 // Optimizations:
300 // Calculating Xw,Yw,Zw,Ow (0,0,0,1) is bypassed as they never change.
301 // Xw,Yw,Zw are always 0 so multiplying one factor is bypassed.
302 // Ow is always 1 so Oxyz coefficients are added directly without multiplying it by 1.
303 // Inefficiencies:
304 // Some factors are re-multiplied.
305
306 #define MULTIPLY_MATRIX_ELEM3_2006( i, a0,a1, b0,b1, c0,c1 ) \
307 p[i] = m[a0] * n[a1] \
308 + m[b0] * n[b1] \
309 + m[c0] * n[c1];
310
311 #define MULTIPLY_MATRIX_ELEM4_2006( i, a0,a1, b0,b1, c0,c1, oi ) \
312 p[i] = m[a0] * n[a1] \
313 + m[b0] * n[b1] \
314 + m[c0] * n[c1] \
315 + n[oi];
316
317 #if MATRIX_ROW_MAJOR_2006
318 MULTIPLY_MATRIX_ELEM3_2006( Xx, Xx,Xx, Yx,Xy, Zx,Xz /*, Xw,Ox */ );
319 MULTIPLY_MATRIX_ELEM3_2006( Xy, Xy,Xx, Yy,Xy, Zy,Xz /*, Yw,Ox */ );
320 MULTIPLY_MATRIX_ELEM3_2006( Xz, Xz,Xx, Yz,Xy, Zz,Xz /*, Zw,Ox */ );
321 MULTIPLY_MATRIX_ELEM4_2006( Ox, Ox,Xx, Oy,Xy, Oz,Xz /*, Ow,Ox */ , Ox );
322 MULTIPLY_MATRIX_ELEM3_2006( Yx, Xx,Yx, Yx,Yy, Zx,Yz /*, Xw,Oy */ );
323 MULTIPLY_MATRIX_ELEM3_2006( Yy, Xy,Yx, Yy,Yy, Zy,Yz /*, Yw,Oy */ );
324 MULTIPLY_MATRIX_ELEM3_2006( Yz, Xz,Yx, Yz,Yy, Zz,Yz /*, Zw,Oy */ );
325 MULTIPLY_MATRIX_ELEM4_2006( Oy, Ox,Yx, Oy,Yy, Oz,Yz /*, Ow,Oy */ , Oy );
326 MULTIPLY_MATRIX_ELEM3_2006( Zx, Xx,Zx, Yx,Zy, Zx,Zz /*, Xw,Oz */ );
327 MULTIPLY_MATRIX_ELEM3_2006( Zy, Xy,Zx, Yy,Zy, Zy,Zz /*, Yw,Oz */ );
328 MULTIPLY_MATRIX_ELEM3_2006( Zz, Xz,Zx, Yz,Zy, Zz,Zz /*, Zw,Oz */ );
329 MULTIPLY_MATRIX_ELEM4_2006( Oz, Ox,Zx, Oy,Zy, Oz,Zz /*, Ow,Oz */ , Oz );
330 #else
331 MULTIPLY_MATRIX_ELEM3_2006( Xx, Xx,Xx, Yx,Xy, Zx,Xz /*, Xw,Ox */ );
332 MULTIPLY_MATRIX_ELEM3_2006( Yx, Xx,Yx, Yx,Yy, Zx,Yz /*, Xw,Oy */ );
333 MULTIPLY_MATRIX_ELEM3_2006( Zx, Xx,Zx, Yx,Zy, Zx,Zz /*, Xw,Oz */ );
334 MULTIPLY_MATRIX_ELEM3_2006( Xy, Xy,Xx, Yy,Xy, Zy,Xz /*, Yw,Ox */ );
335 MULTIPLY_MATRIX_ELEM3_2006( Yy, Xy,Yx, Yy,Yy, Zy,Yz /*, Yw,Oy */ );
336 MULTIPLY_MATRIX_ELEM3_2006( Zy, Xy,Zx, Yy,Zy, Zy,Zz /*, Yw,Oz */ );
337 MULTIPLY_MATRIX_ELEM3_2006( Xz, Xz,Xx, Yz,Xy, Zz,Xz /*, Zw,Ox */ );
338 MULTIPLY_MATRIX_ELEM3_2006( Yz, Xz,Yx, Yz,Yy, Zz,Yz /*, Zw,Oy */ );
339 MULTIPLY_MATRIX_ELEM3_2006( Zz, Xz,Zx, Yz,Zy, Zz,Zz /*, Zw,Oz */ );
340 MULTIPLY_MATRIX_ELEM4_2006( Ox, Ox,Xx, Oy,Xy, Oz,Xz /*, Ow,Ox */ , Ox );
341 MULTIPLY_MATRIX_ELEM4_2006( Oy, Ox,Yx, Oy,Yy, Oz,Yz /*, Ow,Oy */ , Oy );
342 MULTIPLY_MATRIX_ELEM4_2006( Oz, Ox,Zx, Oy,Zy, Oz,Zz /*, Ow,Oz */ , Oz );
343 #endif
344 }
345
346 /*****************************************************************************
347 * Transpose a matrix.
348 * Reverses the mapping between two coordinate systems.
349 *****************************************************************************/
350 template<typename MATRIX_DEST, typename MATRIX_SRC>
351 void
TransposeMatrix(MATRIX_DEST & dest,const MATRIX_SRC & src)352 TransposeMatrix( MATRIX_DEST& dest, const MATRIX_SRC& src )
353 {
354 // Transpose rotation sub-matrix.
355 // X = X'
356 dest[Xx] = src[Xx];
357 dest[Xy] = src[Yx];
358 dest[Xz] = src[Zx];
359 // Y = Y'
360 dest[Yx] = src[Xy];
361 dest[Yy] = src[Yy];
362 dest[Yz] = src[Zy];
363 // Z = Z'
364 dest[Zx] = src[Xz];
365 dest[Zy] = src[Yz];
366 dest[Zz] = src[Zz];
367
368 // Negate origin sub-matrix.
369 dest[Ox] = -src[Ox];
370 dest[Oy] = -src[Oy];
371 dest[Oz] = -src[Oz];
372 }
373
374 /*****************************************************************************
375 * Rotate coordinates.
376 *****************************************************************************/
377 template<typename FP, typename MATRIX>
378 FP
379 RotX( FP x, FP y, FP z, const MATRIX& m )
380 {
381 return x * m[Xx] + y * m[Xy] + z * m[Xz];
382 }
383 template<typename FP, typename MATRIX>
384 FP
385 RotY( FP x, FP y, FP z, const MATRIX& m )
386 {
387 return x * m[Yx] + y * m[Yy] + z * m[Yz];
388 }
389 template<typename FP, typename MATRIX>
390 FP
391 RotZ( FP x, FP y, FP z, const MATRIX& m )
392 {
393 return x * m[Zx] + y * m[Zy] + z * m[Zz];
394 }
395
396 /*****************************************************************************
397 * Rotate/translate coordinates.
398 *****************************************************************************/
399 template<typename FP, typename MATRIX>
400 FP
401 RotTransX( FP x, FP y, FP z, const MATRIX& m )
402 {
403 return x * m[Xx] + y * m[Xy] + z * m[Xz] + m[Ox];
404 }
405 template<typename FP, typename MATRIX>
406 FP
407 RotTransY( FP x, FP y, FP z, const MATRIX& m )
408 {
409 return x * m[Yx] + y * m[Yy] + z * m[Yz] + m[Oy];
410 }
411 template<typename FP, typename MATRIX>
412 FP
413 RotTransZ( FP x, FP y, FP z, const MATRIX& m )
414 {
415 return x * m[Zx] + y * m[Zy] + z * m[Zz] + m[Oz];
416 }
417
418 /*****************************************************************************
419 * Rotate/translate vertexs.
420 *****************************************************************************/
421 template<typename FP, typename MATRIX>
422 void
RotTrans(FP & x2,FP & y2,FP & z2,const FP x,const FP y,const FP z,const MATRIX & m)423 RotTrans( FP& x2, FP& y2, FP& z2, const FP x, const FP y, const FP z, const MATRIX& m )
424 {
425 x2 = x * m[Xx] + y * m[Xy] + z * m[Xz] + m[Ox];
426 y2 = x * m[Yx] + y * m[Yy] + z * m[Yz] + m[Oy];
427 z2 = x * m[Zx] + y * m[Zy] + z * m[Zz] + m[Oz];
428 }
429 template<typename FP, typename MATRIX>
430 void
RotTrans(WorldVertex & v2,const FP x,const FP y,const FP z,const MATRIX & m)431 RotTrans( WorldVertex& v2, const FP x, const FP y, const FP z, const MATRIX& m )
432 {
433 v2.x = x * m[Xx] + y * m[Xy] + z * m[Xz] + m[Ox];
434 v2.y = x * m[Yx] + y * m[Yy] + z * m[Yz] + m[Oy];
435 v2.z = x * m[Zx] + y * m[Zy] + z * m[Zz] + m[Oz];
436 }
437 template<typename MATRIX>
438 void
RotTrans(WorldVertex & v2,const LocalVertex & v,const MATRIX & m)439 RotTrans( WorldVertex& v2, const LocalVertex& v, const MATRIX& m )
440 {
441 v2.x = v.x * m[Xx] + v.y * m[Xy] + v.z * m[Xz] + m[Ox];
442 v2.y = v.x * m[Yx] + v.y * m[Yy] + v.z * m[Yz] + m[Oy];
443 v2.z = v.x * m[Zx] + v.y * m[Zy] + v.z * m[Zz] + m[Oz];
444 }
445 template<typename MATRIX>
446 void
RotTrans(EyeVertex & v2,const LocalVertex & v,const MATRIX & m)447 RotTrans( EyeVertex& v2, const LocalVertex& v, const MATRIX& m )
448 {
449 v2.x = v.x * m[Xx] + v.y * m[Xy] + v.z * m[Xz] + m[Ox];
450 v2.y = v.x * m[Yx] + v.y * m[Yy] + v.z * m[Yz] + m[Oy];
451 v2.z = v.x * m[Zx] + v.y * m[Zy] + v.z * m[Zz] + m[Oz];
452 }
453
454 /*****************************************************************************
455 * Transform a 3D vertex thru one coordinate system.
456 * More specialized variants are at eng_class_math.hh.
457 *****************************************************************************/
458 template<typename MATRIX>
459 void
TransformVertex(EyeVertex & v2,const WorldVertex & v,const MATRIX & m)460 TransformVertex( EyeVertex& v2, const WorldVertex& v, const MATRIX& m )
461 {
462 fp x = v.x + m[Ox];
463 fp y = v.y + m[Oy];
464 fp z = v.z + m[Oz];
465 v2.x = x * m[Xx] + y * m[Xy] + z * m[Xz];
466 v2.y = x * m[Yx] + y * m[Yy] + z * m[Yz];
467 v2.z = x * m[Zx] + y * m[Zy] + z * m[Zz];
468 }
469
470 /*****************************************************************************
471 * Calling this variant requires specifying template arg types:
472 * TransformVertex<EyeVertex,WorldVertex,Matrix>();
473 *****************************************************************************/
474 template<typename VERTEX_OUT, typename VERTEX_IN, typename MATRIX>
475 VERTEX_OUT
476 TransformVertex( const VERTEX_IN& v, const MATRIX& m )
477 {
478 fp x = v.x + m[Ox];
479 fp y = v.y + m[Oy];
480 fp z = v.z + m[Oz]; // uses C++ return-value optimization
481 return VERTEX_OUT( x * m[Xx] + y * m[Xy] + z * m[Xz], // = x
482 x * m[Yx] + y * m[Yy] + z * m[Yz], // = y
483 x * m[Zx] + y * m[Zy] + z * m[Zz] ); // = z
484 }
485
486 /*****************************************************************************
487 * Translate a matrix by row.
488 * For translating the eye/viewpoint.
489 *****************************************************************************/
490 template<typename FPM, typename MATRIX>
491 void
TranslateMatrixFixed(uint axis,FPM inc,MATRIX & m)492 TranslateMatrixFixed( uint axis, FPM inc, MATRIX& m /*OUT*/ )
493 {
494 m[Ox] += inc * m[ MIX(axis,0) ];
495 m[Oy] += inc * m[ MIX(axis,1) ];
496 m[Oz] += inc * m[ MIX(axis,2) ];
497 }
498
499 /*****************************************************************************
500 * Translate a matrix relative to itself (local translation).
501 * For translating an object.
502 *****************************************************************************/
503 template<typename FPM, typename MATRIX>
504 void
TranslateMatrixLocal(uint axis,FPM inc,MATRIX & m)505 TranslateMatrixLocal( uint axis, FPM inc, MATRIX& m /*OUT*/ )
506 {
507 m[Ox] += inc * m[ MIXR(axis,0) ];
508 m[Oy] += inc * m[ MIXR(axis,1) ];
509 m[Oz] += inc * m[ MIXR(axis,2) ];
510 }
511
512 /*****************************************************************************
513 * Rotate a matrix around a fixed axis.
514 * This function is suited to rotating the viewpoint/eye.
515 * The word "fixed" should be clear by looking at the math.
516 * Eg, the value of the X coord won't be changed by a rotation around the X axis.
517 *****************************************************************************/
518 // Radian/Degree are just typedefs (not distinct types).
519 template<typename MATRIX>
520 void
RotateMatrixFixedRadian(uint axis,Radian rad,MATRIX & m)521 RotateMatrixFixedRadian( uint axis, Radian rad, MATRIX& m /*OUT*/ )
522 {
523 #if SPEED
524 fp s, c; SinCos( rad, s, c );
525 #else
526 const fpx s = sin( rad );
527 const fpx c = cos( rad );
528 #endif
529 MATRIX n;
530 CopyMatrix( n, m ); // dest,src
531 switch ( axis )
532 {
533 // X : Pitch
534 case AXIS_X:
535 n[Yx] = m[Yx]*c - m[Zx]*s; // Y = Ycos - Zsin
536 n[Yy] = m[Yy]*c - m[Zy]*s;
537 n[Yz] = m[Yz]*c - m[Zz]*s;
538
539 n[Zx] = m[Yx]*s + m[Zx]*c; // Z = Ysin + Zcos
540 n[Zy] = m[Yy]*s + m[Zy]*c;
541 n[Zz] = m[Yz]*s + m[Zz]*c;
542 break;
543
544 // Y : Yaw
545 case AXIS_Y:
546 n[Xx] = m[Xx]*c - m[Zx]*s; // X = Xcos - Zsin
547 n[Xy] = m[Xy]*c - m[Zy]*s;
548 n[Xz] = m[Xz]*c - m[Zz]*s;
549
550 n[Zx] = m[Xx]*s + m[Zx]*c; // Z = Xsin + Zcos
551 n[Zy] = m[Xy]*s + m[Zy]*c;
552 n[Zz] = m[Xz]*s + m[Zz]*c;
553 break;
554
555 // Z : Roll
556 case AXIS_Z:
557 n[Xx] = m[Xx]*c - m[Yx]*s; // X = Xcos - Ysin
558 n[Xy] = m[Xy]*c - m[Yy]*s;
559 n[Xz] = m[Xz]*c - m[Yz]*s;
560
561 n[Yx] = m[Xx]*s + m[Yx]*c; // Y = Xsin + Ycos
562 n[Yy] = m[Xy]*s + m[Yy]*c;
563 n[Yz] = m[Xz]*s + m[Yz]*c;
564 break;
565
566 default: ASSERT(0); return;
567 }
568
569 CopyMatrix( m, n ); // dest,src
570 }
571 template<typename MATRIX>
572 void
RotateMatrixFixedDegree(uint axis,Degree deg,MATRIX & m)573 RotateMatrixFixedDegree( uint axis, Degree deg, MATRIX& m /*OUT*/ )
574 {
575 RotateMatrixFixedRadian( axis, deg2rad(deg), m );
576 }
577
578 /*****************************************************************************
579 * Rotate a local coordinate system around its own axis.
580 * This function is suited to rotating an independent object.
581 * The rotation is relative to local coodinate system (not the fixed/eye system).
582 * The trick is to load an identity-mapped matrix and rotate it, then transform
583 * it thru the given matrix (which defines the local coordinate system)
584 * as though it was the coords (1.0, 1.0, 1.0). These coords of course define
585 * the X,Y,Z axises. The transformation is effectively a local rotation.
586 *****************************************************************************/
587 // Radian/Degree are just typedefs (not distinct types).
588 template<typename MATRIX>
589 void
RotateMatrixLocalRadian(uint axis,Radian rad,MATRIX & m)590 RotateMatrixLocalRadian( uint axis, Radian rad, MATRIX& m /*OUT*/ )
591 {
592 // Identity (1:1) matrix r.
593 MATRIX r;
594 IdentityMatrix( r );
595
596 // Rotate matrix r.
597 RotateMatrixFixedRadian( axis, rad, r );
598
599 // Transform r thru m.
600 // Ie, thru matrix m, pass r as if it were the rotated coords (1.0, 1.0, 1.0).
601 MATRIX t;
602 t[Xx] = RotTransX( m[Xx], m[Xy], m[Xz], r );
603 t[Xy] = RotTransY( m[Xx], m[Xy], m[Xz], r );
604 t[Xz] = RotTransZ( m[Xx], m[Xy], m[Xz], r );
605
606 t[Yx] = RotTransX( m[Yx], m[Yy], m[Yz], r );
607 t[Yy] = RotTransY( m[Yx], m[Yy], m[Yz], r );
608 t[Yz] = RotTransZ( m[Yx], m[Yy], m[Yz], r );
609
610 t[Zx] = RotTransX( m[Zx], m[Zy], m[Zz], r );
611 t[Zy] = RotTransY( m[Zx], m[Zy], m[Zz], r );
612 t[Zz] = RotTransZ( m[Zx], m[Zy], m[Zz], r );
613
614 // m = r excluding origin elements.
615 m[Xx] = t[Xx];
616 m[Xy] = t[Xy];
617 m[Xz] = t[Xz];
618
619 m[Yx] = t[Yx];
620 m[Yy] = t[Yy];
621 m[Yz] = t[Yz];
622
623 m[Zx] = t[Zx];
624 m[Zy] = t[Zy];
625 m[Zz] = t[Zz];
626 }
627 template<typename MATRIX>
628 void
RotateMatrixLocalDegree(uint axis,Degree deg,MATRIX & m)629 RotateMatrixLocalDegree( uint axis, Degree deg, MATRIX& m /*OUT*/ )
630 {
631 RotateMatrixLocalRadian( axis, deg2rad(deg), m );
632 }
633
634 /*****************************************************************************
635 * Print matrix.
636 *****************************************************************************/
637 template<typename MATRIX>
638 void
PrintMatrix(const MATRIX & m)639 PrintMatrix( const MATRIX& m )
640 {
641 cout.setf( ios::fixed, ios::floatfield );
642 cout.precision( 6 );
643 cout << "Xxyzw: " << m[Xx] << " " << m[Xy] << " " << m[Xz] << std::endl
644 << "Yxyzw: " << m[Yx] << " " << m[Yy] << " " << m[Yz] << std::endl
645 << "Zxyzw: " << m[Zx] << " " << m[Zy] << " " << m[Zz] << std::endl
646 << "Oxyzw: " << m[Ox] << " " << m[Oy] << " " << m[Oz] << std::endl;
647 }
648 template<typename MATRIX>
649 void
PrintMatrixOrigin(const MATRIX & m)650 PrintMatrixOrigin( const MATRIX& m )
651 {
652 cout.setf( ios::fixed, ios::floatfield );
653 cout.precision( 6 );
654 cout << m[Ox] << ' ' << m[Oy] << ' ' << m[Oz] << std::endl;
655 }
656
657 ////////////////////////////////////////////////////////////////////////////////
658 ////////////////////////////// V E C T O R //////////////////////////////////
659 ////////////////////////////////////////////////////////////////////////////////
660
661 /*****************************************************************************
662 * Distance between vector and implied zero origin.
663 *****************************************************************************/
664 INLINE fp
Distance(const Vector2 & v)665 Distance( const Vector2& v )
666 {
667 return sqrtf( v.x * v.x
668 + v.y * v.y );
669 }
670 INLINE fp
Distance(const Vector3 & v)671 Distance( const Vector3& v )
672 {
673 return sqrtf( v.x * v.x
674 + v.y * v.y
675 + v.z * v.z );
676 }
677
678 /*****************************************************************************
679 * Distance between two vectors -- DistanceSquared() is faster than this.
680 *****************************************************************************/
681 template<typename VECTOR> // 2D/3D
682 fp
Distance(const VECTOR & v1,const VECTOR & v2)683 Distance( const VECTOR& v1, const VECTOR& v2 )
684 {
685 VECTOR delta = v2 - v1;
686 return gfx_2006::Distance( delta );
687 }
688
689 /*****************************************************************************
690 * Calculate the distance^2 (squared) between two vectors.
691 * DISTANCE RETURNED IS SQUARED (distance^2).
692 * SQUARE ROOT OF DISTANCE IS SKIPPED FOR SPEED.
693 * sqrt() and the underlying FPU instruction is very slow.
694 *****************************************************************************/
695 INLINE fpx
DistanceSquared(const Vector3 & v1,const Vector3 & v2)696 DistanceSquared( const Vector3& v1, const Vector3& v2 )
697 {
698 fpx xd = v2.x - v1.x; // extra precision
699 fpx yd = v2.y - v1.y;
700 fpx zd = v2.z - v1.z;
701 return xd*xd + yd*yd + zd*zd;
702 }
703
704 // Distance^2 between two matrixs.
705 // Don't pass different kinds of matrixs (eg Eye origin is negative but Dyna is positive).
706 template<typename MATRIX1, typename MATRIX2>
707 fpx
DistanceSquaredMatrix(const MATRIX1 & m1,const MATRIX2 & m2)708 DistanceSquaredMatrix( const MATRIX1& m1, const MATRIX2& m2 )
709 {
710 Vector3 v1( m1[Ox], m1[Oy], m1[Oz] );
711 Vector3 v2( m2[Ox], m2[Oy], m2[Oz] );
712 return gfx_2006::DistanceSquared( v1, v2 );
713 }
714
715 /*****************************************************************************
716 * @return A normalized vector (unit-length 1.0) of the same vector.
717 * Not to be confused with calculating the normal vector of two vectors (CrossProduct).
718 *****************************************************************************/
719 INLINE Vector3
Normalize(const Vector3 & v)720 Normalize( const Vector3& v )
721 {
722 fp dist = gfx_2006::Distance( v );
723 return Vector3( v.x / dist,
724 v.y / dist,
725 v.z / dist );
726 }
727
728 /*****************************************************************************
729 * Calculate a cross product of two vectors which yields a normal vector
730 * that is perpendicular to the plane containing the two source vectors.
731 * Note: The cross product won't be unit length.
732 * c = a x b
733 *****************************************************************************/
734 INLINE void
CrossProduct(NormalVertex & n,const Vector3 & v1,const Vector3 & v2,const Vector3 & v3)735 CrossProduct( NormalVertex& n, /*OUT*/
736 const Vector3& v1, /* origin of normal vector */
737 const Vector3& v2,
738 const Vector3& v3 )
739 {
740 fp a, b, c;
741 fp d, e, f;
742
743 a = v2.x - v1.x;
744 b = v2.y - v1.y;
745 c = v2.z - v1.z;
746
747 d = v3.x - v1.x;
748 e = v3.y - v1.y;
749 f = v3.z - v1.z;
750
751 n.x = b*f - c*e;
752 n.y = c*d - a*f;
753 n.z = a*e - b*d;
754 }
755
756 /*****************************************************************************
757 * Calculate the dot product of two vectors which yields a scalar.
758 * If the angle between the two vectors is between 0 and 90 degrees,
759 * the dot product will be positive, else negative.
760 *****************************************************************************/
761 INLINE fp
DotProduct(const Vector2 & v1,const Vector2 & v2)762 DotProduct( const Vector2& v1, const Vector2& v2 )
763 {
764 return v1.x * v2.x
765 + v1.y * v2.y;
766 }
767 INLINE fp
DotProduct(const Vector3 & v1,const Vector3 & v2)768 DotProduct( const Vector3& v1, const Vector3& v2 )
769 {
770 return v1.x * v2.x
771 + v1.y * v2.y
772 + v1.z * v2.z;
773 }
774
775 /*****************************************************************************
776 * Calculate angle (in radians) between two vectors sharing the same origin.
777 *****************************************************************************/
778 template<typename VECTOR> // 2D/3D template
779 Radian
Angle(const VECTOR & u,const VECTOR & v)780 Angle( const VECTOR& u, const VECTOR& v )
781 {
782 // Dot product formula indirectly provides the angle.
783 // angle = arccos( DotProduct(u,v) / Distance(u) * Distance(v) )
784 fp uv = gfx_2006::Distance(u) * gfx_2006::Distance(v);
785 if ( ABS(uv) < 0.1 ) return 0.0; // prevent NaN and div by 0
786 fp theta = DotProduct(u,v) / uv;
787 theta = Clamp( theta, -1.0, 1.0 ); // acos() won't tolerate 1.00000001
788 DEBUG_CODE( errno = 0; )
789 fp rad = acos( theta );
790 ASSERT(errno == 0); // if acos() failed
791 return rad;
792 }
793
794 /*****************************************************************************
795 * Interpolate between two vectors.
796 * @param v1, v2
797 * @param frac
798 * Fraction in range {0,..,1.0}.
799 * If fraction is 0.0, v1 is returned.
800 * If fraction is 1.0, v2 is returned.
801 * If fraction is 0.5, midpoint is returned.
802 * @return Interpolated 3D point.
803 *****************************************************************************/
804 INLINE Vector3
Interpolate(const Vector3 & v1,const Vector3 & v2,const fp fraction)805 Interpolate( const Vector3& v1, const Vector3& v2, const fp fraction )
806 {
807 ASSERT( fraction >= 0.0 && fraction <= 1.0 );
808 return Vector3( v1 + ((v2-v1) * fraction) );
809 }
810
811 ////////////////////////////////////////////////////////////////////////////////
812 //////////////////////////// M I S C M A T H //////////////////////////////
813 ////////////////////////////////////////////////////////////////////////////////
814
815 /*****************************************************************************
816 * x^2
817 *****************************************************************************/
818 template<typename T>
819 T
SQUARE(T x)820 SQUARE( T x )
821 {
822 return x * x;
823 }
824
825 /*****************************************************************************
826 * Average.
827 *****************************************************************************/
828 template<typename T>
829 T
AVG(T x,T y)830 AVG( T x, T y )
831 {
832 return (x + y) / 2;
833 }
834
835 /*****************************************************************************
836 * Force a number to be within a range.
837 *****************************************************************************/
838 template<typename T>
839 T
Range(T val,T lo,T hi)840 Range( T val, T lo, T hi )
841 {
842 if ( val < lo ) return lo;
843 if ( val > hi ) return hi;
844 return val;
845 }
846
847 /*****************************************************************************
848 * Map a number from one range into another.
849 *****************************************************************************/
850 INLINE fp
Remap(fp value,fp rangeOld,fp rangeNew)851 Remap( fp value, fp rangeOld, fp rangeNew )
852 {
853 return value/rangeOld*rangeNew;
854 }
855
856 /*****************************************************************************
857 * Truncate a float at an interval.
858 * Eg, Truncate(24,10) returns 20.
859 *****************************************************************************/
860 INLINE fp
Truncate(fp val,fp interval)861 Truncate( fp val, fp interval )
862 {
863 float integral; // not fp as address is passed to modff()
864 modff( val/interval, &integral );
865 return integral*interval;
866 }
867
868 /*****************************************************************************
869 * Normalize a 3D point (unit 1 distance) then return one normalized coordinate.
870 * The return value typically is passed to asin().
871 *****************************************************************************/
872 INLINE fp
NormalizeCoord(uint axis,const Vector3 & v)873 NormalizeCoord( uint axis, const Vector3& v )
874 {
875 fp dist = gfx_2006::Distance( v );
876 #if 0
877 return v[axis] / dist;
878 #else // 2007
879 if ( axis == AXIS_X )
880 return v.x / dist;
881 else if ( axis == AXIS_Y )
882 return v.y / dist;
883 else if ( axis == AXIS_Z )
884 return v.z / dist;
885 else { ASSERT(0); }
886 #endif
887 }
888
889 } // namespace gfx_2006
890
891 #endif // MATH_MATRIX_2006_HH
892