scalar/vec_aos.h

Go to the documentation of this file.
00001 /*
00002    Copyright (C) 2009 Sony Computer Entertainment Inc.
00003    All rights reserved.
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 
00015 */
00016 
00017 #ifndef _VECTORMATH_VEC_AOS_CPP_H
00018 #define _VECTORMATH_VEC_AOS_CPP_H
00019 
00020 //-----------------------------------------------------------------------------
00021 // Constants
00022 
00023 #define _VECTORMATH_SLERP_TOL 0.999f
00024 
00025 //-----------------------------------------------------------------------------
00026 // Definitions
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             // zero
00127             mantissa = 0;
00128 
00129         } else if (exponent == 31) {
00130             // infinity or nan -> infinity
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             // zero or denorm -> zero
00162             mantissa = 0;
00163 
00164         } else if (exponent == 255 && mantissa != 0) {
00165             // nan -> infinity
00166             exponent = 31;
00167             mantissa = 0;
00168 
00169         } else if (exponent >= 127 - 15 + 31) {
00170             // overflow or infinity -> infinity
00171             exponent = 31;
00172             mantissa = 0;
00173 
00174         } else if (exponent <= 127 - 15) {
00175             // underflow -> zero
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             // zero
00628             mantissa = 0;
00629 
00630         } else if (exponent == 31) {
00631             // infinity or nan -> infinity
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             // zero or denorm -> zero
00663             mantissa = 0;
00664 
00665         } else if (exponent == 255 && mantissa != 0) {
00666             // nan -> infinity
00667             exponent = 31;
00668             mantissa = 0;
00669 
00670         } else if (exponent >= 127 - 15 + 31) {
00671             // overflow or infinity -> infinity
00672             exponent = 31;
00673             mantissa = 0;
00674 
00675         } else if (exponent <= 127 - 15) {
00676             // underflow -> zero
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             // zero
01094             mantissa = 0;
01095 
01096         } else if (exponent == 31) {
01097             // infinity or nan -> infinity
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             // zero or denorm -> zero
01129             mantissa = 0;
01130 
01131         } else if (exponent == 255 && mantissa != 0) {
01132             // nan -> infinity
01133             exponent = 31;
01134             mantissa = 0;
01135 
01136         } else if (exponent >= 127 - 15 + 31) {
01137             // overflow or infinity -> infinity
01138             exponent = 31;
01139             mantissa = 0;
01140 
01141         } else if (exponent <= 127 - 15) {
01142             // underflow -> zero
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 } // namespace Aos
01424 } // namespace Vectormath
01425 
01426 #endif