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         }
  99         B = x;
 100
 101 #ifdef __GNUC__
 102         asm ( "frsqrte %0,%1" : "=f" ( y0 ) : "f" ( B ) );
 103 #else
 104         y0 = __frsqrte( B );
 105 #endif
 106         /* First refinement step */
 107
 108         y1 = y0 + half * y0 * ( one - B * y0 * y0 );
 109
 110         /* Second refinement step -- copy the output of the last step to the input of this step */
 111
 112         y0 = y1;
 113         y1 = y0 + half * y0 * ( one - B * y0 * y0 );
 114
 115         /* Get sqrt(x) from x * 1/sqrt(x) */
 116         return x * y1;
 117 }
 118 #else
 119 static inline double idSqrt( double x ) {
 120         return sqrt( x );
 121 }
 122 #endif
 123
 124
 125 //class idVec3  : public idHeap<idVec3> {
 126 class idVec3 {
 127 public:
 128 #ifndef FAT_VEC3
 129 float x,y,z;
 130 #else
 131 float x,y,z,dist;
 132 #endif
 133
 134 #ifndef FAT_VEC3
 135 idVec3() {};
 136 #else
 137 idVec3() {dist = 0.0f; };
 138 #endif
 139 idVec3( const float x, const float y, const float z );
 140
 141 operator float *();
 142
 143 float operator[]( const int index ) const;
 144 float           &operator[]( const int index );
 145
 146 void            set( const float x, const float y, const float z );
 147
 148 idVec3 operator-() const;
 149
 150 idVec3          &operator=( const idVec3 &a );
 151
 152 float operator*( const idVec3 &a ) const;
 153 idVec3 operator*( const float a ) const;
 154 friend idVec3 operator*( float a, idVec3 b );
 155
 156 idVec3 operator+( const idVec3 &a ) const;
 157 idVec3 operator-( const idVec3 &a ) const;
 158
 159 idVec3          &operator+=( const idVec3 &a );
 160 idVec3          &operator-=( const idVec3 &a );
 161 idVec3          &operator*=( const float a );
 162
 163 int operator==( const idVec3 &a ) const;
 164 int operator!=( const idVec3 &a ) const;
 165
 166 idVec3          Cross( const idVec3 &a ) const;
 167 idVec3          &Cross( const idVec3 &a, const idVec3 &b );
 168
 169 float           Length( void ) const;
 170 float           Normalize( void );
 171
 172 void            Zero( void );
 173 void            Snap( void );
 174 void            SnapTowards( const idVec3 &to );
 175
 176 float           toYaw( void );
 177 float           toPitch( void );
 178 angles_t        toAngles( void );
 179 friend idVec3   LerpVector( const idVec3 &w1, const idVec3 &w2, const float t );
 180
 181 char            *string( void );
 182 };
 183
 184 extern idVec3 vec_zero;
 185
 186 ID_INLINE idVec3::idVec3( const float x, const float y, const float z ) {
 187         this->x = x;
 188         this->y = y;
 189         this->z = z;
 190 #ifdef  FAT_VEC3
 191         this->dist = 0.0f;
 192 #endif
 193 }
 194
 195 ID_INLINE float idVec3::operator[]( const int index ) const {
 196         return ( &x )[ index ];
 197 }
 198
 199 ID_INLINE float &idVec3::operator[]( const int index ) {
 200         return ( &x )[ index ];
 201 }
 202
 203 ID_INLINE idVec3::operator float *( void ) {
 204         return &x;
 205 }
 206
 207 ID_INLINE idVec3 idVec3::operator-() const {
 208         return idVec3( -x, -y, -z );
 209 }
 210
 211 ID_INLINE idVec3 &idVec3::operator=( const idVec3 &a ) {
 212         x = a.x;
 213         y = a.y;
 214         z = a.z;
 215
 216         return *this;
 217 }
 218
 219 ID_INLINE void idVec3::set( const float x, const float y, const float z ) {
 220         this->x = x;
 221         this->y = y;
 222         this->z = z;
 223 }
 224
 225 ID_INLINE idVec3 idVec3::operator-( const idVec3 &a ) const {
 226         return idVec3( x - a.x, y - a.y, z - a.z );
 227 }
 228
 229 ID_INLINE float idVec3::operator*( const idVec3 &a ) const {
 230         return x * a.x + y * a.y + z * a.z;
 231 }
 232
 233 ID_INLINE idVec3 idVec3::operator*( const float a ) const {
 234         return idVec3( x * a, y * a, z * a );
 235 }
 236
 237 ID_INLINE idVec3 operator*( const float a, const idVec3 b ) {
 238         return idVec3( b.x * a, b.y * a, b.z * a );
 239 }
 240
 241 ID_INLINE idVec3 idVec3::operator+( const idVec3 &a ) const {
 242         return idVec3( x + a.x, y + a.y, z + a.z );
 243 }
 244
 245 ID_INLINE idVec3 &idVec3::operator+=( const idVec3 &a ) {
 246         x += a.x;
 247         y += a.y;
 248         z += a.z;
 249
 250         return *this;
 251 }
 252
 253 ID_INLINE idVec3 &idVec3::operator-=( const idVec3 &a ) {
 254         x -= a.x;
 255         y -= a.y;
 256         z -= a.z;
 257
 258         return *this;
 259 }
 260
 261 ID_INLINE idVec3 &idVec3::operator*=( const float a ) {
 262         x *= a;
 263         y *= a;
 264         z *= a;
 265
 266         return *this;
 267 }
 268
 269 ID_INLINE int idVec3::operator==( const idVec3 &a ) const {
 270         if ( Q_fabs( x - a.x ) > EQUAL_EPSILON ) {
 271                 return false;
 272         }
 273
 274         if ( Q_fabs( y - a.y ) > EQUAL_EPSILON ) {
 275                 return false;
 276         }
 277
 278         if ( Q_fabs( z - a.z ) > EQUAL_EPSILON ) {
 279                 return false;
 280         }
 281
 282         return true;
 283 }
 284
 285 ID_INLINE int idVec3::operator!=( const idVec3 &a ) const {
 286         if ( Q_fabs( x - a.x ) > EQUAL_EPSILON ) {
 287                 return true;
 288         }
 289
 290         if ( Q_fabs( y - a.y ) > EQUAL_EPSILON ) {
 291                 return true;
 292         }
 293
 294         if ( Q_fabs( z - a.z ) > EQUAL_EPSILON ) {
 295                 return true;
 296         }
 297
 298         return false;
 299 }
 300
 301 ID_INLINE idVec3 idVec3::Cross( const idVec3 &a ) const {
 302         return idVec3( y * a.z - z * a.y, z * a.x - x * a.z, x * a.y - y * a.x );
 303 }
 304
 305 ID_INLINE idVec3 &idVec3::Cross( const idVec3 &a, const idVec3 &b ) {
 306         x = a.y * b.z - a.z * b.y;
 307         y = a.z * b.x - a.x * b.z;
 308         z = a.x * b.y - a.y * b.x;
 309
 310         return *this;
 311 }
 312
 313 ID_INLINE float idVec3::Length( void ) const {
 314         float length;
 315
 316         length = x * x + y * y + z * z;
 317         return ( float )idSqrt( length );
 318 }
 319
 320 ID_INLINE float idVec3::Normalize( void ) {
 321         float length;
 322         float ilength;
 323
 324         length = this->Length();
 325         if ( length ) {
 326                 ilength = 1.0f / length;
 327                 x *= ilength;
 328                 y *= ilength;
 329                 z *= ilength;
 330         }
 331
 332         return length;
 333 }
 334
 335 ID_INLINE void idVec3::Zero( void ) {
 336         x = 0.0f;
 337         y = 0.0f;
 338         z = 0.0f;
 339 }
 340
 341 ID_INLINE void idVec3::Snap( void ) {
 342         x = float( int( x ) );
 343         y = float( int( y ) );
 344         z = float( int( z ) );
 345 }
 346
 347 /*
 348    ======================
 349    SnapTowards
 350
 351    Round a vector to integers for more efficient network
 352    transmission, but make sure that it rounds towards a given point
 353    rather than blindly truncating.  This prevents it from truncating
 354    into a wall.
 355    ======================
 356  */
 357 ID_INLINE void idVec3::SnapTowards( const idVec3 &to ) {
 358         if ( to.x <= x ) {
 359                 x = float( int( x ) );
 360         }
 361         else {
 362                 x = float( int( x ) + 1 );
 363         }
 364
 365         if ( to.y <= y ) {
 366                 y = float( int( y ) );
 367         }
 368         else {
 369                 y = float( int( y ) + 1 );
 370         }
 371
 372         if ( to.z <= z ) {
 373                 z = float( int( z ) );
 374         }
 375         else {
 376                 z = float( int( z ) + 1 );
 377         }
 378 }
 379
 380 //===============================================================
 381
 382 class Bounds {
 383 public:
 384 idVec3 b[2];
 385
 386 Bounds();
 387 Bounds( const idVec3 &mins, const idVec3 &maxs );
 388
 389 void    Clear();
 390 void    Zero();
 391 float   Radius();           // radius from origin, not from center
 392 idVec3  Center();
 393 void    AddPoint( const idVec3 &v );
 394 void    AddBounds( const Bounds &bb );
 395 bool    IsCleared();
 396 bool    ContainsPoint( const idVec3 &p );
 397 bool    IntersectsBounds( const Bounds &b2 );       // touching is NOT intersecting
 398 };
 399
 400 extern Bounds boundsZero;
 401
 402 ID_INLINE Bounds::Bounds(){
 403 }
 404
 405 ID_INLINE bool Bounds::IsCleared() {
 406         return b[0][0] > b[1][0];
 407 }
 408
 409 ID_INLINE bool Bounds::ContainsPoint( const idVec3 &p ) {
 410         if ( p[0] < b[0][0] || p[1] < b[0][1] || p[2] < b[0][2]
 411                  || p[0] > b[1][0] || p[1] > b[1][1] || p[2] > b[1][2] ) {
 412                 return false;
 413         }
 414         return true;
 415 }
 416
 417 ID_INLINE bool Bounds::IntersectsBounds( const Bounds &b2 ) {
 418         if ( b2.b[1][0] < b[0][0] || b2.b[1][1] < b[0][1] || b2.b[1][2] < b[0][2]
 419                  || b2.b[0][0] > b[1][0] || b2.b[0][1] > b[1][1] || b2.b[0][2] > b[1][2] ) {
 420                 return false;
 421         }
 422         return true;
 423 }
 424
 425 ID_INLINE Bounds::Bounds( const idVec3 &mins, const idVec3 &maxs ) {
 426         b[0] = mins;
 427         b[1] = maxs;
 428 }
 429
 430 ID_INLINE idVec3 Bounds::Center() {
 431         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 );
 432 }
 433
 434 ID_INLINE void Bounds::Clear() {
 435         b[0][0] = b[0][1] = b[0][2] = 99999;
 436         b[1][0] = b[1][1] = b[1][2] = -99999;
 437 }
 438
 439 ID_INLINE void Bounds::Zero() {
 440         b[0][0] = b[0][1] = b[0][2] =
 441                                                         b[1][0] = b[1][1] = b[1][2] = 0;
 442 }
 443
 444 ID_INLINE void Bounds::AddPoint( const idVec3 &v ) {
 445         if ( v[0] < b[0][0] ) {
 446                 b[0][0] = v[0];
 447         }
 448         if ( v[0] > b[1][0] ) {
 449                 b[1][0] = v[0];
 450         }
 451         if ( v[1] < b[0][1] ) {
 452                 b[0][1] = v[1];
 453         }
 454         if ( v[1] > b[1][1] ) {
 455                 b[1][1] = v[1];
 456         }
 457         if ( v[2] < b[0][2] ) {
 458                 b[0][2] = v[2];
 459         }
 460         if ( v[2] > b[1][2] ) {
 461                 b[1][2] = v[2];
 462         }
 463 }
 464
 465
 466 ID_INLINE void Bounds::AddBounds( const Bounds &bb ) {
 467         if ( bb.b[0][0] < b[0][0] ) {
 468                 b[0][0] = bb.b[0][0];
 469         }
 470         if ( bb.b[0][1] < b[0][1] ) {
 471                 b[0][1] = bb.b[0][1];
 472         }
 473         if ( bb.b[0][2] < b[0][2] ) {
 474                 b[0][2] = bb.b[0][2];
 475         }
 476
 477         if ( bb.b[1][0] > b[1][0] ) {
 478                 b[1][0] = bb.b[1][0];
 479         }
 480         if ( bb.b[1][1] > b[1][1] ) {
 481                 b[1][1] = bb.b[1][1];
 482         }
 483         if ( bb.b[1][2] > b[1][2] ) {
 484                 b[1][2] = bb.b[1][2];
 485         }
 486 }
 487
 488 ID_INLINE float Bounds::Radius() {
 489         int i;
 490         float total;
 491         float a, aa;
 492
 493         total = 0;
 494         for ( i = 0 ; i < 3 ; i++ ) {
 495                 a = (float)fabs( b[0][i] );
 496                 aa = (float)fabs( b[1][i] );
 497                 if ( aa > a ) {
 498                         a = aa;
 499                 }
 500                 total += a * a;
 501         }
 502
 503         return (float)idSqrt( total );
 504 }
 505
 506 //===============================================================
 507
 508
 509 class idVec2 {
 510 public:
 511 float x;
 512 float y;
 513
 514 operator float *();
 515 float operator[]( int index ) const;
 516 float           &operator[]( int index );
 517 };
 518
 519 ID_INLINE float idVec2::operator[]( int index ) const {
 520         return ( &x )[ index ];
 521 }
 522
 523 ID_INLINE float& idVec2::operator[]( int index ) {
 524         return ( &x )[ index ];
 525 }
 526
 527 ID_INLINE idVec2::operator float *( void ) {
 528         return &x;
 529 }
 530
 531 class idVec4 : public idVec3 {
 532 public:
 533 #ifndef FAT_VEC3
 534 float dist;
 535 #endif
 536 idVec4();
 537 ~idVec4() {};
 538
 539 idVec4( float x, float y, float z, float dist );
 540 float operator[]( int index ) const;
 541 float           &operator[]( int index );
 542 };
 543
 544 ID_INLINE idVec4::idVec4() {}
 545 ID_INLINE idVec4::idVec4( float x, float y, float z, float dist ) {
 546         this->x = x;
 547         this->y = y;
 548         this->z = z;
 549         this->dist = dist;
 550 }
 551
 552 ID_INLINE float idVec4::operator[]( int index ) const {
 553         return ( &x )[ index ];
 554 }
 555
 556 ID_INLINE float& idVec4::operator[]( int index ) {
 557         return ( &x )[ index ];
 558 }
 559
 560
 561 class idVec5_t : public idVec3 {
 562 public:
 563 float s;
 564 float t;
 565 float operator[]( int index ) const;
 566 float           &operator[]( int index );
 567 };
 568
 569
 570 ID_INLINE float idVec5_t::operator[]( int index ) const {
 571         return ( &x )[ index ];
 572 }
 573
 574 ID_INLINE float& idVec5_t::operator[]( int index ) {
 575         return ( &x )[ index ];
 576 }
 577
 578 #endif /* !__MATH_VECTOR_H__ */