neo/idlib/math/Vector.h

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