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