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