1 /*
2 ===========================================================================
3
4 Doom 3 GPL Source Code
5 Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.
6
7 This file is part of the Doom 3 GPL Source Code ("Doom 3 Source Code").
8
9 Doom 3 Source Code is free software: you can redistribute it and/or modify
10 it under the terms of the GNU General Public License as published by
11 the Free Software Foundation, either version 3 of the License, or
12 (at your option) any later version.
13
14 Doom 3 Source Code is distributed in the hope that it will be useful,
15 but WITHOUT ANY WARRANTY; without even the implied warranty of
16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 GNU General Public License for more details.
18
19 You should have received a copy of the GNU General Public License
20 along with Doom 3 Source Code. If not, see <http://www.gnu.org/licenses/>.
21
22 In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code. If not, please request a copy in writing from id Software at the address below.
23
24 If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
25
26 ===========================================================================
27 */
28
29 #ifndef __MATH_VECTOR_H__
30 #define __MATH_VECTOR_H__
31
32 #include "idlib/math/Math.h"
33 #include "idlib/math/Random.h"
34 #include "idlib/math/Simd.h"
35 #include "idlib/Heap.h"
36
37 /*
38 ===============================================================================
39
40 Vector classes
41
42 ===============================================================================
43 */
44
45 #define VECTOR_EPSILON 0.001f
46
47 class idAngles;
48 class idPolar3;
49 class idMat3;
50
51 //===============================================================
52 //
53 // idVec2 - 2D vector
54 //
55 //===============================================================
56
57 class idVec2 {
58 public:
59 float x;
60 float y;
61
62 idVec2( void );
63 explicit idVec2( const float x, const float y );
64
65 void Set( const float x, const float y );
66 void Zero( void );
67
68 float operator[]( int index ) const;
69 float & operator[]( int index );
70 idVec2 operator-() const;
71 float operator*( const idVec2 &a ) const;
72 idVec2 operator*( const float a ) const;
73 idVec2 operator/( const float a ) const;
74 idVec2 operator+( const idVec2 &a ) const;
75 idVec2 operator-( const idVec2 &a ) const;
76 idVec2 & operator+=( const idVec2 &a );
77 idVec2 & operator-=( const idVec2 &a );
78 idVec2 & operator/=( const idVec2 &a );
79 idVec2 & operator/=( const float a );
80 idVec2 & operator*=( const float a );
81
82 friend idVec2 operator*( const float a, const idVec2 b );
83
84 bool Compare( const idVec2 &a ) const; // exact compare, no epsilon
85 bool Compare( const idVec2 &a, const float epsilon ) const; // compare with epsilon
86 bool operator==( const idVec2 &a ) const; // exact compare, no epsilon
87 bool operator!=( const idVec2 &a ) const; // exact compare, no epsilon
88
89 float Length( void ) const;
90 float LengthFast( void ) const;
91 float LengthSqr( void ) const;
92 float Normalize( void ); // returns length
93 float NormalizeFast( void ); // returns length
94 idVec2 & Truncate( float length ); // cap length
95 void Clamp( const idVec2 &min, const idVec2 &max );
96 void Snap( void ); // snap to closest integer value
97 void SnapInt( void ); // snap towards integer (floor)
98
99 int GetDimension( void ) const;
100
101 const float * ToFloatPtr( void ) const;
102 float * ToFloatPtr( void );
103 const char * ToString( int precision = 2 ) const;
104
105 void Lerp( const idVec2 &v1, const idVec2 &v2, const float l );
106 };
107
108 extern idVec2 vec2_origin;
109 #define vec2_zero vec2_origin
110
idVec2(void)111 ID_INLINE idVec2::idVec2( void ) {
112 }
113
idVec2(const float x,const float y)114 ID_INLINE idVec2::idVec2( const float x, const float y ) {
115 this->x = x;
116 this->y = y;
117 }
118
Set(const float x,const float y)119 ID_INLINE void idVec2::Set( const float x, const float y ) {
120 this->x = x;
121 this->y = y;
122 }
123
Zero(void)124 ID_INLINE void idVec2::Zero( void ) {
125 x = y = 0.0f;
126 }
127
Compare(const idVec2 & a)128 ID_INLINE bool idVec2::Compare( const idVec2 &a ) const {
129 return ( ( x == a.x ) && ( y == a.y ) );
130 }
131
Compare(const idVec2 & a,const float epsilon)132 ID_INLINE bool idVec2::Compare( const idVec2 &a, const float epsilon ) const {
133 if ( idMath::Fabs( x - a.x ) > epsilon ) {
134 return false;
135 }
136
137 if ( idMath::Fabs( y - a.y ) > epsilon ) {
138 return false;
139 }
140
141 return true;
142 }
143
144 ID_INLINE bool idVec2::operator==( const idVec2 &a ) const {
145 return Compare( a );
146 }
147
148 ID_INLINE bool idVec2::operator!=( const idVec2 &a ) const {
149 return !Compare( a );
150 }
151
152 ID_INLINE float idVec2::operator[]( int index ) const {
153 return ( &x )[ index ];
154 }
155
156 ID_INLINE float& idVec2::operator[]( int index ) {
157 return ( &x )[ index ];
158 }
159
Length(void)160 ID_INLINE float idVec2::Length( void ) const {
161 return ( float )idMath::Sqrt( x * x + y * y );
162 }
163
LengthFast(void)164 ID_INLINE float idVec2::LengthFast( void ) const {
165 float sqrLength;
166
167 sqrLength = x * x + y * y;
168 return sqrLength * idMath::RSqrt( sqrLength );
169 }
170
LengthSqr(void)171 ID_INLINE float idVec2::LengthSqr( void ) const {
172 return ( x * x + y * y );
173 }
174
Normalize(void)175 ID_INLINE float idVec2::Normalize( void ) {
176 float sqrLength, invLength;
177
178 sqrLength = x * x + y * y;
179 invLength = idMath::InvSqrt( sqrLength );
180 x *= invLength;
181 y *= invLength;
182 return invLength * sqrLength;
183 }
184
NormalizeFast(void)185 ID_INLINE float idVec2::NormalizeFast( void ) {
186 float lengthSqr, invLength;
187
188 lengthSqr = x * x + y * y;
189 invLength = idMath::RSqrt( lengthSqr );
190 x *= invLength;
191 y *= invLength;
192 return invLength * lengthSqr;
193 }
194
Truncate(float length)195 ID_INLINE idVec2 &idVec2::Truncate( float length ) {
196 float length2;
197 float ilength;
198
199 if ( !length ) {
200 Zero();
201 }
202 else {
203 length2 = LengthSqr();
204 if ( length2 > length * length ) {
205 ilength = length * idMath::InvSqrt( length2 );
206 x *= ilength;
207 y *= ilength;
208 }
209 }
210
211 return *this;
212 }
213
Clamp(const idVec2 & min,const idVec2 & max)214 ID_INLINE void idVec2::Clamp( const idVec2 &min, const idVec2 &max ) {
215 if ( x < min.x ) {
216 x = min.x;
217 } else if ( x > max.x ) {
218 x = max.x;
219 }
220 if ( y < min.y ) {
221 y = min.y;
222 } else if ( y > max.y ) {
223 y = max.y;
224 }
225 }
226
Snap(void)227 ID_INLINE void idVec2::Snap( void ) {
228 x = floor( x + 0.5f );
229 y = floor( y + 0.5f );
230 }
231
SnapInt(void)232 ID_INLINE void idVec2::SnapInt( void ) {
233 x = float( int( x ) );
234 y = float( int( y ) );
235 }
236
237 ID_INLINE idVec2 idVec2::operator-() const {
238 return idVec2( -x, -y );
239 }
240
241 ID_INLINE idVec2 idVec2::operator-( const idVec2 &a ) const {
242 return idVec2( x - a.x, y - a.y );
243 }
244
245 ID_INLINE float idVec2::operator*( const idVec2 &a ) const {
246 return x * a.x + y * a.y;
247 }
248
249 ID_INLINE idVec2 idVec2::operator*( const float a ) const {
250 return idVec2( x * a, y * a );
251 }
252
253 ID_INLINE idVec2 idVec2::operator/( const float a ) const {
254 float inva = 1.0f / a;
255 return idVec2( x * inva, y * inva );
256 }
257
258 ID_INLINE idVec2 operator*( const float a, const idVec2 b ) {
259 return idVec2( b.x * a, b.y * a );
260 }
261
262 ID_INLINE idVec2 idVec2::operator+( const idVec2 &a ) const {
263 return idVec2( x + a.x, y + a.y );
264 }
265
266 ID_INLINE idVec2 &idVec2::operator+=( const idVec2 &a ) {
267 x += a.x;
268 y += a.y;
269
270 return *this;
271 }
272
273 ID_INLINE idVec2 &idVec2::operator/=( const idVec2 &a ) {
274 x /= a.x;
275 y /= a.y;
276
277 return *this;
278 }
279
280 ID_INLINE idVec2 &idVec2::operator/=( const float a ) {
281 float inva = 1.0f / a;
282 x *= inva;
283 y *= inva;
284
285 return *this;
286 }
287
288 ID_INLINE idVec2 &idVec2::operator-=( const idVec2 &a ) {
289 x -= a.x;
290 y -= a.y;
291
292 return *this;
293 }
294
295 ID_INLINE idVec2 &idVec2::operator*=( const float a ) {
296 x *= a;
297 y *= a;
298
299 return *this;
300 }
301
GetDimension(void)302 ID_INLINE int idVec2::GetDimension( void ) const {
303 return 2;
304 }
305
ToFloatPtr(void)306 ID_INLINE const float *idVec2::ToFloatPtr( void ) const {
307 return &x;
308 }
309
ToFloatPtr(void)310 ID_INLINE float *idVec2::ToFloatPtr( void ) {
311 return &x;
312 }
313
314
315 //===============================================================
316 //
317 // idVec3 - 3D vector
318 //
319 //===============================================================
320
321 class idVec3 {
322 public:
323 float x;
324 float y;
325 float z;
326
327 idVec3( void );
328 explicit idVec3( const float x, const float y, const float z );
329
330 void Set( const float x, const float y, const float z );
331 void Zero( void );
332
333 float operator[]( const int index ) const;
334 float & operator[]( const int index );
335 idVec3 operator-() const;
336 idVec3 & operator=( const idVec3 &a ); // required because of a msvc 6 & 7 bug
337 float operator*( const idVec3 &a ) const;
338 idVec3 operator*( const float a ) const;
339 idVec3 operator/( const float a ) const;
340 idVec3 operator+( const idVec3 &a ) const;
341 idVec3 operator-( const idVec3 &a ) const;
342 idVec3 & operator+=( const idVec3 &a );
343 idVec3 & operator-=( const idVec3 &a );
344 idVec3 & operator/=( const idVec3 &a );
345 idVec3 & operator/=( const float a );
346 idVec3 & operator*=( const float a );
347
348 friend idVec3 operator*( const float a, const idVec3 b );
349
350 bool Compare( const idVec3 &a ) const; // exact compare, no epsilon
351 bool Compare( const idVec3 &a, const float epsilon ) const; // compare with epsilon
352 bool operator==( const idVec3 &a ) const; // exact compare, no epsilon
353 bool operator!=( const idVec3 &a ) const; // exact compare, no epsilon
354
355 bool FixDegenerateNormal( void ); // fix degenerate axial cases
356 bool FixDenormals( void ); // change tiny numbers to zero
357
358 idVec3 Cross( const idVec3 &a ) const;
359 idVec3 & Cross( const idVec3 &a, const idVec3 &b );
360 float Length( void ) const;
361 float LengthSqr( void ) const;
362 float LengthFast( void ) const;
363 float Normalize( void ); // returns length
364 float NormalizeFast( void ); // returns length
365 idVec3 & Truncate( float length ); // cap length
366 void Clamp( const idVec3 &min, const idVec3 &max );
367 void Snap( void ); // snap to closest integer value
368 void SnapInt( void ); // snap towards integer (floor)
369
370 int GetDimension( void ) const;
371
372 float ToYaw( void ) const;
373 float ToPitch( void ) const;
374 idAngles ToAngles( void ) const;
375 idPolar3 ToPolar( void ) const;
376 idMat3 ToMat3( void ) const; // vector should be normalized
377 const idVec2 & ToVec2( void ) const;
378 idVec2 & ToVec2( void );
379 const float * ToFloatPtr( void ) const;
380 float * ToFloatPtr( void );
381 const char * ToString( int precision = 2 ) const;
382
383 void NormalVectors( idVec3 &left, idVec3 &down ) const; // vector should be normalized
384 void OrthogonalBasis( idVec3 &left, idVec3 &up ) const;
385
386 void ProjectOntoPlane( const idVec3 &normal, const float overBounce = 1.0f );
387 bool ProjectAlongPlane( const idVec3 &normal, const float epsilon, const float overBounce = 1.0f );
388 void ProjectSelfOntoSphere( const float radius );
389
390 void Lerp( const idVec3 &v1, const idVec3 &v2, const float l );
391 void SLerp( const idVec3 &v1, const idVec3 &v2, const float l );
392 };
393
394 extern idVec3 vec3_origin;
395 #define vec3_zero vec3_origin
396
idVec3(void)397 ID_INLINE idVec3::idVec3( void ) {
398 }
399
idVec3(const float x,const float y,const float z)400 ID_INLINE idVec3::idVec3( const float x, const float y, const float z ) {
401 this->x = x;
402 this->y = y;
403 this->z = z;
404 }
405
406 ID_INLINE float idVec3::operator[]( const int index ) const {
407 return ( &x )[ index ];
408 }
409
410 ID_INLINE float &idVec3::operator[]( const int index ) {
411 return ( &x )[ index ];
412 }
413
Set(const float x,const float y,const float z)414 ID_INLINE void idVec3::Set( const float x, const float y, const float z ) {
415 this->x = x;
416 this->y = y;
417 this->z = z;
418 }
419
Zero(void)420 ID_INLINE void idVec3::Zero( void ) {
421 x = y = z = 0.0f;
422 }
423
424 ID_INLINE idVec3 idVec3::operator-() const {
425 return idVec3( -x, -y, -z );
426 }
427
428 ID_INLINE idVec3 &idVec3::operator=( const idVec3 &a ) {
429 x = a.x;
430 y = a.y;
431 z = a.z;
432 return *this;
433 }
434
435 ID_INLINE idVec3 idVec3::operator-( const idVec3 &a ) const {
436 return idVec3( x - a.x, y - a.y, z - a.z );
437 }
438
439 ID_INLINE float idVec3::operator*( const idVec3 &a ) const {
440 return x * a.x + y * a.y + z * a.z;
441 }
442
443 ID_INLINE idVec3 idVec3::operator*( const float a ) const {
444 return idVec3( x * a, y * a, z * a );
445 }
446
447 ID_INLINE idVec3 idVec3::operator/( const float a ) const {
448 float inva = 1.0f / a;
449 return idVec3( x * inva, y * inva, z * inva );
450 }
451
452 ID_INLINE idVec3 operator*( const float a, const idVec3 b ) {
453 return idVec3( b.x * a, b.y * a, b.z * a );
454 }
455
456 ID_INLINE idVec3 idVec3::operator+( const idVec3 &a ) const {
457 return idVec3( x + a.x, y + a.y, z + a.z );
458 }
459
460 ID_INLINE idVec3 &idVec3::operator+=( const idVec3 &a ) {
461 x += a.x;
462 y += a.y;
463 z += a.z;
464
465 return *this;
466 }
467
468 ID_INLINE idVec3 &idVec3::operator/=( const idVec3 &a ) {
469 x /= a.x;
470 y /= a.y;
471 z /= a.z;
472
473 return *this;
474 }
475
476 ID_INLINE idVec3 &idVec3::operator/=( const float a ) {
477 float inva = 1.0f / a;
478 x *= inva;
479 y *= inva;
480 z *= inva;
481
482 return *this;
483 }
484
485 ID_INLINE idVec3 &idVec3::operator-=( const idVec3 &a ) {
486 x -= a.x;
487 y -= a.y;
488 z -= a.z;
489
490 return *this;
491 }
492
493 ID_INLINE idVec3 &idVec3::operator*=( const float a ) {
494 x *= a;
495 y *= a;
496 z *= a;
497
498 return *this;
499 }
500
Compare(const idVec3 & a)501 ID_INLINE bool idVec3::Compare( const idVec3 &a ) const {
502 return ( ( x == a.x ) && ( y == a.y ) && ( z == a.z ) );
503 }
504
Compare(const idVec3 & a,const float epsilon)505 ID_INLINE bool idVec3::Compare( const idVec3 &a, const float epsilon ) const {
506 if ( idMath::Fabs( x - a.x ) > epsilon ) {
507 return false;
508 }
509
510 if ( idMath::Fabs( y - a.y ) > epsilon ) {
511 return false;
512 }
513
514 if ( idMath::Fabs( z - a.z ) > epsilon ) {
515 return false;
516 }
517
518 return true;
519 }
520
521 ID_INLINE bool idVec3::operator==( const idVec3 &a ) const {
522 return Compare( a );
523 }
524
525 ID_INLINE bool idVec3::operator!=( const idVec3 &a ) const {
526 return !Compare( a );
527 }
528
NormalizeFast(void)529 ID_INLINE float idVec3::NormalizeFast( void ) {
530 float sqrLength, invLength;
531
532 sqrLength = x * x + y * y + z * z;
533 invLength = idMath::RSqrt( sqrLength );
534 x *= invLength;
535 y *= invLength;
536 z *= invLength;
537 return invLength * sqrLength;
538 }
539
FixDegenerateNormal(void)540 ID_INLINE bool idVec3::FixDegenerateNormal( void ) {
541 if ( x == 0.0f ) {
542 if ( y == 0.0f ) {
543 if ( z > 0.0f ) {
544 if ( z != 1.0f ) {
545 z = 1.0f;
546 return true;
547 }
548 } else {
549 if ( z != -1.0f ) {
550 z = -1.0f;
551 return true;
552 }
553 }
554 return false;
555 } else if ( z == 0.0f ) {
556 if ( y > 0.0f ) {
557 if ( y != 1.0f ) {
558 y = 1.0f;
559 return true;
560 }
561 } else {
562 if ( y != -1.0f ) {
563 y = -1.0f;
564 return true;
565 }
566 }
567 return false;
568 }
569 } else if ( y == 0.0f ) {
570 if ( z == 0.0f ) {
571 if ( x > 0.0f ) {
572 if ( x != 1.0f ) {
573 x = 1.0f;
574 return true;
575 }
576 } else {
577 if ( x != -1.0f ) {
578 x = -1.0f;
579 return true;
580 }
581 }
582 return false;
583 }
584 }
585 if ( idMath::Fabs( x ) == 1.0f ) {
586 if ( y != 0.0f || z != 0.0f ) {
587 y = z = 0.0f;
588 return true;
589 }
590 return false;
591 } else if ( idMath::Fabs( y ) == 1.0f ) {
592 if ( x != 0.0f || z != 0.0f ) {
593 x = z = 0.0f;
594 return true;
595 }
596 return false;
597 } else if ( idMath::Fabs( z ) == 1.0f ) {
598 if ( x != 0.0f || y != 0.0f ) {
599 x = y = 0.0f;
600 return true;
601 }
602 return false;
603 }
604 return false;
605 }
606
FixDenormals(void)607 ID_INLINE bool idVec3::FixDenormals( void ) {
608 bool denormal = false;
609 if ( fabs( x ) < 1e-30f ) {
610 x = 0.0f;
611 denormal = true;
612 }
613 if ( fabs( y ) < 1e-30f ) {
614 y = 0.0f;
615 denormal = true;
616 }
617 if ( fabs( z ) < 1e-30f ) {
618 z = 0.0f;
619 denormal = true;
620 }
621 return denormal;
622 }
623
Cross(const idVec3 & a)624 ID_INLINE idVec3 idVec3::Cross( const idVec3 &a ) const {
625 return idVec3( y * a.z - z * a.y, z * a.x - x * a.z, x * a.y - y * a.x );
626 }
627
Cross(const idVec3 & a,const idVec3 & b)628 ID_INLINE idVec3 &idVec3::Cross( const idVec3 &a, const idVec3 &b ) {
629 x = a.y * b.z - a.z * b.y;
630 y = a.z * b.x - a.x * b.z;
631 z = a.x * b.y - a.y * b.x;
632
633 return *this;
634 }
635
Length(void)636 ID_INLINE float idVec3::Length( void ) const {
637 return ( float )idMath::Sqrt( x * x + y * y + z * z );
638 }
639
LengthSqr(void)640 ID_INLINE float idVec3::LengthSqr( void ) const {
641 return ( x * x + y * y + z * z );
642 }
643
LengthFast(void)644 ID_INLINE float idVec3::LengthFast( void ) const {
645 float sqrLength;
646
647 sqrLength = x * x + y * y + z * z;
648 return sqrLength * idMath::RSqrt( sqrLength );
649 }
650
Normalize(void)651 ID_INLINE float idVec3::Normalize( void ) {
652 float sqrLength, invLength;
653
654 sqrLength = x * x + y * y + z * z;
655 invLength = idMath::InvSqrt( sqrLength );
656 x *= invLength;
657 y *= invLength;
658 z *= invLength;
659 return invLength * sqrLength;
660 }
661
Truncate(float length)662 ID_INLINE idVec3 &idVec3::Truncate( float length ) {
663 float length2;
664 float ilength;
665
666 if ( !length ) {
667 Zero();
668 }
669 else {
670 length2 = LengthSqr();
671 if ( length2 > length * length ) {
672 ilength = length * idMath::InvSqrt( length2 );
673 x *= ilength;
674 y *= ilength;
675 z *= ilength;
676 }
677 }
678
679 return *this;
680 }
681
Clamp(const idVec3 & min,const idVec3 & max)682 ID_INLINE void idVec3::Clamp( const idVec3 &min, const idVec3 &max ) {
683 if ( x < min.x ) {
684 x = min.x;
685 } else if ( x > max.x ) {
686 x = max.x;
687 }
688 if ( y < min.y ) {
689 y = min.y;
690 } else if ( y > max.y ) {
691 y = max.y;
692 }
693 if ( z < min.z ) {
694 z = min.z;
695 } else if ( z > max.z ) {
696 z = max.z;
697 }
698 }
699
Snap(void)700 ID_INLINE void idVec3::Snap( void ) {
701 x = floor( x + 0.5f );
702 y = floor( y + 0.5f );
703 z = floor( z + 0.5f );
704 }
705
SnapInt(void)706 ID_INLINE void idVec3::SnapInt( void ) {
707 x = float( int( x ) );
708 y = float( int( y ) );
709 z = float( int( z ) );
710 }
711
GetDimension(void)712 ID_INLINE int idVec3::GetDimension( void ) const {
713 return 3;
714 }
715
ToVec2(void)716 ID_INLINE const idVec2 &idVec3::ToVec2( void ) const {
717 return *reinterpret_cast<const idVec2 *>(this);
718 }
719
ToVec2(void)720 ID_INLINE idVec2 &idVec3::ToVec2( void ) {
721 return *reinterpret_cast<idVec2 *>(this);
722 }
723
ToFloatPtr(void)724 ID_INLINE const float *idVec3::ToFloatPtr( void ) const {
725 return &x;
726 }
727
ToFloatPtr(void)728 ID_INLINE float *idVec3::ToFloatPtr( void ) {
729 return &x;
730 }
731
NormalVectors(idVec3 & left,idVec3 & down)732 ID_INLINE void idVec3::NormalVectors( idVec3 &left, idVec3 &down ) const {
733 float d;
734
735 d = x * x + y * y;
736 if ( !d ) {
737 left[0] = 1;
738 left[1] = 0;
739 left[2] = 0;
740 } else {
741 d = idMath::InvSqrt( d );
742 left[0] = -y * d;
743 left[1] = x * d;
744 left[2] = 0;
745 }
746 down = left.Cross( *this );
747 }
748
OrthogonalBasis(idVec3 & left,idVec3 & up)749 ID_INLINE void idVec3::OrthogonalBasis( idVec3 &left, idVec3 &up ) const {
750 float l, s;
751
752 if ( idMath::Fabs( z ) > 0.7f ) {
753 l = y * y + z * z;
754 s = idMath::InvSqrt( l );
755 up[0] = 0;
756 up[1] = z * s;
757 up[2] = -y * s;
758 left[0] = l * s;
759 left[1] = -x * up[2];
760 left[2] = x * up[1];
761 }
762 else {
763 l = x * x + y * y;
764 s = idMath::InvSqrt( l );
765 left[0] = -y * s;
766 left[1] = x * s;
767 left[2] = 0;
768 up[0] = -z * left[1];
769 up[1] = z * left[0];
770 up[2] = l * s;
771 }
772 }
773
ProjectOntoPlane(const idVec3 & normal,const float overBounce)774 ID_INLINE void idVec3::ProjectOntoPlane( const idVec3 &normal, const float overBounce ) {
775 float backoff;
776
777 backoff = *this * normal;
778
779 if ( overBounce != 1.0 ) {
780 if ( backoff < 0 ) {
781 backoff *= overBounce;
782 } else {
783 backoff /= overBounce;
784 }
785 }
786
787 *this -= backoff * normal;
788 }
789
ProjectAlongPlane(const idVec3 & normal,const float epsilon,const float overBounce)790 ID_INLINE bool idVec3::ProjectAlongPlane( const idVec3 &normal, const float epsilon, const float overBounce ) {
791 idVec3 cross;
792 float len;
793
794 cross = this->Cross( normal ).Cross( (*this) );
795 // normalize so a fixed epsilon can be used
796 cross.Normalize();
797 len = normal * cross;
798 if ( idMath::Fabs( len ) < epsilon ) {
799 return false;
800 }
801 cross *= overBounce * ( normal * (*this) ) / len;
802 (*this) -= cross;
803 return true;
804 }
805
806
807 //===============================================================
808 //
809 // idVec4 - 4D vector
810 //
811 //===============================================================
812
813 class idVec4 {
814 public:
815 float x;
816 float y;
817 float z;
818 float w;
819
820 idVec4( void );
821 explicit idVec4( const float x, const float y, const float z, const float w );
822
823 void Set( const float x, const float y, const float z, const float w );
824 void Zero( void );
825
826 float operator[]( const int index ) const;
827 float & operator[]( const int index );
828 idVec4 operator-() const;
829 float operator*( const idVec4 &a ) const;
830 idVec4 operator*( const float a ) const;
831 idVec4 operator/( const float a ) const;
832 idVec4 operator+( const idVec4 &a ) const;
833 idVec4 operator-( const idVec4 &a ) const;
834 idVec4 & operator+=( const idVec4 &a );
835 idVec4 & operator-=( const idVec4 &a );
836 idVec4 & operator/=( const idVec4 &a );
837 idVec4 & operator/=( const float a );
838 idVec4 & operator*=( const float a );
839
840 friend idVec4 operator*( const float a, const idVec4 b );
841
842 bool Compare( const idVec4 &a ) const; // exact compare, no epsilon
843 bool Compare( const idVec4 &a, const float epsilon ) const; // compare with epsilon
844 bool operator==( const idVec4 &a ) const; // exact compare, no epsilon
845 bool operator!=( const idVec4 &a ) const; // exact compare, no epsilon
846
847 float Length( void ) const;
848 float LengthSqr( void ) const;
849 float Normalize( void ); // returns length
850 float NormalizeFast( void ); // returns length
851
852 int GetDimension( void ) const;
853
854 const idVec2 & ToVec2( void ) const;
855 idVec2 & ToVec2( void );
856 const idVec3 & ToVec3( void ) const;
857 idVec3 & ToVec3( void );
858 const float * ToFloatPtr( void ) const;
859 float * ToFloatPtr( void );
860 const char * ToString( int precision = 2 ) const;
861
862 void Lerp( const idVec4 &v1, const idVec4 &v2, const float l );
863 };
864
865 extern idVec4 vec4_origin;
866 #define vec4_zero vec4_origin
867
idVec4(void)868 ID_INLINE idVec4::idVec4( void ) {
869 }
870
idVec4(const float x,const float y,const float z,const float w)871 ID_INLINE idVec4::idVec4( const float x, const float y, const float z, const float w ) {
872 this->x = x;
873 this->y = y;
874 this->z = z;
875 this->w = w;
876 }
877
Set(const float x,const float y,const float z,const float w)878 ID_INLINE void idVec4::Set( const float x, const float y, const float z, const float w ) {
879 this->x = x;
880 this->y = y;
881 this->z = z;
882 this->w = w;
883 }
884
Zero(void)885 ID_INLINE void idVec4::Zero( void ) {
886 x = y = z = w = 0.0f;
887 }
888
889 ID_INLINE float idVec4::operator[]( int index ) const {
890 return ( &x )[ index ];
891 }
892
893 ID_INLINE float& idVec4::operator[]( int index ) {
894 return ( &x )[ index ];
895 }
896
897 ID_INLINE idVec4 idVec4::operator-() const {
898 return idVec4( -x, -y, -z, -w );
899 }
900
901 ID_INLINE idVec4 idVec4::operator-( const idVec4 &a ) const {
902 return idVec4( x - a.x, y - a.y, z - a.z, w - a.w );
903 }
904
905 ID_INLINE float idVec4::operator*( const idVec4 &a ) const {
906 return x * a.x + y * a.y + z * a.z + w * a.w;
907 }
908
909 ID_INLINE idVec4 idVec4::operator*( const float a ) const {
910 return idVec4( x * a, y * a, z * a, w * a );
911 }
912
913 ID_INLINE idVec4 idVec4::operator/( const float a ) const {
914 float inva = 1.0f / a;
915 return idVec4( x * inva, y * inva, z * inva, w * inva );
916 }
917
918 ID_INLINE idVec4 operator*( const float a, const idVec4 b ) {
919 return idVec4( b.x * a, b.y * a, b.z * a, b.w * a );
920 }
921
922 ID_INLINE idVec4 idVec4::operator+( const idVec4 &a ) const {
923 return idVec4( x + a.x, y + a.y, z + a.z, w + a.w );
924 }
925
926 ID_INLINE idVec4 &idVec4::operator+=( const idVec4 &a ) {
927 x += a.x;
928 y += a.y;
929 z += a.z;
930 w += a.w;
931
932 return *this;
933 }
934
935 ID_INLINE idVec4 &idVec4::operator/=( const idVec4 &a ) {
936 x /= a.x;
937 y /= a.y;
938 z /= a.z;
939 w /= a.w;
940
941 return *this;
942 }
943
944 ID_INLINE idVec4 &idVec4::operator/=( const float a ) {
945 float inva = 1.0f / a;
946 x *= inva;
947 y *= inva;
948 z *= inva;
949 w *= inva;
950
951 return *this;
952 }
953
954 ID_INLINE idVec4 &idVec4::operator-=( const idVec4 &a ) {
955 x -= a.x;
956 y -= a.y;
957 z -= a.z;
958 w -= a.w;
959
960 return *this;
961 }
962
963 ID_INLINE idVec4 &idVec4::operator*=( const float a ) {
964 x *= a;
965 y *= a;
966 z *= a;
967 w *= a;
968
969 return *this;
970 }
971
Compare(const idVec4 & a)972 ID_INLINE bool idVec4::Compare( const idVec4 &a ) const {
973 return ( ( x == a.x ) && ( y == a.y ) && ( z == a.z ) && w == a.w );
974 }
975
Compare(const idVec4 & a,const float epsilon)976 ID_INLINE bool idVec4::Compare( const idVec4 &a, const float epsilon ) const {
977 if ( idMath::Fabs( x - a.x ) > epsilon ) {
978 return false;
979 }
980
981 if ( idMath::Fabs( y - a.y ) > epsilon ) {
982 return false;
983 }
984
985 if ( idMath::Fabs( z - a.z ) > epsilon ) {
986 return false;
987 }
988
989 if ( idMath::Fabs( w - a.w ) > epsilon ) {
990 return false;
991 }
992
993 return true;
994 }
995
996 ID_INLINE bool idVec4::operator==( const idVec4 &a ) const {
997 return Compare( a );
998 }
999
1000 ID_INLINE bool idVec4::operator!=( const idVec4 &a ) const {
1001 return !Compare( a );
1002 }
1003
Length(void)1004 ID_INLINE float idVec4::Length( void ) const {
1005 return ( float )idMath::Sqrt( x * x + y * y + z * z + w * w );
1006 }
1007
LengthSqr(void)1008 ID_INLINE float idVec4::LengthSqr( void ) const {
1009 return ( x * x + y * y + z * z + w * w );
1010 }
1011
Normalize(void)1012 ID_INLINE float idVec4::Normalize( void ) {
1013 float sqrLength, invLength;
1014
1015 sqrLength = x * x + y * y + z * z + w * w;
1016 invLength = idMath::InvSqrt( sqrLength );
1017 x *= invLength;
1018 y *= invLength;
1019 z *= invLength;
1020 w *= invLength;
1021 return invLength * sqrLength;
1022 }
1023
NormalizeFast(void)1024 ID_INLINE float idVec4::NormalizeFast( void ) {
1025 float sqrLength, invLength;
1026
1027 sqrLength = x * x + y * y + z * z + w * w;
1028 invLength = idMath::RSqrt( sqrLength );
1029 x *= invLength;
1030 y *= invLength;
1031 z *= invLength;
1032 w *= invLength;
1033 return invLength * sqrLength;
1034 }
1035
GetDimension(void)1036 ID_INLINE int idVec4::GetDimension( void ) const {
1037 return 4;
1038 }
1039
ToVec2(void)1040 ID_INLINE const idVec2 &idVec4::ToVec2( void ) const {
1041 return *reinterpret_cast<const idVec2 *>(this);
1042 }
1043
ToVec2(void)1044 ID_INLINE idVec2 &idVec4::ToVec2( void ) {
1045 return *reinterpret_cast<idVec2 *>(this);
1046 }
1047
ToVec3(void)1048 ID_INLINE const idVec3 &idVec4::ToVec3( void ) const {
1049 return *reinterpret_cast<const idVec3 *>(this);
1050 }
1051
ToVec3(void)1052 ID_INLINE idVec3 &idVec4::ToVec3( void ) {
1053 return *reinterpret_cast<idVec3 *>(this);
1054 }
1055
ToFloatPtr(void)1056 ID_INLINE const float *idVec4::ToFloatPtr( void ) const {
1057 return &x;
1058 }
1059
ToFloatPtr(void)1060 ID_INLINE float *idVec4::ToFloatPtr( void ) {
1061 return &x;
1062 }
1063
1064
1065 //===============================================================
1066 //
1067 // idVec5 - 5D vector
1068 //
1069 //===============================================================
1070
1071 class idVec5 {
1072 public:
1073 float x;
1074 float y;
1075 float z;
1076 float s;
1077 float t;
1078
1079 idVec5( void );
1080 explicit idVec5( const idVec3 &xyz, const idVec2 &st );
1081 explicit idVec5( const float x, const float y, const float z, const float s, const float t );
1082
1083 float operator[]( int index ) const;
1084 float & operator[]( int index );
1085 idVec5 & operator=( const idVec3 &a );
1086
1087 int GetDimension( void ) const;
1088
1089 const idVec3 & ToVec3( void ) const;
1090 idVec3 & ToVec3( void );
1091 const float * ToFloatPtr( void ) const;
1092 float * ToFloatPtr( void );
1093 const char * ToString( int precision = 2 ) const;
1094
1095 void Lerp( const idVec5 &v1, const idVec5 &v2, const float l );
1096 };
1097
1098 extern idVec5 vec5_origin;
1099 #define vec5_zero vec5_origin
1100
idVec5(void)1101 ID_INLINE idVec5::idVec5( void ) {
1102 }
1103
idVec5(const idVec3 & xyz,const idVec2 & st)1104 ID_INLINE idVec5::idVec5( const idVec3 &xyz, const idVec2 &st ) {
1105 x = xyz.x;
1106 y = xyz.y;
1107 z = xyz.z;
1108 s = st[0];
1109 t = st[1];
1110 }
1111
idVec5(const float x,const float y,const float z,const float s,const float t)1112 ID_INLINE idVec5::idVec5( const float x, const float y, const float z, const float s, const float t ) {
1113 this->x = x;
1114 this->y = y;
1115 this->z = z;
1116 this->s = s;
1117 this->t = t;
1118 }
1119
1120 ID_INLINE float idVec5::operator[]( int index ) const {
1121 return ( &x )[ index ];
1122 }
1123
1124 ID_INLINE float& idVec5::operator[]( int index ) {
1125 return ( &x )[ index ];
1126 }
1127
1128 ID_INLINE idVec5 &idVec5::operator=( const idVec3 &a ) {
1129 x = a.x;
1130 y = a.y;
1131 z = a.z;
1132 s = t = 0;
1133 return *this;
1134 }
1135
GetDimension(void)1136 ID_INLINE int idVec5::GetDimension( void ) const {
1137 return 5;
1138 }
1139
ToVec3(void)1140 ID_INLINE const idVec3 &idVec5::ToVec3( void ) const {
1141 return *reinterpret_cast<const idVec3 *>(this);
1142 }
1143
ToVec3(void)1144 ID_INLINE idVec3 &idVec5::ToVec3( void ) {
1145 return *reinterpret_cast<idVec3 *>(this);
1146 }
1147
ToFloatPtr(void)1148 ID_INLINE const float *idVec5::ToFloatPtr( void ) const {
1149 return &x;
1150 }
1151
ToFloatPtr(void)1152 ID_INLINE float *idVec5::ToFloatPtr( void ) {
1153 return &x;
1154 }
1155
1156
1157 //===============================================================
1158 //
1159 // idVec6 - 6D vector
1160 //
1161 //===============================================================
1162
1163 class idVec6 {
1164 public:
1165 idVec6( void );
1166 explicit idVec6( const float *a );
1167 explicit idVec6( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 );
1168
1169 void Set( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 );
1170 void Zero( void );
1171
1172 float operator[]( const int index ) const;
1173 float & operator[]( const int index );
1174 idVec6 operator-() const;
1175 idVec6 operator*( const float a ) const;
1176 idVec6 operator/( const float a ) const;
1177 float operator*( const idVec6 &a ) const;
1178 idVec6 operator-( const idVec6 &a ) const;
1179 idVec6 operator+( const idVec6 &a ) const;
1180 idVec6 & operator*=( const float a );
1181 idVec6 & operator/=( const float a );
1182 idVec6 & operator+=( const idVec6 &a );
1183 idVec6 & operator-=( const idVec6 &a );
1184
1185 friend idVec6 operator*( const float a, const idVec6 b );
1186
1187 bool Compare( const idVec6 &a ) const; // exact compare, no epsilon
1188 bool Compare( const idVec6 &a, const float epsilon ) const; // compare with epsilon
1189 bool operator==( const idVec6 &a ) const; // exact compare, no epsilon
1190 bool operator!=( const idVec6 &a ) const; // exact compare, no epsilon
1191
1192 float Length( void ) const;
1193 float LengthSqr( void ) const;
1194 float Normalize( void ); // returns length
1195 float NormalizeFast( void ); // returns length
1196
1197 int GetDimension( void ) const;
1198
1199 const idVec3 & SubVec3( int index ) const;
1200 idVec3 & SubVec3( int index );
1201 const float * ToFloatPtr( void ) const;
1202 float * ToFloatPtr( void );
1203 const char * ToString( int precision = 2 ) const;
1204
1205 private:
1206 float p[6];
1207 };
1208
1209 extern idVec6 vec6_origin;
1210 #define vec6_zero vec6_origin
1211 extern idVec6 vec6_infinity;
1212
idVec6(void)1213 ID_INLINE idVec6::idVec6( void ) {
1214 }
1215
idVec6(const float * a)1216 ID_INLINE idVec6::idVec6( const float *a ) {
1217 memcpy( p, a, 6 * sizeof( float ) );
1218 }
1219
idVec6(const float a1,const float a2,const float a3,const float a4,const float a5,const float a6)1220 ID_INLINE idVec6::idVec6( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 ) {
1221 p[0] = a1;
1222 p[1] = a2;
1223 p[2] = a3;
1224 p[3] = a4;
1225 p[4] = a5;
1226 p[5] = a6;
1227 }
1228
1229 ID_INLINE idVec6 idVec6::operator-() const {
1230 return idVec6( -p[0], -p[1], -p[2], -p[3], -p[4], -p[5] );
1231 }
1232
1233 ID_INLINE float idVec6::operator[]( const int index ) const {
1234 return p[index];
1235 }
1236
1237 ID_INLINE float &idVec6::operator[]( const int index ) {
1238 return p[index];
1239 }
1240
1241 ID_INLINE idVec6 idVec6::operator*( const float a ) const {
1242 return idVec6( p[0]*a, p[1]*a, p[2]*a, p[3]*a, p[4]*a, p[5]*a );
1243 }
1244
1245 ID_INLINE float idVec6::operator*( const idVec6 &a ) const {
1246 return p[0] * a[0] + p[1] * a[1] + p[2] * a[2] + p[3] * a[3] + p[4] * a[4] + p[5] * a[5];
1247 }
1248
1249 ID_INLINE idVec6 idVec6::operator/( const float a ) const {
1250 float inva;
1251
1252 assert( a != 0.0f );
1253 inva = 1.0f / a;
1254 return idVec6( p[0]*inva, p[1]*inva, p[2]*inva, p[3]*inva, p[4]*inva, p[5]*inva );
1255 }
1256
1257 ID_INLINE idVec6 idVec6::operator+( const idVec6 &a ) const {
1258 return idVec6( p[0] + a[0], p[1] + a[1], p[2] + a[2], p[3] + a[3], p[4] + a[4], p[5] + a[5] );
1259 }
1260
1261 ID_INLINE idVec6 idVec6::operator-( const idVec6 &a ) const {
1262 return idVec6( p[0] - a[0], p[1] - a[1], p[2] - a[2], p[3] - a[3], p[4] - a[4], p[5] - a[5] );
1263 }
1264
1265 ID_INLINE idVec6 &idVec6::operator*=( const float a ) {
1266 p[0] *= a;
1267 p[1] *= a;
1268 p[2] *= a;
1269 p[3] *= a;
1270 p[4] *= a;
1271 p[5] *= a;
1272 return *this;
1273 }
1274
1275 ID_INLINE idVec6 &idVec6::operator/=( const float a ) {
1276 float inva;
1277
1278 assert( a != 0.0f );
1279 inva = 1.0f / a;
1280 p[0] *= inva;
1281 p[1] *= inva;
1282 p[2] *= inva;
1283 p[3] *= inva;
1284 p[4] *= inva;
1285 p[5] *= inva;
1286 return *this;
1287 }
1288
1289 ID_INLINE idVec6 &idVec6::operator+=( const idVec6 &a ) {
1290 p[0] += a[0];
1291 p[1] += a[1];
1292 p[2] += a[2];
1293 p[3] += a[3];
1294 p[4] += a[4];
1295 p[5] += a[5];
1296 return *this;
1297 }
1298
1299 ID_INLINE idVec6 &idVec6::operator-=( const idVec6 &a ) {
1300 p[0] -= a[0];
1301 p[1] -= a[1];
1302 p[2] -= a[2];
1303 p[3] -= a[3];
1304 p[4] -= a[4];
1305 p[5] -= a[5];
1306 return *this;
1307 }
1308
1309 ID_INLINE idVec6 operator*( const float a, const idVec6 b ) {
1310 return b * a;
1311 }
1312
Compare(const idVec6 & a)1313 ID_INLINE bool idVec6::Compare( const idVec6 &a ) const {
1314 return ( ( p[0] == a[0] ) && ( p[1] == a[1] ) && ( p[2] == a[2] ) &&
1315 ( p[3] == a[3] ) && ( p[4] == a[4] ) && ( p[5] == a[5] ) );
1316 }
1317
Compare(const idVec6 & a,const float epsilon)1318 ID_INLINE bool idVec6::Compare( const idVec6 &a, const float epsilon ) const {
1319 if ( idMath::Fabs( p[0] - a[0] ) > epsilon ) {
1320 return false;
1321 }
1322
1323 if ( idMath::Fabs( p[1] - a[1] ) > epsilon ) {
1324 return false;
1325 }
1326
1327 if ( idMath::Fabs( p[2] - a[2] ) > epsilon ) {
1328 return false;
1329 }
1330
1331 if ( idMath::Fabs( p[3] - a[3] ) > epsilon ) {
1332 return false;
1333 }
1334
1335 if ( idMath::Fabs( p[4] - a[4] ) > epsilon ) {
1336 return false;
1337 }
1338
1339 if ( idMath::Fabs( p[5] - a[5] ) > epsilon ) {
1340 return false;
1341 }
1342
1343 return true;
1344 }
1345
1346 ID_INLINE bool idVec6::operator==( const idVec6 &a ) const {
1347 return Compare( a );
1348 }
1349
1350 ID_INLINE bool idVec6::operator!=( const idVec6 &a ) const {
1351 return !Compare( a );
1352 }
1353
Set(const float a1,const float a2,const float a3,const float a4,const float a5,const float a6)1354 ID_INLINE void idVec6::Set( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 ) {
1355 p[0] = a1;
1356 p[1] = a2;
1357 p[2] = a3;
1358 p[3] = a4;
1359 p[4] = a5;
1360 p[5] = a6;
1361 }
1362
Zero(void)1363 ID_INLINE void idVec6::Zero( void ) {
1364 p[0] = p[1] = p[2] = p[3] = p[4] = p[5] = 0.0f;
1365 }
1366
Length(void)1367 ID_INLINE float idVec6::Length( void ) const {
1368 return ( float )idMath::Sqrt( p[0] * p[0] + p[1] * p[1] + p[2] * p[2] + p[3] * p[3] + p[4] * p[4] + p[5] * p[5] );
1369 }
1370
LengthSqr(void)1371 ID_INLINE float idVec6::LengthSqr( void ) const {
1372 return ( p[0] * p[0] + p[1] * p[1] + p[2] * p[2] + p[3] * p[3] + p[4] * p[4] + p[5] * p[5] );
1373 }
1374
Normalize(void)1375 ID_INLINE float idVec6::Normalize( void ) {
1376 float sqrLength, invLength;
1377
1378 sqrLength = p[0] * p[0] + p[1] * p[1] + p[2] * p[2] + p[3] * p[3] + p[4] * p[4] + p[5] * p[5];
1379 invLength = idMath::InvSqrt( sqrLength );
1380 p[0] *= invLength;
1381 p[1] *= invLength;
1382 p[2] *= invLength;
1383 p[3] *= invLength;
1384 p[4] *= invLength;
1385 p[5] *= invLength;
1386 return invLength * sqrLength;
1387 }
1388
NormalizeFast(void)1389 ID_INLINE float idVec6::NormalizeFast( void ) {
1390 float sqrLength, invLength;
1391
1392 sqrLength = p[0] * p[0] + p[1] * p[1] + p[2] * p[2] + p[3] * p[3] + p[4] * p[4] + p[5] * p[5];
1393 invLength = idMath::RSqrt( sqrLength );
1394 p[0] *= invLength;
1395 p[1] *= invLength;
1396 p[2] *= invLength;
1397 p[3] *= invLength;
1398 p[4] *= invLength;
1399 p[5] *= invLength;
1400 return invLength * sqrLength;
1401 }
1402
GetDimension(void)1403 ID_INLINE int idVec6::GetDimension( void ) const {
1404 return 6;
1405 }
1406
SubVec3(int index)1407 ID_INLINE const idVec3 &idVec6::SubVec3( int index ) const {
1408 return *reinterpret_cast<const idVec3 *>(p + index * 3);
1409 }
1410
SubVec3(int index)1411 ID_INLINE idVec3 &idVec6::SubVec3( int index ) {
1412 return *reinterpret_cast<idVec3 *>(p + index * 3);
1413 }
1414
ToFloatPtr(void)1415 ID_INLINE const float *idVec6::ToFloatPtr( void ) const {
1416 return p;
1417 }
1418
ToFloatPtr(void)1419 ID_INLINE float *idVec6::ToFloatPtr( void ) {
1420 return p;
1421 }
1422
1423
1424 //===============================================================
1425 //
1426 // idVecX - arbitrary sized vector
1427 //
1428 // The vector lives on 16 byte aligned and 16 byte padded memory.
1429 //
1430 // NOTE: due to the temporary memory pool idVecX cannot be used by multiple threads
1431 //
1432 //===============================================================
1433
1434 #define VECX_MAX_TEMP 1024
1435 #define VECX_QUAD( x ) ( ( ( ( x ) + 3 ) & ~3 ) * sizeof( float ) )
1436 #define VECX_CLEAREND() int s = size; while( s < ( ( s + 3) & ~3 ) ) { p[s++] = 0.0f; }
1437 #define VECX_ALLOCA( n ) ( (float *) _alloca16( VECX_QUAD( n ) ) )
1438 #define VECX_SIMD
1439
1440 class idVecX {
1441 friend class idMatX;
1442
1443 public:
1444 idVecX( void );
1445 explicit idVecX( int length );
1446 explicit idVecX( int length, float *data );
1447 ~idVecX( void );
1448
1449 float operator[]( const int index ) const;
1450 float & operator[]( const int index );
1451 idVecX operator-() const;
1452 idVecX & operator=( const idVecX &a );
1453 idVecX operator*( const float a ) const;
1454 idVecX operator/( const float a ) const;
1455 float operator*( const idVecX &a ) const;
1456 idVecX operator-( const idVecX &a ) const;
1457 idVecX operator+( const idVecX &a ) const;
1458 idVecX & operator*=( const float a );
1459 idVecX & operator/=( const float a );
1460 idVecX & operator+=( const idVecX &a );
1461 idVecX & operator-=( const idVecX &a );
1462
1463 friend idVecX operator*( const float a, const idVecX b );
1464
1465 bool Compare( const idVecX &a ) const; // exact compare, no epsilon
1466 bool Compare( const idVecX &a, const float epsilon ) const; // compare with epsilon
1467 bool operator==( const idVecX &a ) const; // exact compare, no epsilon
1468 bool operator!=( const idVecX &a ) const; // exact compare, no epsilon
1469
1470 void SetSize( int size );
1471 void ChangeSize( int size, bool makeZero = false );
GetSize(void)1472 int GetSize( void ) const { return size; }
1473 void SetData( int length, float *data );
1474 void Zero( void );
1475 void Zero( int length );
1476 void Random( int seed, float l = 0.0f, float u = 1.0f );
1477 void Random( int length, int seed, float l = 0.0f, float u = 1.0f );
1478 void Negate( void );
1479 void Clamp( float min, float max );
1480 idVecX & SwapElements( int e1, int e2 );
1481
1482 float Length( void ) const;
1483 float LengthSqr( void ) const;
1484 idVecX Normalize( void ) const;
1485 float NormalizeSelf( void );
1486
1487 int GetDimension( void ) const;
1488
1489 const idVec3 & SubVec3( int index ) const;
1490 idVec3 & SubVec3( int index );
1491 const idVec6 & SubVec6( int index ) const;
1492 idVec6 & SubVec6( int index );
1493 const float * ToFloatPtr( void ) const;
1494 float * ToFloatPtr( void );
1495 const char * ToString( int precision = 2 ) const;
1496
1497 private:
1498 int size; // size of the vector
1499 int alloced; // if -1 p points to data set with SetData
1500 float * p; // memory the vector is stored
1501
1502 static float temp[VECX_MAX_TEMP+4]; // used to store intermediate results
1503 static float * tempPtr; // pointer to 16 byte aligned temporary memory
1504 static int tempIndex; // index into memory pool, wraps around
1505
1506 private:
1507 void SetTempSize( int size );
1508 };
1509
1510
idVecX(void)1511 ID_INLINE idVecX::idVecX( void ) {
1512 size = alloced = 0;
1513 p = NULL;
1514 }
1515
idVecX(int length)1516 ID_INLINE idVecX::idVecX( int length ) {
1517 size = alloced = 0;
1518 p = NULL;
1519 SetSize( length );
1520 }
1521
idVecX(int length,float * data)1522 ID_INLINE idVecX::idVecX( int length, float *data ) {
1523 size = alloced = 0;
1524 p = NULL;
1525 SetData( length, data );
1526 }
1527
~idVecX(void)1528 ID_INLINE idVecX::~idVecX( void ) {
1529 // if not temp memory
1530 if ( p && ( p < idVecX::tempPtr || p >= idVecX::tempPtr + VECX_MAX_TEMP ) && alloced != -1 ) {
1531 Mem_Free16( p );
1532 }
1533 }
1534
1535 ID_INLINE float idVecX::operator[]( const int index ) const {
1536 assert( index >= 0 && index < size );
1537 return p[index];
1538 }
1539
1540 ID_INLINE float &idVecX::operator[]( const int index ) {
1541 assert( index >= 0 && index < size );
1542 return p[index];
1543 }
1544
1545 ID_INLINE idVecX idVecX::operator-() const {
1546 int i;
1547 idVecX m;
1548
1549 m.SetTempSize( size );
1550 for ( i = 0; i < size; i++ ) {
1551 m.p[i] = -p[i];
1552 }
1553 return m;
1554 }
1555
1556 ID_INLINE idVecX &idVecX::operator=( const idVecX &a ) {
1557 SetSize( a.size );
1558 #ifdef VECX_SIMD
1559 SIMDProcessor->Copy16( p, a.p, a.size );
1560 #else
1561 memcpy( p, a.p, a.size * sizeof( float ) );
1562 #endif
1563 idVecX::tempIndex = 0;
1564 return *this;
1565 }
1566
1567 ID_INLINE idVecX idVecX::operator+( const idVecX &a ) const {
1568 idVecX m;
1569
1570 assert( size == a.size );
1571 m.SetTempSize( size );
1572 #ifdef VECX_SIMD
1573 SIMDProcessor->Add16( m.p, p, a.p, size );
1574 #else
1575 int i;
1576 for ( i = 0; i < size; i++ ) {
1577 m.p[i] = p[i] + a.p[i];
1578 }
1579 #endif
1580 return m;
1581 }
1582
1583 ID_INLINE idVecX idVecX::operator-( const idVecX &a ) const {
1584 idVecX m;
1585
1586 assert( size == a.size );
1587 m.SetTempSize( size );
1588 #ifdef VECX_SIMD
1589 SIMDProcessor->Sub16( m.p, p, a.p, size );
1590 #else
1591 int i;
1592 for ( i = 0; i < size; i++ ) {
1593 m.p[i] = p[i] - a.p[i];
1594 }
1595 #endif
1596 return m;
1597 }
1598
1599 ID_INLINE idVecX &idVecX::operator+=( const idVecX &a ) {
1600 assert( size == a.size );
1601 #ifdef VECX_SIMD
1602 SIMDProcessor->AddAssign16( p, a.p, size );
1603 #else
1604 int i;
1605 for ( i = 0; i < size; i++ ) {
1606 p[i] += a.p[i];
1607 }
1608 #endif
1609 idVecX::tempIndex = 0;
1610 return *this;
1611 }
1612
1613 ID_INLINE idVecX &idVecX::operator-=( const idVecX &a ) {
1614 assert( size == a.size );
1615 #ifdef VECX_SIMD
1616 SIMDProcessor->SubAssign16( p, a.p, size );
1617 #else
1618 int i;
1619 for ( i = 0; i < size; i++ ) {
1620 p[i] -= a.p[i];
1621 }
1622 #endif
1623 idVecX::tempIndex = 0;
1624 return *this;
1625 }
1626
1627 ID_INLINE idVecX idVecX::operator*( const float a ) const {
1628 idVecX m;
1629
1630 m.SetTempSize( size );
1631 #ifdef VECX_SIMD
1632 SIMDProcessor->Mul16( m.p, p, a, size );
1633 #else
1634 int i;
1635 for ( i = 0; i < size; i++ ) {
1636 m.p[i] = p[i] * a;
1637 }
1638 #endif
1639 return m;
1640 }
1641
1642 ID_INLINE idVecX &idVecX::operator*=( const float a ) {
1643 #ifdef VECX_SIMD
1644 SIMDProcessor->MulAssign16( p, a, size );
1645 #else
1646 int i;
1647 for ( i = 0; i < size; i++ ) {
1648 p[i] *= a;
1649 }
1650 #endif
1651 return *this;
1652 }
1653
1654 ID_INLINE idVecX idVecX::operator/( const float a ) const {
1655 assert( a != 0.0f );
1656 return (*this) * ( 1.0f / a );
1657 }
1658
1659 ID_INLINE idVecX &idVecX::operator/=( const float a ) {
1660 assert( a != 0.0f );
1661 (*this) *= ( 1.0f / a );
1662 return *this;
1663 }
1664
1665 ID_INLINE idVecX operator*( const float a, const idVecX b ) {
1666 return b * a;
1667 }
1668
1669 ID_INLINE float idVecX::operator*( const idVecX &a ) const {
1670 int i;
1671 float sum = 0.0f;
1672
1673 assert( size == a.size );
1674 for ( i = 0; i < size; i++ ) {
1675 sum += p[i] * a.p[i];
1676 }
1677 return sum;
1678 }
1679
Compare(const idVecX & a)1680 ID_INLINE bool idVecX::Compare( const idVecX &a ) const {
1681 int i;
1682
1683 assert( size == a.size );
1684 for ( i = 0; i < size; i++ ) {
1685 if ( p[i] != a.p[i] ) {
1686 return false;
1687 }
1688 }
1689 return true;
1690 }
1691
Compare(const idVecX & a,const float epsilon)1692 ID_INLINE bool idVecX::Compare( const idVecX &a, const float epsilon ) const {
1693 int i;
1694
1695 assert( size == a.size );
1696 for ( i = 0; i < size; i++ ) {
1697 if ( idMath::Fabs( p[i] - a.p[i] ) > epsilon ) {
1698 return false;
1699 }
1700 }
1701 return true;
1702 }
1703
1704 ID_INLINE bool idVecX::operator==( const idVecX &a ) const {
1705 return Compare( a );
1706 }
1707
1708 ID_INLINE bool idVecX::operator!=( const idVecX &a ) const {
1709 return !Compare( a );
1710 }
1711
SetSize(int newSize)1712 ID_INLINE void idVecX::SetSize( int newSize ) {
1713 int alloc = ( newSize + 3 ) & ~3;
1714 if ( alloc > alloced && alloced != -1 ) {
1715 if ( p ) {
1716 Mem_Free16( p );
1717 }
1718 p = (float *) Mem_Alloc16( alloc * sizeof( float ) );
1719 alloced = alloc;
1720 }
1721 size = newSize;
1722 VECX_CLEAREND();
1723 }
1724
ChangeSize(int newSize,bool makeZero)1725 ID_INLINE void idVecX::ChangeSize( int newSize, bool makeZero ) {
1726 int alloc = ( newSize + 3 ) & ~3;
1727 if ( alloc > alloced && alloced != -1 ) {
1728 float *oldVec = p;
1729 p = (float *) Mem_Alloc16( alloc * sizeof( float ) );
1730 alloced = alloc;
1731 if ( oldVec ) {
1732 for ( int i = 0; i < size; i++ ) {
1733 p[i] = oldVec[i];
1734 }
1735 Mem_Free16( oldVec );
1736 }
1737 if ( makeZero ) {
1738 // zero any new elements
1739 for ( int i = size; i < newSize; i++ ) {
1740 p[i] = 0.0f;
1741 }
1742 }
1743 }
1744 size = newSize;
1745 VECX_CLEAREND();
1746 }
1747
SetTempSize(int newSize)1748 ID_INLINE void idVecX::SetTempSize( int newSize ) {
1749
1750 size = newSize;
1751 alloced = ( newSize + 3 ) & ~3;
1752 assert( alloced < VECX_MAX_TEMP );
1753 if ( idVecX::tempIndex + alloced > VECX_MAX_TEMP ) {
1754 idVecX::tempIndex = 0;
1755 }
1756 p = idVecX::tempPtr + idVecX::tempIndex;
1757 idVecX::tempIndex += alloced;
1758 VECX_CLEAREND();
1759 }
1760
SetData(int length,float * data)1761 ID_INLINE void idVecX::SetData( int length, float *data ) {
1762 if ( p && ( p < idVecX::tempPtr || p >= idVecX::tempPtr + VECX_MAX_TEMP ) && alloced != -1 ) {
1763 Mem_Free16( p );
1764 }
1765 assert( ( ( (uintptr_t) data ) & 15 ) == 0 ); // data must be 16 byte aligned
1766 p = data;
1767 size = length;
1768 alloced = -1;
1769 VECX_CLEAREND();
1770 }
1771
Zero(void)1772 ID_INLINE void idVecX::Zero( void ) {
1773 #ifdef VECX_SIMD
1774 SIMDProcessor->Zero16( p, size );
1775 #else
1776 memset( p, 0, size * sizeof( float ) );
1777 #endif
1778 }
1779
Zero(int length)1780 ID_INLINE void idVecX::Zero( int length ) {
1781 SetSize( length );
1782 #ifdef VECX_SIMD
1783 SIMDProcessor->Zero16( p, length );
1784 #else
1785 memset( p, 0, size * sizeof( float ) );
1786 #endif
1787 }
1788
Random(int seed,float l,float u)1789 ID_INLINE void idVecX::Random( int seed, float l, float u ) {
1790 int i;
1791 float c;
1792 idRandom rnd( seed );
1793
1794 c = u - l;
1795 for ( i = 0; i < size; i++ ) {
1796 p[i] = l + rnd.RandomFloat() * c;
1797 }
1798 }
1799
Random(int length,int seed,float l,float u)1800 ID_INLINE void idVecX::Random( int length, int seed, float l, float u ) {
1801 int i;
1802 float c;
1803 idRandom rnd( seed );
1804
1805 SetSize( length );
1806 c = u - l;
1807 for ( i = 0; i < size; i++ ) {
1808 p[i] = l + rnd.RandomFloat() * c;
1809 }
1810 }
1811
Negate(void)1812 ID_INLINE void idVecX::Negate( void ) {
1813 #ifdef VECX_SIMD
1814 SIMDProcessor->Negate16( p, size );
1815 #else
1816 int i;
1817 for ( i = 0; i < size; i++ ) {
1818 p[i] = -p[i];
1819 }
1820 #endif
1821 }
1822
Clamp(float min,float max)1823 ID_INLINE void idVecX::Clamp( float min, float max ) {
1824 int i;
1825 for ( i = 0; i < size; i++ ) {
1826 if ( p[i] < min ) {
1827 p[i] = min;
1828 } else if ( p[i] > max ) {
1829 p[i] = max;
1830 }
1831 }
1832 }
1833
SwapElements(int e1,int e2)1834 ID_INLINE idVecX &idVecX::SwapElements( int e1, int e2 ) {
1835 float tmp;
1836 tmp = p[e1];
1837 p[e1] = p[e2];
1838 p[e2] = tmp;
1839 return *this;
1840 }
1841
Length(void)1842 ID_INLINE float idVecX::Length( void ) const {
1843 int i;
1844 float sum = 0.0f;
1845
1846 for ( i = 0; i < size; i++ ) {
1847 sum += p[i] * p[i];
1848 }
1849 return idMath::Sqrt( sum );
1850 }
1851
LengthSqr(void)1852 ID_INLINE float idVecX::LengthSqr( void ) const {
1853 int i;
1854 float sum = 0.0f;
1855
1856 for ( i = 0; i < size; i++ ) {
1857 sum += p[i] * p[i];
1858 }
1859 return sum;
1860 }
1861
Normalize(void)1862 ID_INLINE idVecX idVecX::Normalize( void ) const {
1863 int i;
1864 idVecX m;
1865 float invSqrt, sum = 0.0f;
1866
1867 m.SetTempSize( size );
1868 for ( i = 0; i < size; i++ ) {
1869 sum += p[i] * p[i];
1870 }
1871 invSqrt = idMath::InvSqrt( sum );
1872 for ( i = 0; i < size; i++ ) {
1873 m.p[i] = p[i] * invSqrt;
1874 }
1875 return m;
1876 }
1877
NormalizeSelf(void)1878 ID_INLINE float idVecX::NormalizeSelf( void ) {
1879 float invSqrt, sum = 0.0f;
1880 int i;
1881 for ( i = 0; i < size; i++ ) {
1882 sum += p[i] * p[i];
1883 }
1884 invSqrt = idMath::InvSqrt( sum );
1885 for ( i = 0; i < size; i++ ) {
1886 p[i] *= invSqrt;
1887 }
1888 return invSqrt * sum;
1889 }
1890
GetDimension(void)1891 ID_INLINE int idVecX::GetDimension( void ) const {
1892 return size;
1893 }
1894
SubVec3(int index)1895 ID_INLINE idVec3 &idVecX::SubVec3( int index ) {
1896 assert( index >= 0 && index * 3 + 3 <= size );
1897 return *reinterpret_cast<idVec3 *>(p + index * 3);
1898 }
1899
SubVec3(int index)1900 ID_INLINE const idVec3 &idVecX::SubVec3( int index ) const {
1901 assert( index >= 0 && index * 3 + 3 <= size );
1902 return *reinterpret_cast<const idVec3 *>(p + index * 3);
1903 }
1904
SubVec6(int index)1905 ID_INLINE idVec6 &idVecX::SubVec6( int index ) {
1906 assert( index >= 0 && index * 6 + 6 <= size );
1907 return *reinterpret_cast<idVec6 *>(p + index * 6);
1908 }
1909
SubVec6(int index)1910 ID_INLINE const idVec6 &idVecX::SubVec6( int index ) const {
1911 assert( index >= 0 && index * 6 + 6 <= size );
1912 return *reinterpret_cast<const idVec6 *>(p + index * 6);
1913 }
1914
ToFloatPtr(void)1915 ID_INLINE const float *idVecX::ToFloatPtr( void ) const {
1916 return p;
1917 }
1918
ToFloatPtr(void)1919 ID_INLINE float *idVecX::ToFloatPtr( void ) {
1920 return p;
1921 }
1922
1923
1924 //===============================================================
1925 //
1926 // idPolar3
1927 //
1928 //===============================================================
1929
1930 class idPolar3 {
1931 public:
1932 float radius, theta, phi;
1933
1934 idPolar3( void );
1935 explicit idPolar3( const float radius, const float theta, const float phi );
1936
1937 void Set( const float radius, const float theta, const float phi );
1938
1939 float operator[]( const int index ) const;
1940 float & operator[]( const int index );
1941 idPolar3 operator-() const;
1942 idPolar3 & operator=( const idPolar3 &a );
1943
1944 idVec3 ToVec3( void ) const;
1945 };
1946
idPolar3(void)1947 ID_INLINE idPolar3::idPolar3( void ) {
1948 }
1949
idPolar3(const float radius,const float theta,const float phi)1950 ID_INLINE idPolar3::idPolar3( const float radius, const float theta, const float phi ) {
1951 assert( radius > 0 );
1952 this->radius = radius;
1953 this->theta = theta;
1954 this->phi = phi;
1955 }
1956
Set(const float radius,const float theta,const float phi)1957 ID_INLINE void idPolar3::Set( const float radius, const float theta, const float phi ) {
1958 assert( radius > 0 );
1959 this->radius = radius;
1960 this->theta = theta;
1961 this->phi = phi;
1962 }
1963
1964 ID_INLINE float idPolar3::operator[]( const int index ) const {
1965 return ( &radius )[ index ];
1966 }
1967
1968 ID_INLINE float &idPolar3::operator[]( const int index ) {
1969 return ( &radius )[ index ];
1970 }
1971
1972 ID_INLINE idPolar3 idPolar3::operator-() const {
1973 return idPolar3( radius, -theta, -phi );
1974 }
1975
1976 ID_INLINE idPolar3 &idPolar3::operator=( const idPolar3 &a ) {
1977 radius = a.radius;
1978 theta = a.theta;
1979 phi = a.phi;
1980 return *this;
1981 }
1982
ToVec3(void)1983 ID_INLINE idVec3 idPolar3::ToVec3( void ) const {
1984 float sp, cp, st, ct;
1985 idMath::SinCos( phi, sp, cp );
1986 idMath::SinCos( theta, st, ct );
1987 return idVec3( cp * radius * ct, cp * radius * st, radius * sp );
1988 }
1989
1990
1991 /*
1992 ===============================================================================
1993
1994 Old 3D vector macros, should no longer be used.
1995
1996 ===============================================================================
1997 */
1998
1999 #define DotProduct( a, b) ((a)[0]*(b)[0]+(a)[1]*(b)[1]+(a)[2]*(b)[2])
2000 #define VectorSubtract( a, b, c ) ((c)[0]=(a)[0]-(b)[0],(c)[1]=(a)[1]-(b)[1],(c)[2]=(a)[2]-(b)[2])
2001 #define VectorAdd( a, b, c ) ((c)[0]=(a)[0]+(b)[0],(c)[1]=(a)[1]+(b)[1],(c)[2]=(a)[2]+(b)[2])
2002 #define VectorScale( v, s, o ) ((o)[0]=(v)[0]*(s),(o)[1]=(v)[1]*(s),(o)[2]=(v)[2]*(s))
2003 #define VectorMA( v, s, b, o ) ((o)[0]=(v)[0]+(b)[0]*(s),(o)[1]=(v)[1]+(b)[1]*(s),(o)[2]=(v)[2]+(b)[2]*(s))
2004 #define VectorCopy( a, b ) ((b)[0]=(a)[0],(b)[1]=(a)[1],(b)[2]=(a)[2])
2005
2006
2007 #endif /* !__MATH_VECTOR_H__ */
2008