00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #ifndef _VECTORMATH_VEC_AOS_CPP_H
00018 #define _VECTORMATH_VEC_AOS_CPP_H
00019
00020
00021
00022
00023 #define _VECTORMATH_SLERP_TOL 0.999f
00024
00025
00026
00027
00028 #ifndef _VECTORMATH_INTERNAL_FUNCTIONS
00029 #define _VECTORMATH_INTERNAL_FUNCTIONS
00030
00031 #endif
00032
00033 namespace Vectormath {
00034 namespace Aos {
00035
00036 inline Vector3::Vector3( const Vector3 & vec )
00037 {
00038 mX = vec.mX;
00039 mY = vec.mY;
00040 mZ = vec.mZ;
00041 }
00042
00043 inline Vector3::Vector3( float _x, float _y, float _z )
00044 {
00045 mX = _x;
00046 mY = _y;
00047 mZ = _z;
00048 }
00049
00050 inline Vector3::Vector3( const Point3 & pnt )
00051 {
00052 mX = pnt.getX();
00053 mY = pnt.getY();
00054 mZ = pnt.getZ();
00055 }
00056
00057 inline Vector3::Vector3( float scalar )
00058 {
00059 mX = scalar;
00060 mY = scalar;
00061 mZ = scalar;
00062 }
00063
00064 inline const Vector3 Vector3::xAxis( )
00065 {
00066 return Vector3( 1.0f, 0.0f, 0.0f );
00067 }
00068
00069 inline const Vector3 Vector3::yAxis( )
00070 {
00071 return Vector3( 0.0f, 1.0f, 0.0f );
00072 }
00073
00074 inline const Vector3 Vector3::zAxis( )
00075 {
00076 return Vector3( 0.0f, 0.0f, 1.0f );
00077 }
00078
00079 inline const Vector3 lerp( float t, const Vector3 & vec0, const Vector3 & vec1 )
00080 {
00081 return ( vec0 + ( ( vec1 - vec0 ) * t ) );
00082 }
00083
00084 inline const Vector3 slerp( float t, const Vector3 & unitVec0, const Vector3 & unitVec1 )
00085 {
00086 float recipSinAngle, scale0, scale1, cosAngle, angle;
00087 cosAngle = dot( unitVec0, unitVec1 );
00088 if ( cosAngle < _VECTORMATH_SLERP_TOL ) {
00089 angle = acosf( cosAngle );
00090 recipSinAngle = ( 1.0f / sinf( angle ) );
00091 scale0 = ( sinf( ( ( 1.0f - t ) * angle ) ) * recipSinAngle );
00092 scale1 = ( sinf( ( t * angle ) ) * recipSinAngle );
00093 } else {
00094 scale0 = ( 1.0f - t );
00095 scale1 = t;
00096 }
00097 return ( ( unitVec0 * scale0 ) + ( unitVec1 * scale1 ) );
00098 }
00099
00100 inline void loadXYZ( Vector3 & vec, const float * fptr )
00101 {
00102 vec = Vector3( fptr[0], fptr[1], fptr[2] );
00103 }
00104
00105 inline void storeXYZ( const Vector3 & vec, float * fptr )
00106 {
00107 fptr[0] = vec.getX();
00108 fptr[1] = vec.getY();
00109 fptr[2] = vec.getZ();
00110 }
00111
00112 inline void loadHalfFloats( Vector3 & vec, const unsigned short * hfptr )
00113 {
00114 union Data32 {
00115 unsigned int u32;
00116 float f32;
00117 };
00118
00119 for (int i = 0; i < 3; i++) {
00120 unsigned short fp16 = hfptr[i];
00121 unsigned int sign = fp16 >> 15;
00122 unsigned int exponent = (fp16 >> 10) & ((1 << 5) - 1);
00123 unsigned int mantissa = fp16 & ((1 << 10) - 1);
00124
00125 if (exponent == 0) {
00126
00127 mantissa = 0;
00128
00129 } else if (exponent == 31) {
00130
00131 exponent = 255;
00132 mantissa = 0;
00133
00134 } else {
00135 exponent += 127 - 15;
00136 mantissa <<= 13;
00137 }
00138
00139 Data32 d;
00140 d.u32 = (sign << 31) | (exponent << 23) | mantissa;
00141 vec[i] = d.f32;
00142 }
00143 }
00144
00145 inline void storeHalfFloats( const Vector3 & vec, unsigned short * hfptr )
00146 {
00147 union Data32 {
00148 unsigned int u32;
00149 float f32;
00150 };
00151
00152 for (int i = 0; i < 3; i++) {
00153 Data32 d;
00154 d.f32 = vec[i];
00155
00156 unsigned int sign = d.u32 >> 31;
00157 unsigned int exponent = (d.u32 >> 23) & ((1 << 8) - 1);
00158 unsigned int mantissa = d.u32 & ((1 << 23) - 1);;
00159
00160 if (exponent == 0) {
00161
00162 mantissa = 0;
00163
00164 } else if (exponent == 255 && mantissa != 0) {
00165
00166 exponent = 31;
00167 mantissa = 0;
00168
00169 } else if (exponent >= 127 - 15 + 31) {
00170
00171 exponent = 31;
00172 mantissa = 0;
00173
00174 } else if (exponent <= 127 - 15) {
00175
00176 exponent = 0;
00177 mantissa = 0;
00178
00179 } else {
00180 exponent -= 127 - 15;
00181 mantissa >>= 13;
00182 }
00183
00184 hfptr[i] = (unsigned short)((sign << 15) | (exponent << 10) | mantissa);
00185 }
00186 }
00187
00188 inline Vector3 & Vector3::operator =( const Vector3 & vec )
00189 {
00190 mX = vec.mX;
00191 mY = vec.mY;
00192 mZ = vec.mZ;
00193 return *this;
00194 }
00195
00196 inline Vector3 & Vector3::setX( float _x )
00197 {
00198 mX = _x;
00199 return *this;
00200 }
00201
00202 inline float Vector3::getX( ) const
00203 {
00204 return mX;
00205 }
00206
00207 inline Vector3 & Vector3::setY( float _y )
00208 {
00209 mY = _y;
00210 return *this;
00211 }
00212
00213 inline float Vector3::getY( ) const
00214 {
00215 return mY;
00216 }
00217
00218 inline Vector3 & Vector3::setZ( float _z )
00219 {
00220 mZ = _z;
00221 return *this;
00222 }
00223
00224 inline float Vector3::getZ( ) const
00225 {
00226 return mZ;
00227 }
00228
00229 inline Vector3 & Vector3::setElem( int idx, float value )
00230 {
00231 *(&mX + idx) = value;
00232 return *this;
00233 }
00234
00235 inline float Vector3::getElem( int idx ) const
00236 {
00237 return *(&mX + idx);
00238 }
00239
00240 inline float & Vector3::operator []( int idx )
00241 {
00242 return *(&mX + idx);
00243 }
00244
00245 inline float Vector3::operator []( int idx ) const
00246 {
00247 return *(&mX + idx);
00248 }
00249
00250 inline const Vector3 Vector3::operator +( const Vector3 & vec ) const
00251 {
00252 return Vector3(
00253 ( mX + vec.mX ),
00254 ( mY + vec.mY ),
00255 ( mZ + vec.mZ )
00256 );
00257 }
00258
00259 inline const Vector3 Vector3::operator -( const Vector3 & vec ) const
00260 {
00261 return Vector3(
00262 ( mX - vec.mX ),
00263 ( mY - vec.mY ),
00264 ( mZ - vec.mZ )
00265 );
00266 }
00267
00268 inline const Point3 Vector3::operator +( const Point3 & pnt ) const
00269 {
00270 return Point3(
00271 ( mX + pnt.getX() ),
00272 ( mY + pnt.getY() ),
00273 ( mZ + pnt.getZ() )
00274 );
00275 }
00276
00277 inline const Vector3 Vector3::operator *( float scalar ) const
00278 {
00279 return Vector3(
00280 ( mX * scalar ),
00281 ( mY * scalar ),
00282 ( mZ * scalar )
00283 );
00284 }
00285
00286 inline Vector3 & Vector3::operator +=( const Vector3 & vec )
00287 {
00288 *this = *this + vec;
00289 return *this;
00290 }
00291
00292 inline Vector3 & Vector3::operator -=( const Vector3 & vec )
00293 {
00294 *this = *this - vec;
00295 return *this;
00296 }
00297
00298 inline Vector3 & Vector3::operator *=( float scalar )
00299 {
00300 *this = *this * scalar;
00301 return *this;
00302 }
00303
00304 inline const Vector3 Vector3::operator /( float scalar ) const
00305 {
00306 return Vector3(
00307 ( mX / scalar ),
00308 ( mY / scalar ),
00309 ( mZ / scalar )
00310 );
00311 }
00312
00313 inline Vector3 & Vector3::operator /=( float scalar )
00314 {
00315 *this = *this / scalar;
00316 return *this;
00317 }
00318
00319 inline const Vector3 Vector3::operator -( ) const
00320 {
00321 return Vector3(
00322 -mX,
00323 -mY,
00324 -mZ
00325 );
00326 }
00327
00328 inline const Vector3 operator *( float scalar, const Vector3 & vec )
00329 {
00330 return vec * scalar;
00331 }
00332
00333 inline const Vector3 mulPerElem( const Vector3 & vec0, const Vector3 & vec1 )
00334 {
00335 return Vector3(
00336 ( vec0.getX() * vec1.getX() ),
00337 ( vec0.getY() * vec1.getY() ),
00338 ( vec0.getZ() * vec1.getZ() )
00339 );
00340 }
00341
00342 inline const Vector3 divPerElem( const Vector3 & vec0, const Vector3 & vec1 )
00343 {
00344 return Vector3(
00345 ( vec0.getX() / vec1.getX() ),
00346 ( vec0.getY() / vec1.getY() ),
00347 ( vec0.getZ() / vec1.getZ() )
00348 );
00349 }
00350
00351 inline const Vector3 recipPerElem( const Vector3 & vec )
00352 {
00353 return Vector3(
00354 ( 1.0f / vec.getX() ),
00355 ( 1.0f / vec.getY() ),
00356 ( 1.0f / vec.getZ() )
00357 );
00358 }
00359
00360 inline const Vector3 sqrtPerElem( const Vector3 & vec )
00361 {
00362 return Vector3(
00363 sqrtf( vec.getX() ),
00364 sqrtf( vec.getY() ),
00365 sqrtf( vec.getZ() )
00366 );
00367 }
00368
00369 inline const Vector3 rsqrtPerElem( const Vector3 & vec )
00370 {
00371 return Vector3(
00372 ( 1.0f / sqrtf( vec.getX() ) ),
00373 ( 1.0f / sqrtf( vec.getY() ) ),
00374 ( 1.0f / sqrtf( vec.getZ() ) )
00375 );
00376 }
00377
00378 inline const Vector3 absPerElem( const Vector3 & vec )
00379 {
00380 return Vector3(
00381 fabsf( vec.getX() ),
00382 fabsf( vec.getY() ),
00383 fabsf( vec.getZ() )
00384 );
00385 }
00386
00387 inline const Vector3 copySignPerElem( const Vector3 & vec0, const Vector3 & vec1 )
00388 {
00389 return Vector3(
00390 ( vec1.getX() < 0.0f )? -fabsf( vec0.getX() ) : fabsf( vec0.getX() ),
00391 ( vec1.getY() < 0.0f )? -fabsf( vec0.getY() ) : fabsf( vec0.getY() ),
00392 ( vec1.getZ() < 0.0f )? -fabsf( vec0.getZ() ) : fabsf( vec0.getZ() )
00393 );
00394 }
00395
00396 inline const Vector3 maxPerElem( const Vector3 & vec0, const Vector3 & vec1 )
00397 {
00398 return Vector3(
00399 (vec0.getX() > vec1.getX())? vec0.getX() : vec1.getX(),
00400 (vec0.getY() > vec1.getY())? vec0.getY() : vec1.getY(),
00401 (vec0.getZ() > vec1.getZ())? vec0.getZ() : vec1.getZ()
00402 );
00403 }
00404
00405 inline float maxElem( const Vector3 & vec )
00406 {
00407 float result;
00408 result = (vec.getX() > vec.getY())? vec.getX() : vec.getY();
00409 result = (vec.getZ() > result)? vec.getZ() : result;
00410 return result;
00411 }
00412
00413 inline const Vector3 minPerElem( const Vector3 & vec0, const Vector3 & vec1 )
00414 {
00415 return Vector3(
00416 (vec0.getX() < vec1.getX())? vec0.getX() : vec1.getX(),
00417 (vec0.getY() < vec1.getY())? vec0.getY() : vec1.getY(),
00418 (vec0.getZ() < vec1.getZ())? vec0.getZ() : vec1.getZ()
00419 );
00420 }
00421
00422 inline float minElem( const Vector3 & vec )
00423 {
00424 float result;
00425 result = (vec.getX() < vec.getY())? vec.getX() : vec.getY();
00426 result = (vec.getZ() < result)? vec.getZ() : result;
00427 return result;
00428 }
00429
00430 inline float sum( const Vector3 & vec )
00431 {
00432 float result;
00433 result = ( vec.getX() + vec.getY() );
00434 result = ( result + vec.getZ() );
00435 return result;
00436 }
00437
00438 inline float dot( const Vector3 & vec0, const Vector3 & vec1 )
00439 {
00440 float result;
00441 result = ( vec0.getX() * vec1.getX() );
00442 result = ( result + ( vec0.getY() * vec1.getY() ) );
00443 result = ( result + ( vec0.getZ() * vec1.getZ() ) );
00444 return result;
00445 }
00446
00447 inline float lengthSqr( const Vector3 & vec )
00448 {
00449 float result;
00450 result = ( vec.getX() * vec.getX() );
00451 result = ( result + ( vec.getY() * vec.getY() ) );
00452 result = ( result + ( vec.getZ() * vec.getZ() ) );
00453 return result;
00454 }
00455
00456 inline float length( const Vector3 & vec )
00457 {
00458 return ::sqrtf( lengthSqr( vec ) );
00459 }
00460
00461 inline const Vector3 normalize( const Vector3 & vec )
00462 {
00463 float lenSqr, lenInv;
00464 lenSqr = lengthSqr( vec );
00465 lenInv = ( 1.0f / sqrtf( lenSqr ) );
00466 return Vector3(
00467 ( vec.getX() * lenInv ),
00468 ( vec.getY() * lenInv ),
00469 ( vec.getZ() * lenInv )
00470 );
00471 }
00472
00473 inline const Vector3 cross( const Vector3 & vec0, const Vector3 & vec1 )
00474 {
00475 return Vector3(
00476 ( ( vec0.getY() * vec1.getZ() ) - ( vec0.getZ() * vec1.getY() ) ),
00477 ( ( vec0.getZ() * vec1.getX() ) - ( vec0.getX() * vec1.getZ() ) ),
00478 ( ( vec0.getX() * vec1.getY() ) - ( vec0.getY() * vec1.getX() ) )
00479 );
00480 }
00481
00482 inline const Vector3 select( const Vector3 & vec0, const Vector3 & vec1, bool select1 )
00483 {
00484 return Vector3(
00485 ( select1 )? vec1.getX() : vec0.getX(),
00486 ( select1 )? vec1.getY() : vec0.getY(),
00487 ( select1 )? vec1.getZ() : vec0.getZ()
00488 );
00489 }
00490
00491 #ifdef _VECTORMATH_DEBUG
00492
00493 inline void print( const Vector3 & vec )
00494 {
00495 printf( "( %f %f %f )\n", vec.getX(), vec.getY(), vec.getZ() );
00496 }
00497
00498 inline void print( const Vector3 & vec, const char * name )
00499 {
00500 printf( "%s: ( %f %f %f )\n", name, vec.getX(), vec.getY(), vec.getZ() );
00501 }
00502
00503 #endif
00504
00505 inline Vector4::Vector4( const Vector4 & vec )
00506 {
00507 mX = vec.mX;
00508 mY = vec.mY;
00509 mZ = vec.mZ;
00510 mW = vec.mW;
00511 }
00512
00513 inline Vector4::Vector4( float _x, float _y, float _z, float _w )
00514 {
00515 mX = _x;
00516 mY = _y;
00517 mZ = _z;
00518 mW = _w;
00519 }
00520
00521 inline Vector4::Vector4( const Vector3 & xyz, float _w )
00522 {
00523 this->setXYZ( xyz );
00524 this->setW( _w );
00525 }
00526
00527 inline Vector4::Vector4( const Vector3 & vec )
00528 {
00529 mX = vec.getX();
00530 mY = vec.getY();
00531 mZ = vec.getZ();
00532 mW = 0.0f;
00533 }
00534
00535 inline Vector4::Vector4( const Point3 & pnt )
00536 {
00537 mX = pnt.getX();
00538 mY = pnt.getY();
00539 mZ = pnt.getZ();
00540 mW = 1.0f;
00541 }
00542
00543 inline Vector4::Vector4( const Quat & quat )
00544 {
00545 mX = quat.getX();
00546 mY = quat.getY();
00547 mZ = quat.getZ();
00548 mW = quat.getW();
00549 }
00550
00551 inline Vector4::Vector4( float scalar )
00552 {
00553 mX = scalar;
00554 mY = scalar;
00555 mZ = scalar;
00556 mW = scalar;
00557 }
00558
00559 inline const Vector4 Vector4::xAxis( )
00560 {
00561 return Vector4( 1.0f, 0.0f, 0.0f, 0.0f );
00562 }
00563
00564 inline const Vector4 Vector4::yAxis( )
00565 {
00566 return Vector4( 0.0f, 1.0f, 0.0f, 0.0f );
00567 }
00568
00569 inline const Vector4 Vector4::zAxis( )
00570 {
00571 return Vector4( 0.0f, 0.0f, 1.0f, 0.0f );
00572 }
00573
00574 inline const Vector4 Vector4::wAxis( )
00575 {
00576 return Vector4( 0.0f, 0.0f, 0.0f, 1.0f );
00577 }
00578
00579 inline const Vector4 lerp( float t, const Vector4 & vec0, const Vector4 & vec1 )
00580 {
00581 return ( vec0 + ( ( vec1 - vec0 ) * t ) );
00582 }
00583
00584 inline const Vector4 slerp( float t, const Vector4 & unitVec0, const Vector4 & unitVec1 )
00585 {
00586 float recipSinAngle, scale0, scale1, cosAngle, angle;
00587 cosAngle = dot( unitVec0, unitVec1 );
00588 if ( cosAngle < _VECTORMATH_SLERP_TOL ) {
00589 angle = acosf( cosAngle );
00590 recipSinAngle = ( 1.0f / sinf( angle ) );
00591 scale0 = ( sinf( ( ( 1.0f - t ) * angle ) ) * recipSinAngle );
00592 scale1 = ( sinf( ( t * angle ) ) * recipSinAngle );
00593 } else {
00594 scale0 = ( 1.0f - t );
00595 scale1 = t;
00596 }
00597 return ( ( unitVec0 * scale0 ) + ( unitVec1 * scale1 ) );
00598 }
00599
00600 inline void loadXYZW( Vector4 & vec, const float * fptr )
00601 {
00602 vec = Vector4( fptr[0], fptr[1], fptr[2], fptr[3] );
00603 }
00604
00605 inline void storeXYZW( const Vector4 & vec, float * fptr )
00606 {
00607 fptr[0] = vec.getX();
00608 fptr[1] = vec.getY();
00609 fptr[2] = vec.getZ();
00610 fptr[3] = vec.getW();
00611 }
00612
00613 inline void loadHalfFloats( Vector4 & vec, const unsigned short * hfptr )
00614 {
00615 union Data32 {
00616 unsigned int u32;
00617 float f32;
00618 };
00619
00620 for (int i = 0; i < 4; i++) {
00621 unsigned short fp16 = hfptr[i];
00622 unsigned int sign = fp16 >> 15;
00623 unsigned int exponent = (fp16 >> 10) & ((1 << 5) - 1);
00624 unsigned int mantissa = fp16 & ((1 << 10) - 1);
00625
00626 if (exponent == 0) {
00627
00628 mantissa = 0;
00629
00630 } else if (exponent == 31) {
00631
00632 exponent = 255;
00633 mantissa = 0;
00634
00635 } else {
00636 exponent += 127 - 15;
00637 mantissa <<= 13;
00638 }
00639
00640 Data32 d;
00641 d.u32 = (sign << 31) | (exponent << 23) | mantissa;
00642 vec[i] = d.f32;
00643 }
00644 }
00645
00646 inline void storeHalfFloats( const Vector4 & vec, unsigned short * hfptr )
00647 {
00648 union Data32 {
00649 unsigned int u32;
00650 float f32;
00651 };
00652
00653 for (int i = 0; i < 4; i++) {
00654 Data32 d;
00655 d.f32 = vec[i];
00656
00657 unsigned int sign = d.u32 >> 31;
00658 unsigned int exponent = (d.u32 >> 23) & ((1 << 8) - 1);
00659 unsigned int mantissa = d.u32 & ((1 << 23) - 1);;
00660
00661 if (exponent == 0) {
00662
00663 mantissa = 0;
00664
00665 } else if (exponent == 255 && mantissa != 0) {
00666
00667 exponent = 31;
00668 mantissa = 0;
00669
00670 } else if (exponent >= 127 - 15 + 31) {
00671
00672 exponent = 31;
00673 mantissa = 0;
00674
00675 } else if (exponent <= 127 - 15) {
00676
00677 exponent = 0;
00678 mantissa = 0;
00679
00680 } else {
00681 exponent -= 127 - 15;
00682 mantissa >>= 13;
00683 }
00684
00685 hfptr[i] = (unsigned short)((sign << 15) | (exponent << 10) | mantissa);
00686 }
00687 }
00688
00689 inline Vector4 & Vector4::operator =( const Vector4 & vec )
00690 {
00691 mX = vec.mX;
00692 mY = vec.mY;
00693 mZ = vec.mZ;
00694 mW = vec.mW;
00695 return *this;
00696 }
00697
00698 inline Vector4 & Vector4::setXYZ( const Vector3 & vec )
00699 {
00700 mX = vec.getX();
00701 mY = vec.getY();
00702 mZ = vec.getZ();
00703 return *this;
00704 }
00705
00706 inline const Vector3 Vector4::getXYZ( ) const
00707 {
00708 return Vector3( mX, mY, mZ );
00709 }
00710
00711 inline Vector4 & Vector4::setX( float _x )
00712 {
00713 mX = _x;
00714 return *this;
00715 }
00716
00717 inline float Vector4::getX( ) const
00718 {
00719 return mX;
00720 }
00721
00722 inline Vector4 & Vector4::setY( float _y )
00723 {
00724 mY = _y;
00725 return *this;
00726 }
00727
00728 inline float Vector4::getY( ) const
00729 {
00730 return mY;
00731 }
00732
00733 inline Vector4 & Vector4::setZ( float _z )
00734 {
00735 mZ = _z;
00736 return *this;
00737 }
00738
00739 inline float Vector4::getZ( ) const
00740 {
00741 return mZ;
00742 }
00743
00744 inline Vector4 & Vector4::setW( float _w )
00745 {
00746 mW = _w;
00747 return *this;
00748 }
00749
00750 inline float Vector4::getW( ) const
00751 {
00752 return mW;
00753 }
00754
00755 inline Vector4 & Vector4::setElem( int idx, float value )
00756 {
00757 *(&mX + idx) = value;
00758 return *this;
00759 }
00760
00761 inline float Vector4::getElem( int idx ) const
00762 {
00763 return *(&mX + idx);
00764 }
00765
00766 inline float & Vector4::operator []( int idx )
00767 {
00768 return *(&mX + idx);
00769 }
00770
00771 inline float Vector4::operator []( int idx ) const
00772 {
00773 return *(&mX + idx);
00774 }
00775
00776 inline const Vector4 Vector4::operator +( const Vector4 & vec ) const
00777 {
00778 return Vector4(
00779 ( mX + vec.mX ),
00780 ( mY + vec.mY ),
00781 ( mZ + vec.mZ ),
00782 ( mW + vec.mW )
00783 );
00784 }
00785
00786 inline const Vector4 Vector4::operator -( const Vector4 & vec ) const
00787 {
00788 return Vector4(
00789 ( mX - vec.mX ),
00790 ( mY - vec.mY ),
00791 ( mZ - vec.mZ ),
00792 ( mW - vec.mW )
00793 );
00794 }
00795
00796 inline const Vector4 Vector4::operator *( float scalar ) const
00797 {
00798 return Vector4(
00799 ( mX * scalar ),
00800 ( mY * scalar ),
00801 ( mZ * scalar ),
00802 ( mW * scalar )
00803 );
00804 }
00805
00806 inline Vector4 & Vector4::operator +=( const Vector4 & vec )
00807 {
00808 *this = *this + vec;
00809 return *this;
00810 }
00811
00812 inline Vector4 & Vector4::operator -=( const Vector4 & vec )
00813 {
00814 *this = *this - vec;
00815 return *this;
00816 }
00817
00818 inline Vector4 & Vector4::operator *=( float scalar )
00819 {
00820 *this = *this * scalar;
00821 return *this;
00822 }
00823
00824 inline const Vector4 Vector4::operator /( float scalar ) const
00825 {
00826 return Vector4(
00827 ( mX / scalar ),
00828 ( mY / scalar ),
00829 ( mZ / scalar ),
00830 ( mW / scalar )
00831 );
00832 }
00833
00834 inline Vector4 & Vector4::operator /=( float scalar )
00835 {
00836 *this = *this / scalar;
00837 return *this;
00838 }
00839
00840 inline const Vector4 Vector4::operator -( ) const
00841 {
00842 return Vector4(
00843 -mX,
00844 -mY,
00845 -mZ,
00846 -mW
00847 );
00848 }
00849
00850 inline const Vector4 operator *( float scalar, const Vector4 & vec )
00851 {
00852 return vec * scalar;
00853 }
00854
00855 inline const Vector4 mulPerElem( const Vector4 & vec0, const Vector4 & vec1 )
00856 {
00857 return Vector4(
00858 ( vec0.getX() * vec1.getX() ),
00859 ( vec0.getY() * vec1.getY() ),
00860 ( vec0.getZ() * vec1.getZ() ),
00861 ( vec0.getW() * vec1.getW() )
00862 );
00863 }
00864
00865 inline const Vector4 divPerElem( const Vector4 & vec0, const Vector4 & vec1 )
00866 {
00867 return Vector4(
00868 ( vec0.getX() / vec1.getX() ),
00869 ( vec0.getY() / vec1.getY() ),
00870 ( vec0.getZ() / vec1.getZ() ),
00871 ( vec0.getW() / vec1.getW() )
00872 );
00873 }
00874
00875 inline const Vector4 recipPerElem( const Vector4 & vec )
00876 {
00877 return Vector4(
00878 ( 1.0f / vec.getX() ),
00879 ( 1.0f / vec.getY() ),
00880 ( 1.0f / vec.getZ() ),
00881 ( 1.0f / vec.getW() )
00882 );
00883 }
00884
00885 inline const Vector4 sqrtPerElem( const Vector4 & vec )
00886 {
00887 return Vector4(
00888 sqrtf( vec.getX() ),
00889 sqrtf( vec.getY() ),
00890 sqrtf( vec.getZ() ),
00891 sqrtf( vec.getW() )
00892 );
00893 }
00894
00895 inline const Vector4 rsqrtPerElem( const Vector4 & vec )
00896 {
00897 return Vector4(
00898 ( 1.0f / sqrtf( vec.getX() ) ),
00899 ( 1.0f / sqrtf( vec.getY() ) ),
00900 ( 1.0f / sqrtf( vec.getZ() ) ),
00901 ( 1.0f / sqrtf( vec.getW() ) )
00902 );
00903 }
00904
00905 inline const Vector4 absPerElem( const Vector4 & vec )
00906 {
00907 return Vector4(
00908 fabsf( vec.getX() ),
00909 fabsf( vec.getY() ),
00910 fabsf( vec.getZ() ),
00911 fabsf( vec.getW() )
00912 );
00913 }
00914
00915 inline const Vector4 copySignPerElem( const Vector4 & vec0, const Vector4 & vec1 )
00916 {
00917 return Vector4(
00918 ( vec1.getX() < 0.0f )? -fabsf( vec0.getX() ) : fabsf( vec0.getX() ),
00919 ( vec1.getY() < 0.0f )? -fabsf( vec0.getY() ) : fabsf( vec0.getY() ),
00920 ( vec1.getZ() < 0.0f )? -fabsf( vec0.getZ() ) : fabsf( vec0.getZ() ),
00921 ( vec1.getW() < 0.0f )? -fabsf( vec0.getW() ) : fabsf( vec0.getW() )
00922 );
00923 }
00924
00925 inline const Vector4 maxPerElem( const Vector4 & vec0, const Vector4 & vec1 )
00926 {
00927 return Vector4(
00928 (vec0.getX() > vec1.getX())? vec0.getX() : vec1.getX(),
00929 (vec0.getY() > vec1.getY())? vec0.getY() : vec1.getY(),
00930 (vec0.getZ() > vec1.getZ())? vec0.getZ() : vec1.getZ(),
00931 (vec0.getW() > vec1.getW())? vec0.getW() : vec1.getW()
00932 );
00933 }
00934
00935 inline float maxElem( const Vector4 & vec )
00936 {
00937 float result;
00938 result = (vec.getX() > vec.getY())? vec.getX() : vec.getY();
00939 result = (vec.getZ() > result)? vec.getZ() : result;
00940 result = (vec.getW() > result)? vec.getW() : result;
00941 return result;
00942 }
00943
00944 inline const Vector4 minPerElem( const Vector4 & vec0, const Vector4 & vec1 )
00945 {
00946 return Vector4(
00947 (vec0.getX() < vec1.getX())? vec0.getX() : vec1.getX(),
00948 (vec0.getY() < vec1.getY())? vec0.getY() : vec1.getY(),
00949 (vec0.getZ() < vec1.getZ())? vec0.getZ() : vec1.getZ(),
00950 (vec0.getW() < vec1.getW())? vec0.getW() : vec1.getW()
00951 );
00952 }
00953
00954 inline float minElem( const Vector4 & vec )
00955 {
00956 float result;
00957 result = (vec.getX() < vec.getY())? vec.getX() : vec.getY();
00958 result = (vec.getZ() < result)? vec.getZ() : result;
00959 result = (vec.getW() < result)? vec.getW() : result;
00960 return result;
00961 }
00962
00963 inline float sum( const Vector4 & vec )
00964 {
00965 float result;
00966 result = ( vec.getX() + vec.getY() );
00967 result = ( result + vec.getZ() );
00968 result = ( result + vec.getW() );
00969 return result;
00970 }
00971
00972 inline float dot( const Vector4 & vec0, const Vector4 & vec1 )
00973 {
00974 float result;
00975 result = ( vec0.getX() * vec1.getX() );
00976 result = ( result + ( vec0.getY() * vec1.getY() ) );
00977 result = ( result + ( vec0.getZ() * vec1.getZ() ) );
00978 result = ( result + ( vec0.getW() * vec1.getW() ) );
00979 return result;
00980 }
00981
00982 inline float lengthSqr( const Vector4 & vec )
00983 {
00984 float result;
00985 result = ( vec.getX() * vec.getX() );
00986 result = ( result + ( vec.getY() * vec.getY() ) );
00987 result = ( result + ( vec.getZ() * vec.getZ() ) );
00988 result = ( result + ( vec.getW() * vec.getW() ) );
00989 return result;
00990 }
00991
00992 inline float length( const Vector4 & vec )
00993 {
00994 return ::sqrtf( lengthSqr( vec ) );
00995 }
00996
00997 inline const Vector4 normalize( const Vector4 & vec )
00998 {
00999 float lenSqr, lenInv;
01000 lenSqr = lengthSqr( vec );
01001 lenInv = ( 1.0f / sqrtf( lenSqr ) );
01002 return Vector4(
01003 ( vec.getX() * lenInv ),
01004 ( vec.getY() * lenInv ),
01005 ( vec.getZ() * lenInv ),
01006 ( vec.getW() * lenInv )
01007 );
01008 }
01009
01010 inline const Vector4 select( const Vector4 & vec0, const Vector4 & vec1, bool select1 )
01011 {
01012 return Vector4(
01013 ( select1 )? vec1.getX() : vec0.getX(),
01014 ( select1 )? vec1.getY() : vec0.getY(),
01015 ( select1 )? vec1.getZ() : vec0.getZ(),
01016 ( select1 )? vec1.getW() : vec0.getW()
01017 );
01018 }
01019
01020 #ifdef _VECTORMATH_DEBUG
01021
01022 inline void print( const Vector4 & vec )
01023 {
01024 printf( "( %f %f %f %f )\n", vec.getX(), vec.getY(), vec.getZ(), vec.getW() );
01025 }
01026
01027 inline void print( const Vector4 & vec, const char * name )
01028 {
01029 printf( "%s: ( %f %f %f %f )\n", name, vec.getX(), vec.getY(), vec.getZ(), vec.getW() );
01030 }
01031
01032 #endif
01033
01034 inline Point3::Point3( const Point3 & pnt )
01035 {
01036 mX = pnt.mX;
01037 mY = pnt.mY;
01038 mZ = pnt.mZ;
01039 }
01040
01041 inline Point3::Point3( float _x, float _y, float _z )
01042 {
01043 mX = _x;
01044 mY = _y;
01045 mZ = _z;
01046 }
01047
01048 inline Point3::Point3( const Vector3 & vec )
01049 {
01050 mX = vec.getX();
01051 mY = vec.getY();
01052 mZ = vec.getZ();
01053 }
01054
01055 inline Point3::Point3( float scalar )
01056 {
01057 mX = scalar;
01058 mY = scalar;
01059 mZ = scalar;
01060 }
01061
01062 inline const Point3 lerp( float t, const Point3 & pnt0, const Point3 & pnt1 )
01063 {
01064 return ( pnt0 + ( ( pnt1 - pnt0 ) * t ) );
01065 }
01066
01067 inline void loadXYZ( Point3 & pnt, const float * fptr )
01068 {
01069 pnt = Point3( fptr[0], fptr[1], fptr[2] );
01070 }
01071
01072 inline void storeXYZ( const Point3 & pnt, float * fptr )
01073 {
01074 fptr[0] = pnt.getX();
01075 fptr[1] = pnt.getY();
01076 fptr[2] = pnt.getZ();
01077 }
01078
01079 inline void loadHalfFloats( Point3 & vec, const unsigned short * hfptr )
01080 {
01081 union Data32 {
01082 unsigned int u32;
01083 float f32;
01084 };
01085
01086 for (int i = 0; i < 3; i++) {
01087 unsigned short fp16 = hfptr[i];
01088 unsigned int sign = fp16 >> 15;
01089 unsigned int exponent = (fp16 >> 10) & ((1 << 5) - 1);
01090 unsigned int mantissa = fp16 & ((1 << 10) - 1);
01091
01092 if (exponent == 0) {
01093
01094 mantissa = 0;
01095
01096 } else if (exponent == 31) {
01097
01098 exponent = 255;
01099 mantissa = 0;
01100
01101 } else {
01102 exponent += 127 - 15;
01103 mantissa <<= 13;
01104 }
01105
01106 Data32 d;
01107 d.u32 = (sign << 31) | (exponent << 23) | mantissa;
01108 vec[i] = d.f32;
01109 }
01110 }
01111
01112 inline void storeHalfFloats( const Point3 & vec, unsigned short * hfptr )
01113 {
01114 union Data32 {
01115 unsigned int u32;
01116 float f32;
01117 };
01118
01119 for (int i = 0; i < 3; i++) {
01120 Data32 d;
01121 d.f32 = vec[i];
01122
01123 unsigned int sign = d.u32 >> 31;
01124 unsigned int exponent = (d.u32 >> 23) & ((1 << 8) - 1);
01125 unsigned int mantissa = d.u32 & ((1 << 23) - 1);;
01126
01127 if (exponent == 0) {
01128
01129 mantissa = 0;
01130
01131 } else if (exponent == 255 && mantissa != 0) {
01132
01133 exponent = 31;
01134 mantissa = 0;
01135
01136 } else if (exponent >= 127 - 15 + 31) {
01137
01138 exponent = 31;
01139 mantissa = 0;
01140
01141 } else if (exponent <= 127 - 15) {
01142
01143 exponent = 0;
01144 mantissa = 0;
01145
01146 } else {
01147 exponent -= 127 - 15;
01148 mantissa >>= 13;
01149 }
01150
01151 hfptr[i] = (unsigned short)((sign << 15) | (exponent << 10) | mantissa);
01152 }
01153 }
01154
01155 inline Point3 & Point3::operator =( const Point3 & pnt )
01156 {
01157 mX = pnt.mX;
01158 mY = pnt.mY;
01159 mZ = pnt.mZ;
01160 return *this;
01161 }
01162
01163 inline Point3 & Point3::setX( float _x )
01164 {
01165 mX = _x;
01166 return *this;
01167 }
01168
01169 inline float Point3::getX( ) const
01170 {
01171 return mX;
01172 }
01173
01174 inline Point3 & Point3::setY( float _y )
01175 {
01176 mY = _y;
01177 return *this;
01178 }
01179
01180 inline float Point3::getY( ) const
01181 {
01182 return mY;
01183 }
01184
01185 inline Point3 & Point3::setZ( float _z )
01186 {
01187 mZ = _z;
01188 return *this;
01189 }
01190
01191 inline float Point3::getZ( ) const
01192 {
01193 return mZ;
01194 }
01195
01196 inline Point3 & Point3::setElem( int idx, float value )
01197 {
01198 *(&mX + idx) = value;
01199 return *this;
01200 }
01201
01202 inline float Point3::getElem( int idx ) const
01203 {
01204 return *(&mX + idx);
01205 }
01206
01207 inline float & Point3::operator []( int idx )
01208 {
01209 return *(&mX + idx);
01210 }
01211
01212 inline float Point3::operator []( int idx ) const
01213 {
01214 return *(&mX + idx);
01215 }
01216
01217 inline const Vector3 Point3::operator -( const Point3 & pnt ) const
01218 {
01219 return Vector3(
01220 ( mX - pnt.mX ),
01221 ( mY - pnt.mY ),
01222 ( mZ - pnt.mZ )
01223 );
01224 }
01225
01226 inline const Point3 Point3::operator +( const Vector3 & vec ) const
01227 {
01228 return Point3(
01229 ( mX + vec.getX() ),
01230 ( mY + vec.getY() ),
01231 ( mZ + vec.getZ() )
01232 );
01233 }
01234
01235 inline const Point3 Point3::operator -( const Vector3 & vec ) const
01236 {
01237 return Point3(
01238 ( mX - vec.getX() ),
01239 ( mY - vec.getY() ),
01240 ( mZ - vec.getZ() )
01241 );
01242 }
01243
01244 inline Point3 & Point3::operator +=( const Vector3 & vec )
01245 {
01246 *this = *this + vec;
01247 return *this;
01248 }
01249
01250 inline Point3 & Point3::operator -=( const Vector3 & vec )
01251 {
01252 *this = *this - vec;
01253 return *this;
01254 }
01255
01256 inline const Point3 mulPerElem( const Point3 & pnt0, const Point3 & pnt1 )
01257 {
01258 return Point3(
01259 ( pnt0.getX() * pnt1.getX() ),
01260 ( pnt0.getY() * pnt1.getY() ),
01261 ( pnt0.getZ() * pnt1.getZ() )
01262 );
01263 }
01264
01265 inline const Point3 divPerElem( const Point3 & pnt0, const Point3 & pnt1 )
01266 {
01267 return Point3(
01268 ( pnt0.getX() / pnt1.getX() ),
01269 ( pnt0.getY() / pnt1.getY() ),
01270 ( pnt0.getZ() / pnt1.getZ() )
01271 );
01272 }
01273
01274 inline const Point3 recipPerElem( const Point3 & pnt )
01275 {
01276 return Point3(
01277 ( 1.0f / pnt.getX() ),
01278 ( 1.0f / pnt.getY() ),
01279 ( 1.0f / pnt.getZ() )
01280 );
01281 }
01282
01283 inline const Point3 sqrtPerElem( const Point3 & pnt )
01284 {
01285 return Point3(
01286 sqrtf( pnt.getX() ),
01287 sqrtf( pnt.getY() ),
01288 sqrtf( pnt.getZ() )
01289 );
01290 }
01291
01292 inline const Point3 rsqrtPerElem( const Point3 & pnt )
01293 {
01294 return Point3(
01295 ( 1.0f / sqrtf( pnt.getX() ) ),
01296 ( 1.0f / sqrtf( pnt.getY() ) ),
01297 ( 1.0f / sqrtf( pnt.getZ() ) )
01298 );
01299 }
01300
01301 inline const Point3 absPerElem( const Point3 & pnt )
01302 {
01303 return Point3(
01304 fabsf( pnt.getX() ),
01305 fabsf( pnt.getY() ),
01306 fabsf( pnt.getZ() )
01307 );
01308 }
01309
01310 inline const Point3 copySignPerElem( const Point3 & pnt0, const Point3 & pnt1 )
01311 {
01312 return Point3(
01313 ( pnt1.getX() < 0.0f )? -fabsf( pnt0.getX() ) : fabsf( pnt0.getX() ),
01314 ( pnt1.getY() < 0.0f )? -fabsf( pnt0.getY() ) : fabsf( pnt0.getY() ),
01315 ( pnt1.getZ() < 0.0f )? -fabsf( pnt0.getZ() ) : fabsf( pnt0.getZ() )
01316 );
01317 }
01318
01319 inline const Point3 maxPerElem( const Point3 & pnt0, const Point3 & pnt1 )
01320 {
01321 return Point3(
01322 (pnt0.getX() > pnt1.getX())? pnt0.getX() : pnt1.getX(),
01323 (pnt0.getY() > pnt1.getY())? pnt0.getY() : pnt1.getY(),
01324 (pnt0.getZ() > pnt1.getZ())? pnt0.getZ() : pnt1.getZ()
01325 );
01326 }
01327
01328 inline float maxElem( const Point3 & pnt )
01329 {
01330 float result;
01331 result = (pnt.getX() > pnt.getY())? pnt.getX() : pnt.getY();
01332 result = (pnt.getZ() > result)? pnt.getZ() : result;
01333 return result;
01334 }
01335
01336 inline const Point3 minPerElem( const Point3 & pnt0, const Point3 & pnt1 )
01337 {
01338 return Point3(
01339 (pnt0.getX() < pnt1.getX())? pnt0.getX() : pnt1.getX(),
01340 (pnt0.getY() < pnt1.getY())? pnt0.getY() : pnt1.getY(),
01341 (pnt0.getZ() < pnt1.getZ())? pnt0.getZ() : pnt1.getZ()
01342 );
01343 }
01344
01345 inline float minElem( const Point3 & pnt )
01346 {
01347 float result;
01348 result = (pnt.getX() < pnt.getY())? pnt.getX() : pnt.getY();
01349 result = (pnt.getZ() < result)? pnt.getZ() : result;
01350 return result;
01351 }
01352
01353 inline float sum( const Point3 & pnt )
01354 {
01355 float result;
01356 result = ( pnt.getX() + pnt.getY() );
01357 result = ( result + pnt.getZ() );
01358 return result;
01359 }
01360
01361 inline const Point3 scale( const Point3 & pnt, float scaleVal )
01362 {
01363 return mulPerElem( pnt, Point3( scaleVal ) );
01364 }
01365
01366 inline const Point3 scale( const Point3 & pnt, const Vector3 & scaleVec )
01367 {
01368 return mulPerElem( pnt, Point3( scaleVec ) );
01369 }
01370
01371 inline float projection( const Point3 & pnt, const Vector3 & unitVec )
01372 {
01373 float result;
01374 result = ( pnt.getX() * unitVec.getX() );
01375 result = ( result + ( pnt.getY() * unitVec.getY() ) );
01376 result = ( result + ( pnt.getZ() * unitVec.getZ() ) );
01377 return result;
01378 }
01379
01380 inline float distSqrFromOrigin( const Point3 & pnt )
01381 {
01382 return lengthSqr( Vector3( pnt ) );
01383 }
01384
01385 inline float distFromOrigin( const Point3 & pnt )
01386 {
01387 return length( Vector3( pnt ) );
01388 }
01389
01390 inline float distSqr( const Point3 & pnt0, const Point3 & pnt1 )
01391 {
01392 return lengthSqr( ( pnt1 - pnt0 ) );
01393 }
01394
01395 inline float dist( const Point3 & pnt0, const Point3 & pnt1 )
01396 {
01397 return length( ( pnt1 - pnt0 ) );
01398 }
01399
01400 inline const Point3 select( const Point3 & pnt0, const Point3 & pnt1, bool select1 )
01401 {
01402 return Point3(
01403 ( select1 )? pnt1.getX() : pnt0.getX(),
01404 ( select1 )? pnt1.getY() : pnt0.getY(),
01405 ( select1 )? pnt1.getZ() : pnt0.getZ()
01406 );
01407 }
01408
01409 #ifdef _VECTORMATH_DEBUG
01410
01411 inline void print( const Point3 & pnt )
01412 {
01413 printf( "( %f %f %f )\n", pnt.getX(), pnt.getY(), pnt.getZ() );
01414 }
01415
01416 inline void print( const Point3 & pnt, const char * name )
01417 {
01418 printf( "%s: ( %f %f %f )\n", name, pnt.getX(), pnt.getY(), pnt.getZ() );
01419 }
01420
01421 #endif
01422
01423 }
01424 }
01425
01426 #endif
01427