math_vector.h (13524B)
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 #if defined(_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_t : public idHeap<idVec3_t> { 125 class idVec3_t { 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_t() {}; 135 #else 136 idVec3_t() {dist = 0.0f;}; 137 #endif 138 idVec3_t( 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_t operator-() const; 148 149 idVec3_t &operator=( const idVec3_t &a ); 150 151 float operator*( const idVec3_t &a ) const; 152 idVec3_t operator*( const float a ) const; 153 friend idVec3_t operator*( float a, idVec3_t b ); 154 155 idVec3_t operator+( const idVec3_t &a ) const; 156 idVec3_t operator-( const idVec3_t &a ) const; 157 158 idVec3_t &operator+=( const idVec3_t &a ); 159 idVec3_t &operator-=( const idVec3_t &a ); 160 idVec3_t &operator*=( const float a ); 161 162 int operator==( const idVec3_t &a ) const; 163 int operator!=( const idVec3_t &a ) const; 164 165 idVec3_t Cross( const idVec3_t &a ) const; 166 idVec3_t &Cross( const idVec3_t &a, const idVec3_t &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_t &to ); 174 175 float toYaw( void ); 176 float toPitch( void ); 177 angles_t toAngles( void ); 178 friend idVec3_t LerpVector( const idVec3_t &w1, const idVec3_t &w2, const float t ); 179 180 char *string( void ); 181 }; 182 183 extern idVec3_t vec_zero; 184 185 ID_INLINE idVec3_t::idVec3_t( 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_t::operator[]( const int index ) const { 195 return ( &x )[ index ]; 196 } 197 198 ID_INLINE float &idVec3_t::operator[]( const int index ) { 199 return ( &x )[ index ]; 200 } 201 202 ID_INLINE idVec3_t::operator float *( void ) { 203 return &x; 204 } 205 206 ID_INLINE idVec3_t idVec3_t::operator-() const { 207 return idVec3_t( -x, -y, -z ); 208 } 209 210 ID_INLINE idVec3_t &idVec3_t::operator=( const idVec3_t &a ) { 211 x = a.x; 212 y = a.y; 213 z = a.z; 214 215 return *this; 216 } 217 218 ID_INLINE void idVec3_t::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_t idVec3_t::operator-( const idVec3_t &a ) const { 225 return idVec3_t( x - a.x, y - a.y, z - a.z ); 226 } 227 228 ID_INLINE float idVec3_t::operator*( const idVec3_t &a ) const { 229 return x * a.x + y * a.y + z * a.z; 230 } 231 232 ID_INLINE idVec3_t idVec3_t::operator*( const float a ) const { 233 return idVec3_t( x * a, y * a, z * a ); 234 } 235 236 ID_INLINE idVec3_t operator*( const float a, const idVec3_t b ) { 237 return idVec3_t( b.x * a, b.y * a, b.z * a ); 238 } 239 240 ID_INLINE idVec3_t idVec3_t::operator+( const idVec3_t &a ) const { 241 return idVec3_t( x + a.x, y + a.y, z + a.z ); 242 } 243 244 ID_INLINE idVec3_t &idVec3_t::operator+=( const idVec3_t &a ) { 245 x += a.x; 246 y += a.y; 247 z += a.z; 248 249 return *this; 250 } 251 252 ID_INLINE idVec3_t &idVec3_t::operator-=( const idVec3_t &a ) { 253 x -= a.x; 254 y -= a.y; 255 z -= a.z; 256 257 return *this; 258 } 259 260 ID_INLINE idVec3_t &idVec3_t::operator*=( const float a ) { 261 x *= a; 262 y *= a; 263 z *= a; 264 265 return *this; 266 } 267 268 ID_INLINE int idVec3_t::operator==( const idVec3_t &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_t::operator!=( const idVec3_t &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_t idVec3_t::Cross( const idVec3_t &a ) const { 301 return idVec3_t( y * a.z - z * a.y, z * a.x - x * a.z, x * a.y - y * a.x ); 302 } 303 304 ID_INLINE idVec3_t &idVec3_t::Cross( const idVec3_t &a, const idVec3_t &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_t::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_t::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_t::Zero( void ) { 335 x = 0.0f; 336 y = 0.0f; 337 z = 0.0f; 338 } 339 340 ID_INLINE void idVec3_t::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_t::SnapTowards( const idVec3_t &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_t b[2]; 381 382 Bounds(); 383 Bounds( const idVec3_t &mins, const idVec3_t &maxs ); 384 385 void Clear(); 386 void Zero(); 387 float Radius(); // radius from origin, not from center 388 idVec3_t Center(); 389 void AddPoint( const idVec3_t &v ); 390 void AddBounds( const Bounds &bb ); 391 bool IsCleared(); 392 bool ContainsPoint( const idVec3_t &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_t &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_t &mins, const idVec3_t &maxs ) { 422 b[0] = mins; 423 b[1] = maxs; 424 } 425 426 ID_INLINE idVec3_t Bounds::Center() { 427 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 ); 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_t &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_t { 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_t::operator[]( int index ) const { 516 return ( &x )[ index ]; 517 } 518 519 ID_INLINE float& idVec2_t::operator[]( int index ) { 520 return ( &x )[ index ]; 521 } 522 523 ID_INLINE idVec2_t::operator float *( void ) { 524 return &x; 525 } 526 527 class vec4_t : public idVec3_t { 528 public: 529 #ifndef FAT_VEC3 530 float dist; 531 #endif 532 vec4_t(); 533 ~vec4_t() {}; 534 535 vec4_t( float x, float y, float z, float dist ); 536 float operator[]( int index ) const; 537 float &operator[]( int index ); 538 }; 539 540 ID_INLINE vec4_t::vec4_t() {} 541 ID_INLINE vec4_t::vec4_t( 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 vec4_t::operator[]( int index ) const { 549 return ( &x )[ index ]; 550 } 551 552 ID_INLINE float& vec4_t::operator[]( int index ) { 553 return ( &x )[ index ]; 554 } 555 556 557 class idVec5_t : public idVec3_t { 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__ */