misc/mediasource/netradiant-src/libs/splines/math_vector.h

   1 /*
   2 Copyright (C) 1999-2006 Id Software, Inc. and contributors.
   3 For a list of contributors, see the accompanying CONTRIBUTORS file.
   4
   5 This file is part of GtkRadiant.
   6
   7 GtkRadiant is free software; you can redistribute it and/or modify
   8 it under the terms of the GNU General Public License as published by
   9 the Free Software Foundation; either version 2 of the License, or
  10 (at your option) any later version.
  11
  12 GtkRadiant is distributed in the hope that it will be useful,
  13 but WITHOUT ANY WARRANTY; without even the implied warranty of
  14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15 GNU General Public License for more details.
  16
  17 You should have received a copy of the GNU General Public License
  18 along with GtkRadiant; if not, write to the Free Software
  19 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  20 */
  21
  22 #ifndef __MATH_VECTOR_H__
  23 #define __MATH_VECTOR_H__
  24
  25 #ifdef WIN32
  26 #pragma warning(disable : 4244)
  27 #endif
  28
  29 #include <math.h>
  30 #include <assert.h>
  31
  32 //#define DotProduct(a,b)                       ((a)[0]*(b)[0]+(a)[1]*(b)[1]+(a)[2]*(b)[2])
  33 //#define VectorSubtract(a,b,c) ((c)[0]=(a)[0]-(b)[0],(c)[1]=(a)[1]-(b)[1],(c)[2]=(a)[2]-(b)[2])
  34 //#define VectorAdd(a,b,c)              ((c)[0]=(a)[0]+(b)[0],(c)[1]=(a)[1]+(b)[1],(c)[2]=(a)[2]+(b)[2])
  35 //#define VectorCopy(a,b)                       ((b)[0]=(a)[0],(b)[1]=(a)[1],(b)[2]=(a)[2])
  36 //#define VectorCopy(a,b)                       ((b).x=(a).x,(b).y=(a).y,(b).z=(a).z])
  37
  38 //#define       VectorScale(v, s, o)    ((o)[0]=(v)[0]*(s),(o)[1]=(v)[1]*(s),(o)[2]=(v)[2]*(s))
  39 #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))
  40 //#define CrossProduct(a,b,c)           ((c)[0]=(a)[1]*(b)[2]-(a)[2]*(b)[1],(c)[1]=(a)[2]*(b)[0]-(a)[0]*(b)[2],(c)[2]=(a)[0]*(b)[1]-(a)[1]*(b)[0])
  41
  42 #define DotProduct4(x,y)                ((x)[0]*(y)[0]+(x)[1]*(y)[1]+(x)[2]*(y)[2]+(x)[3]*(y)[3])
  43 #define VectorSubtract4(a,b,c)  ((c)[0]=(a)[0]-(b)[0],(c)[1]=(a)[1]-(b)[1],(c)[2]=(a)[2]-(b)[2],(c)[3]=(a)[3]-(b)[3])
  44 #define VectorAdd4(a,b,c)               ((c)[0]=(a)[0]+(b)[0],(c)[1]=(a)[1]+(b)[1],(c)[2]=(a)[2]+(b)[2],(c)[3]=(a)[3]+(b)[3])
  45 #define VectorCopy4(a,b)                ((b)[0]=(a)[0],(b)[1]=(a)[1],(b)[2]=(a)[2],(b)[3]=(a)[3])
  46 #define VectorScale4(v, s, o)   ((o)[0]=(v)[0]*(s),(o)[1]=(v)[1]*(s),(o)[2]=(v)[2]*(s),(o)[3]=(v)[3]*(s))
  47 #define VectorMA4(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),(o)[3]=(v)[3]+(b)[3]*(s))
  48
  49
  50 //#define VectorClear(a)                        ((a)[0]=(a)[1]=(a)[2]=0)
  51 #define VectorNegate(a,b)               ((b)[0]=-(a)[0],(b)[1]=-(a)[1],(b)[2]=-(a)[2])
  52 //#define VectorSet(v, x, y, z) ((v)[0]=(x), (v)[1]=(y), (v)[2]=(z))
  53 #define Vector4Copy(a,b)                ((b)[0]=(a)[0],(b)[1]=(a)[1],(b)[2]=(a)[2],(b)[3]=(a)[3])
  54
  55 #define SnapVector(v) {v[0]=(int)v[0];v[1]=(int)v[1];v[2]=(int)v[2];}
  56
  57
  58 //#include "util_heap.h"
  59
  60 #ifndef EQUAL_EPSILON
  61 #define EQUAL_EPSILON   0.001
  62 #endif
  63
  64 float Q_fabs( float f );
  65
  66 #ifndef ID_INLINE
  67 #ifdef _WIN32
  68 #define ID_INLINE __inline
  69 #else
  70 #define ID_INLINE inline
  71 #endif
  72 #endif
  73
  74 // if this is defined, vec3 will take four elements, which may allow
  75 // easier SIMD optimizations
  76 //#define       FAT_VEC3
  77 //#ifdef __ppc__
  78 //#pragma align(16)
  79 //#endif
  80
  81 class angles_t;
  82 #ifdef __ppc__
  83 // Vanilla PPC code, but since PPC has a reciprocal square root estimate instruction,
  84 // runs *much* faster than calling sqrt(). We'll use two Newton-Raphson
  85 // refinement steps to get bunch more precision in the 1/sqrt() value for very little cost.
  86 // We'll then multiply 1/sqrt times the original value to get the sqrt.
  87 // This is about 12.4 times faster than sqrt() and according to my testing (not exhaustive)
  88 // it returns fairly accurate results (error below 1.0e-5 up to 100000.0 in 0.1 increments).
  89
  90 static inline float idSqrt(float x) {
  91     const float half = 0.5;
  92     const float one = 1.0;
  93     float B, y0, y1;
  94
  95     // This'll NaN if it hits frsqrte. Handle both +0.0 and -0.0
  96     if (fabs(x) == 0.0)
  97         return x;
  98     B = x;
  99
 100 #ifdef __GNUC__
 101     asm("frsqrte %0,%1" : "=f" (y0) : "f" (B));
 102 #else
 103     y0 = __frsqrte(B);
 104 #endif
 105     /* First refinement step */
 106
 107     y1 = y0 + half*y0*(one - B*y0*y0);
 108
 109     /* Second refinement step -- copy the output of the last step to the input of this step */
 110
 111     y0 = y1;
 112     y1 = y0 + half*y0*(one - B*y0*y0);
 113
 114     /* Get sqrt(x) from x * 1/sqrt(x) */
 115     return x * y1;
 116 }
 117 #else
 118 static inline double idSqrt(double x) {
 119     return sqrt(x);
 120 }
 121 #endif
 122
 123
 124 //class idVec3  : public idHeap<idVec3> {
 125 class idVec3 {
 126 public:
 127 #ifndef FAT_VEC3
 128             float x,y,z;
 129 #else
 130             float x,y,z,dist;
 131 #endif
 132
 133 #ifndef FAT_VEC3
 134                                         idVec3() {};
 135 #else
 136                                         idVec3() {dist = 0.0f;};
 137 #endif
 138                                         idVec3( const float x, const float y, const float z );
 139
 140                                         operator float *();
 141
 142         float                   operator[]( const int index ) const;
 143         float                   &operator[]( const int index );
 144
 145         void                    set( const float x, const float y, const float z );
 146
 147         idVec3                  operator-() const;
 148
 149         idVec3                  &operator=( const idVec3 &a );
 150
 151         float                   operator*( const idVec3 &a ) const;
 152         idVec3                  operator*( const float a ) const;
 153         friend idVec3   operator*( float a, idVec3 b );
 154
 155         idVec3                  operator+( const idVec3 &a ) const;
 156         idVec3                  operator-( const idVec3 &a ) const;
 157
 158         idVec3                  &operator+=( const idVec3 &a );
 159         idVec3                  &operator-=( const idVec3 &a );
 160         idVec3                  &operator*=( const float a );
 161
 162         int                             operator==(     const idVec3 &a ) const;
 163         int                             operator!=(     const idVec3 &a ) const;
 164
 165         idVec3                  Cross( const idVec3 &a ) const;
 166         idVec3                  &Cross( const idVec3 &a, const idVec3 &b );
 167
 168         float                   Length( void ) const;
 169         float                   Normalize( void );
 170
 171         void                    Zero( void );
 172         void                    Snap( void );
 173         void                    SnapTowards( const idVec3 &to );
 174
 175         float                   toYaw( void );
 176         float                   toPitch( void );
 177         angles_t                toAngles( void );
 178         friend idVec3   LerpVector( const idVec3 &w1, const idVec3 &w2, const float t );
 179
 180         char                    *string( void );
 181 };
 182
 183 extern idVec3 vec_zero;
 184
 185 ID_INLINE idVec3::idVec3( const float x, const float y, const float z ) {
 186         this->x = x;
 187         this->y = y;
 188         this->z = z;
 189 #ifdef  FAT_VEC3
 190         this->dist = 0.0f;
 191 #endif
 192 }
 193
 194 ID_INLINE float idVec3::operator[]( const int index ) const {
 195         return ( &x )[ index ];
 196 }
 197
 198 ID_INLINE float &idVec3::operator[]( const int index ) {
 199         return ( &x )[ index ];
 200 }
 201
 202 ID_INLINE idVec3::operator float *( void ) {
 203         return &x;
 204 }
 205
 206 ID_INLINE idVec3 idVec3::operator-() const {
 207         return idVec3( -x, -y, -z );
 208 }
 209
 210 ID_INLINE idVec3 &idVec3::operator=( const idVec3 &a ) {
 211         x = a.x;
 212         y = a.y;
 213         z = a.z;
 214
 215         return *this;
 216 }
 217
 218 ID_INLINE void idVec3::set( const float x, const float y, const float z ) {
 219         this->x = x;
 220         this->y = y;
 221         this->z = z;
 222 }
 223
 224 ID_INLINE idVec3 idVec3::operator-( const idVec3 &a ) const {
 225         return idVec3( x - a.x, y - a.y, z - a.z );
 226 }
 227
 228 ID_INLINE float idVec3::operator*( const idVec3 &a ) const {
 229         return x * a.x + y * a.y + z * a.z;
 230 }
 231
 232 ID_INLINE idVec3 idVec3::operator*( const float a ) const {
 233         return idVec3( x * a, y * a, z * a );
 234 }
 235
 236 ID_INLINE idVec3 operator*( const float a, const idVec3 b ) {
 237         return idVec3( b.x * a, b.y * a, b.z * a );
 238 }
 239
 240 ID_INLINE idVec3 idVec3::operator+( const idVec3 &a ) const {
 241         return idVec3( x + a.x, y + a.y, z + a.z );
 242 }
 243
 244 ID_INLINE idVec3 &idVec3::operator+=( const idVec3 &a ) {
 245         x += a.x;
 246         y += a.y;
 247         z += a.z;
 248
 249         return *this;
 250 }
 251
 252 ID_INLINE idVec3 &idVec3::operator-=( const idVec3 &a ) {
 253         x -= a.x;
 254         y -= a.y;
 255         z -= a.z;
 256
 257         return *this;
 258 }
 259
 260 ID_INLINE idVec3 &idVec3::operator*=( const float a ) {
 261         x *= a;
 262         y *= a;
 263         z *= a;
 264
 265         return *this;
 266 }
 267
 268 ID_INLINE int idVec3::operator==( const idVec3 &a ) const {
 269         if ( Q_fabs( x - a.x ) > EQUAL_EPSILON ) {
 270                 return false;
 271         }
 272
 273         if ( Q_fabs( y - a.y ) > EQUAL_EPSILON ) {
 274                 return false;
 275         }
 276
 277         if ( Q_fabs( z - a.z ) > EQUAL_EPSILON ) {
 278                 return false;
 279         }
 280
 281         return true;
 282 }
 283
 284 ID_INLINE int idVec3::operator!=( const idVec3 &a ) const {
 285         if ( Q_fabs( x - a.x ) > EQUAL_EPSILON ) {
 286                 return true;
 287         }
 288
 289         if ( Q_fabs( y - a.y ) > EQUAL_EPSILON ) {
 290                 return true;
 291         }
 292
 293         if ( Q_fabs( z - a.z ) > EQUAL_EPSILON ) {
 294                 return true;
 295         }
 296
 297         return false;
 298 }
 299
 300 ID_INLINE idVec3 idVec3::Cross( const idVec3 &a ) const {
 301         return idVec3( y * a.z - z * a.y, z * a.x - x * a.z, x * a.y - y * a.x );
 302 }
 303
 304 ID_INLINE idVec3 &idVec3::Cross( const idVec3 &a, const idVec3 &b ) {
 305         x = a.y * b.z - a.z * b.y;
 306         y = a.z * b.x - a.x * b.z;
 307         z = a.x * b.y - a.y * b.x;
 308
 309         return *this;
 310 }
 311
 312 ID_INLINE float idVec3::Length( void ) const {
 313         float length;
 314
 315         length = x * x + y * y + z * z;
 316         return ( float )idSqrt( length );
 317 }
 318
 319 ID_INLINE float idVec3::Normalize( void ) {
 320         float length;
 321         float ilength;
 322
 323         length = this->Length();
 324         if ( length ) {
 325                 ilength = 1.0f / length;
 326                 x *= ilength;
 327                 y *= ilength;
 328                 z *= ilength;
 329         }
 330
 331         return length;
 332 }
 333
 334 ID_INLINE void idVec3::Zero( void ) {
 335         x = 0.0f;
 336         y = 0.0f;
 337         z = 0.0f;
 338 }
 339
 340 ID_INLINE void idVec3::Snap( void ) {
 341         x = float( int( x ) );
 342         y = float( int( y ) );
 343         z = float( int( z ) );
 344 }
 345
 346 /*
 347 ======================
 348 SnapTowards
 349
 350 Round a vector to integers for more efficient network
 351 transmission, but make sure that it rounds towards a given point
 352 rather than blindly truncating.  This prevents it from truncating
 353 into a wall.
 354 ======================
 355 */
 356 ID_INLINE void idVec3::SnapTowards( const idVec3 &to ) {
 357         if ( to.x <= x ) {
 358                 x = float( int( x ) );
 359         } else {
 360                 x = float( int( x ) + 1 );
 361         }
 362
 363         if ( to.y <= y ) {
 364                 y = float( int( y ) );
 365         } else {
 366                 y = float( int( y ) + 1 );
 367         }
 368
 369         if ( to.z <= z ) {
 370                 z = float( int( z ) );
 371         } else {
 372                 z = float( int( z ) + 1 );
 373         }
 374 }
 375
 376 //===============================================================
 377
 378 class Bounds {
 379 public:
 380         idVec3  b[2];
 381
 382                         Bounds();
 383                         Bounds( const idVec3 &mins, const idVec3 &maxs );
 384
 385         void    Clear();
 386         void    Zero();
 387         float   Radius();               // radius from origin, not from center
 388         idVec3  Center();
 389         void    AddPoint( const idVec3 &v );
 390         void    AddBounds( const Bounds &bb );
 391         bool    IsCleared();
 392         bool    ContainsPoint( const idVec3 &p );
 393         bool    IntersectsBounds( const Bounds &b2 );   // touching is NOT intersecting
 394 };
 395
 396 extern Bounds   boundsZero;
 397
 398 ID_INLINE Bounds::Bounds(){
 399 }
 400
 401 ID_INLINE bool Bounds::IsCleared() {
 402         return b[0][0] > b[1][0];
 403 }
 404
 405 ID_INLINE bool Bounds::ContainsPoint( const idVec3 &p ) {
 406         if ( p[0] < b[0][0] || p[1] < b[0][1] || p[2] < b[0][2]
 407                 || p[0] > b[1][0] || p[1] > b[1][1] || p[2] > b[1][2] ) {
 408                 return false;
 409         }
 410         return true;
 411 }
 412
 413 ID_INLINE bool Bounds::IntersectsBounds( const Bounds &b2 ) {
 414         if ( b2.b[1][0] < b[0][0] || b2.b[1][1] < b[0][1] || b2.b[1][2] < b[0][2]
 415                 || b2.b[0][0] > b[1][0] || b2.b[0][1] > b[1][1] || b2.b[0][2] > b[1][2] ) {
 416                 return false;
 417         }
 418         return true;
 419 }
 420
 421 ID_INLINE Bounds::Bounds( const idVec3 &mins, const idVec3 &maxs ) {
 422         b[0] = mins;
 423         b[1] = maxs;
 424 }
 425
 426 ID_INLINE idVec3 Bounds::Center() {
 427         return idVec3( ( b[1][0] + b[0][0] ) * 0.5f, ( b[1][1] + b[0][1] ) * 0.5f, ( b[1][2] + b[0][2] ) * 0.5f );
 428 }
 429
 430 ID_INLINE void Bounds::Clear() {
 431         b[0][0] = b[0][1] = b[0][2] = 99999;
 432         b[1][0] = b[1][1] = b[1][2] = -99999;
 433 }
 434
 435 ID_INLINE void Bounds::Zero() {
 436         b[0][0] = b[0][1] = b[0][2] =
 437         b[1][0] = b[1][1] = b[1][2] = 0;
 438 }
 439
 440 ID_INLINE void Bounds::AddPoint( const idVec3 &v ) {
 441         if ( v[0] < b[0][0]) {
 442                 b[0][0] = v[0];
 443         }
 444         if ( v[0] > b[1][0]) {
 445                 b[1][0] = v[0];
 446         }
 447         if ( v[1] < b[0][1] ) {
 448                 b[0][1] = v[1];
 449         }
 450         if ( v[1] > b[1][1]) {
 451                 b[1][1] = v[1];
 452         }
 453         if ( v[2] < b[0][2] ) {
 454                 b[0][2] = v[2];
 455         }
 456         if ( v[2] > b[1][2]) {
 457                 b[1][2] = v[2];
 458         }
 459 }
 460
 461
 462 ID_INLINE void Bounds::AddBounds( const Bounds &bb ) {
 463         if ( bb.b[0][0] < b[0][0]) {
 464                 b[0][0] = bb.b[0][0];
 465         }
 466         if ( bb.b[0][1] < b[0][1]) {
 467                 b[0][1] = bb.b[0][1];
 468         }
 469         if ( bb.b[0][2] < b[0][2]) {
 470                 b[0][2] = bb.b[0][2];
 471         }
 472
 473         if ( bb.b[1][0] > b[1][0]) {
 474                 b[1][0] = bb.b[1][0];
 475         }
 476         if ( bb.b[1][1] > b[1][1]) {
 477                 b[1][1] = bb.b[1][1];
 478         }
 479         if ( bb.b[1][2] > b[1][2]) {
 480                 b[1][2] = bb.b[1][2];
 481         }
 482 }
 483
 484 ID_INLINE float Bounds::Radius( ) {
 485         int             i;
 486         float   total;
 487         float   a, aa;
 488
 489         total = 0;
 490         for (i=0 ; i<3 ; i++) {
 491                 a = (float)fabs( b[0][i] );
 492                 aa = (float)fabs( b[1][i] );
 493                 if ( aa > a ) {
 494                         a = aa;
 495                 }
 496                 total += a * a;
 497         }
 498
 499         return (float)idSqrt( total );
 500 }
 501
 502 //===============================================================
 503
 504
 505 class idVec2 {
 506 public:
 507         float                   x;
 508         float                   y;
 509
 510                                         operator float *();
 511         float                   operator[]( int index ) const;
 512         float                   &operator[]( int index );
 513 };
 514
 515 ID_INLINE float idVec2::operator[]( int index ) const {
 516         return ( &x )[ index ];
 517 }
 518
 519 ID_INLINE float& idVec2::operator[]( int index ) {
 520         return ( &x )[ index ];
 521 }
 522
 523 ID_INLINE idVec2::operator float *( void ) {
 524         return &x;
 525 }
 526
 527 class idVec4 : public idVec3 {
 528 public:
 529 #ifndef FAT_VEC3
 530         float                   dist;
 531 #endif
 532         idVec4();
 533         ~idVec4() {};
 534
 535         idVec4( float x, float y, float z, float dist );
 536         float                   operator[]( int index ) const;
 537         float                   &operator[]( int index );
 538 };
 539
 540 ID_INLINE idVec4::idVec4() {}
 541 ID_INLINE idVec4::idVec4( float x, float y, float z, float dist ) {
 542         this->x = x;
 543         this->y = y;
 544         this->z = z;
 545         this->dist = dist;
 546 }
 547
 548 ID_INLINE float idVec4::operator[]( int index ) const {
 549         return ( &x )[ index ];
 550 }
 551
 552 ID_INLINE float& idVec4::operator[]( int index ) {
 553         return ( &x )[ index ];
 554 }
 555
 556
 557 class idVec5_t : public idVec3 {
 558 public:
 559         float                   s;
 560         float                   t;
 561         float                   operator[]( int index ) const;
 562         float                   &operator[]( int index );
 563 };
 564
 565
 566 ID_INLINE float idVec5_t::operator[]( int index ) const {
 567         return ( &x )[ index ];
 568 }
 569
 570 ID_INLINE float& idVec5_t::operator[]( int index ) {
 571         return ( &x )[ index ];
 572 }
 573
 574 #endif /* !__MATH_VECTOR_H__ */