libs/splines/math_vector.h

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