scalar/quat_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_QUAT_AOS_CPP_H
00018 #define _VECTORMATH_QUAT_AOS_CPP_H
00019 
00020 //-----------------------------------------------------------------------------
00021 // Definitions
00022 
00023 #ifndef _VECTORMATH_INTERNAL_FUNCTIONS
00024 #define _VECTORMATH_INTERNAL_FUNCTIONS
00025 
00026 #endif
00027 
00028 namespace Vectormath {
00029 namespace Aos {
00030 
00031 inline Quat::Quat( const Quat & quat )
00032 {
00033     mX = quat.mX;
00034     mY = quat.mY;
00035     mZ = quat.mZ;
00036     mW = quat.mW;
00037 }
00038 
00039 inline Quat::Quat( float _x, float _y, float _z, float _w )
00040 {
00041     mX = _x;
00042     mY = _y;
00043     mZ = _z;
00044     mW = _w;
00045 }
00046 
00047 inline Quat::Quat( const Vector3 & xyz, float _w )
00048 {
00049     this->setXYZ( xyz );
00050     this->setW( _w );
00051 }
00052 
00053 inline Quat::Quat( const Vector4 & vec )
00054 {
00055     mX = vec.getX();
00056     mY = vec.getY();
00057     mZ = vec.getZ();
00058     mW = vec.getW();
00059 }
00060 
00061 inline Quat::Quat( float scalar )
00062 {
00063     mX = scalar;
00064     mY = scalar;
00065     mZ = scalar;
00066     mW = scalar;
00067 }
00068 
00069 inline const Quat Quat::identity( )
00070 {
00071     return Quat( 0.0f, 0.0f, 0.0f, 1.0f );
00072 }
00073 
00074 inline const Quat lerp( float t, const Quat & quat0, const Quat & quat1 )
00075 {
00076     return ( quat0 + ( ( quat1 - quat0 ) * t ) );
00077 }
00078 
00079 inline const Quat slerp( float t, const Quat & unitQuat0, const Quat & unitQuat1 )
00080 {
00081     Quat start;
00082     float recipSinAngle, scale0, scale1, cosAngle, angle;
00083     cosAngle = dot( unitQuat0, unitQuat1 );
00084     if ( cosAngle < 0.0f ) {
00085         cosAngle = -cosAngle;
00086         start = ( -unitQuat0 );
00087     } else {
00088         start = unitQuat0;
00089     }
00090     if ( cosAngle < _VECTORMATH_SLERP_TOL ) {
00091         angle = acosf( cosAngle );
00092         recipSinAngle = ( 1.0f / sinf( angle ) );
00093         scale0 = ( sinf( ( ( 1.0f - t ) * angle ) ) * recipSinAngle );
00094         scale1 = ( sinf( ( t * angle ) ) * recipSinAngle );
00095     } else {
00096         scale0 = ( 1.0f - t );
00097         scale1 = t;
00098     }
00099     return ( ( start * scale0 ) + ( unitQuat1 * scale1 ) );
00100 }
00101 
00102 inline const Quat squad( float t, const Quat & unitQuat0, const Quat & unitQuat1, const Quat & unitQuat2, const Quat & unitQuat3 )
00103 {
00104     Quat tmp0, tmp1;
00105     tmp0 = slerp( t, unitQuat0, unitQuat3 );
00106     tmp1 = slerp( t, unitQuat1, unitQuat2 );
00107     return slerp( ( ( 2.0f * t ) * ( 1.0f - t ) ), tmp0, tmp1 );
00108 }
00109 
00110 inline void loadXYZW( Quat & quat, const float * fptr )
00111 {
00112     quat = Quat( fptr[0], fptr[1], fptr[2], fptr[3] );
00113 }
00114 
00115 inline void storeXYZW( const Quat & quat, float * fptr )
00116 {
00117     fptr[0] = quat.getX();
00118     fptr[1] = quat.getY();
00119     fptr[2] = quat.getZ();
00120     fptr[3] = quat.getW();
00121 }
00122 
00123 inline Quat & Quat::operator =( const Quat & quat )
00124 {
00125     mX = quat.mX;
00126     mY = quat.mY;
00127     mZ = quat.mZ;
00128     mW = quat.mW;
00129     return *this;
00130 }
00131 
00132 inline Quat & Quat::setXYZ( const Vector3 & vec )
00133 {
00134     mX = vec.getX();
00135     mY = vec.getY();
00136     mZ = vec.getZ();
00137     return *this;
00138 }
00139 
00140 inline const Vector3 Quat::getXYZ( ) const
00141 {
00142     return Vector3( mX, mY, mZ );
00143 }
00144 
00145 inline Quat & Quat::setX( float _x )
00146 {
00147     mX = _x;
00148     return *this;
00149 }
00150 
00151 inline float Quat::getX( ) const
00152 {
00153     return mX;
00154 }
00155 
00156 inline Quat & Quat::setY( float _y )
00157 {
00158     mY = _y;
00159     return *this;
00160 }
00161 
00162 inline float Quat::getY( ) const
00163 {
00164     return mY;
00165 }
00166 
00167 inline Quat & Quat::setZ( float _z )
00168 {
00169     mZ = _z;
00170     return *this;
00171 }
00172 
00173 inline float Quat::getZ( ) const
00174 {
00175     return mZ;
00176 }
00177 
00178 inline Quat & Quat::setW( float _w )
00179 {
00180     mW = _w;
00181     return *this;
00182 }
00183 
00184 inline float Quat::getW( ) const
00185 {
00186     return mW;
00187 }
00188 
00189 inline Quat & Quat::setElem( int idx, float value )
00190 {
00191     *(&mX + idx) = value;
00192     return *this;
00193 }
00194 
00195 inline float Quat::getElem( int idx ) const
00196 {
00197     return *(&mX + idx);
00198 }
00199 
00200 inline float & Quat::operator []( int idx )
00201 {
00202     return *(&mX + idx);
00203 }
00204 
00205 inline float Quat::operator []( int idx ) const
00206 {
00207     return *(&mX + idx);
00208 }
00209 
00210 inline const Quat Quat::operator +( const Quat & quat ) const
00211 {
00212     return Quat(
00213         ( mX + quat.mX ),
00214         ( mY + quat.mY ),
00215         ( mZ + quat.mZ ),
00216         ( mW + quat.mW )
00217     );
00218 }
00219 
00220 inline const Quat Quat::operator -( const Quat & quat ) const
00221 {
00222     return Quat(
00223         ( mX - quat.mX ),
00224         ( mY - quat.mY ),
00225         ( mZ - quat.mZ ),
00226         ( mW - quat.mW )
00227     );
00228 }
00229 
00230 inline const Quat Quat::operator *( float scalar ) const
00231 {
00232     return Quat(
00233         ( mX * scalar ),
00234         ( mY * scalar ),
00235         ( mZ * scalar ),
00236         ( mW * scalar )
00237     );
00238 }
00239 
00240 inline Quat & Quat::operator +=( const Quat & quat )
00241 {
00242     *this = *this + quat;
00243     return *this;
00244 }
00245 
00246 inline Quat & Quat::operator -=( const Quat & quat )
00247 {
00248     *this = *this - quat;
00249     return *this;
00250 }
00251 
00252 inline Quat & Quat::operator *=( float scalar )
00253 {
00254     *this = *this * scalar;
00255     return *this;
00256 }
00257 
00258 inline const Quat Quat::operator /( float scalar ) const
00259 {
00260     return Quat(
00261         ( mX / scalar ),
00262         ( mY / scalar ),
00263         ( mZ / scalar ),
00264         ( mW / scalar )
00265     );
00266 }
00267 
00268 inline Quat & Quat::operator /=( float scalar )
00269 {
00270     *this = *this / scalar;
00271     return *this;
00272 }
00273 
00274 inline const Quat Quat::operator -( ) const
00275 {
00276     return Quat(
00277         -mX,
00278         -mY,
00279         -mZ,
00280         -mW
00281     );
00282 }
00283 
00284 inline const Quat operator *( float scalar, const Quat & quat )
00285 {
00286     return quat * scalar;
00287 }
00288 
00289 inline float dot( const Quat & quat0, const Quat & quat1 )
00290 {
00291     float result;
00292     result = ( quat0.getX() * quat1.getX() );
00293     result = ( result + ( quat0.getY() * quat1.getY() ) );
00294     result = ( result + ( quat0.getZ() * quat1.getZ() ) );
00295     result = ( result + ( quat0.getW() * quat1.getW() ) );
00296     return result;
00297 }
00298 
00299 inline float norm( const Quat & quat )
00300 {
00301     float result;
00302     result = ( quat.getX() * quat.getX() );
00303     result = ( result + ( quat.getY() * quat.getY() ) );
00304     result = ( result + ( quat.getZ() * quat.getZ() ) );
00305     result = ( result + ( quat.getW() * quat.getW() ) );
00306     return result;
00307 }
00308 
00309 inline float length( const Quat & quat )
00310 {
00311     return ::sqrtf( norm( quat ) );
00312 }
00313 
00314 inline const Quat normalize( const Quat & quat )
00315 {
00316     float lenSqr, lenInv;
00317     lenSqr = norm( quat );
00318     lenInv = ( 1.0f / sqrtf( lenSqr ) );
00319     return Quat(
00320         ( quat.getX() * lenInv ),
00321         ( quat.getY() * lenInv ),
00322         ( quat.getZ() * lenInv ),
00323         ( quat.getW() * lenInv )
00324     );
00325 }
00326 
00327 inline const Quat Quat::rotation( const Vector3 & unitVec0, const Vector3 & unitVec1 )
00328 {
00329     float cosHalfAngleX2, recipCosHalfAngleX2;
00330     cosHalfAngleX2 = sqrtf( ( 2.0f * ( 1.0f + dot( unitVec0, unitVec1 ) ) ) );
00331     recipCosHalfAngleX2 = ( 1.0f / cosHalfAngleX2 );
00332     return Quat( ( cross( unitVec0, unitVec1 ) * recipCosHalfAngleX2 ), ( cosHalfAngleX2 * 0.5f ) );
00333 }
00334 
00335 inline const Quat Quat::rotation( float radians, const Vector3 & unitVec )
00336 {
00337     float s, c, angle;
00338     angle = ( radians * 0.5f );
00339     s = sinf( angle );
00340     c = cosf( angle );
00341     return Quat( ( unitVec * s ), c );
00342 }
00343 
00344 inline const Quat Quat::rotationX( float radians )
00345 {
00346     float s, c, angle;
00347     angle = ( radians * 0.5f );
00348     s = sinf( angle );
00349     c = cosf( angle );
00350     return Quat( s, 0.0f, 0.0f, c );
00351 }
00352 
00353 inline const Quat Quat::rotationY( float radians )
00354 {
00355     float s, c, angle;
00356     angle = ( radians * 0.5f );
00357     s = sinf( angle );
00358     c = cosf( angle );
00359     return Quat( 0.0f, s, 0.0f, c );
00360 }
00361 
00362 inline const Quat Quat::rotationZ( float radians )
00363 {
00364     float s, c, angle;
00365     angle = ( radians * 0.5f );
00366     s = sinf( angle );
00367     c = cosf( angle );
00368     return Quat( 0.0f, 0.0f, s, c );
00369 }
00370 
00371 inline const Quat Quat::operator *( const Quat & quat ) const
00372 {
00373     return Quat(
00374         ( ( ( ( mW * quat.mX ) + ( mX * quat.mW ) ) + ( mY * quat.mZ ) ) - ( mZ * quat.mY ) ),
00375         ( ( ( ( mW * quat.mY ) + ( mY * quat.mW ) ) + ( mZ * quat.mX ) ) - ( mX * quat.mZ ) ),
00376         ( ( ( ( mW * quat.mZ ) + ( mZ * quat.mW ) ) + ( mX * quat.mY ) ) - ( mY * quat.mX ) ),
00377         ( ( ( ( mW * quat.mW ) - ( mX * quat.mX ) ) - ( mY * quat.mY ) ) - ( mZ * quat.mZ ) )
00378     );
00379 }
00380 
00381 inline Quat & Quat::operator *=( const Quat & quat )
00382 {
00383     *this = *this * quat;
00384     return *this;
00385 }
00386 
00387 inline const Vector3 rotate( const Quat & quat, const Vector3 & vec )
00388 {
00389     float tmpX, tmpY, tmpZ, tmpW;
00390     tmpX = ( ( ( quat.getW() * vec.getX() ) + ( quat.getY() * vec.getZ() ) ) - ( quat.getZ() * vec.getY() ) );
00391     tmpY = ( ( ( quat.getW() * vec.getY() ) + ( quat.getZ() * vec.getX() ) ) - ( quat.getX() * vec.getZ() ) );
00392     tmpZ = ( ( ( quat.getW() * vec.getZ() ) + ( quat.getX() * vec.getY() ) ) - ( quat.getY() * vec.getX() ) );
00393     tmpW = ( ( ( quat.getX() * vec.getX() ) + ( quat.getY() * vec.getY() ) ) + ( quat.getZ() * vec.getZ() ) );
00394     return Vector3(
00395         ( ( ( ( tmpW * quat.getX() ) + ( tmpX * quat.getW() ) ) - ( tmpY * quat.getZ() ) ) + ( tmpZ * quat.getY() ) ),
00396         ( ( ( ( tmpW * quat.getY() ) + ( tmpY * quat.getW() ) ) - ( tmpZ * quat.getX() ) ) + ( tmpX * quat.getZ() ) ),
00397         ( ( ( ( tmpW * quat.getZ() ) + ( tmpZ * quat.getW() ) ) - ( tmpX * quat.getY() ) ) + ( tmpY * quat.getX() ) )
00398     );
00399 }
00400 
00401 inline const Quat conj( const Quat & quat )
00402 {
00403     return Quat( -quat.getX(), -quat.getY(), -quat.getZ(), quat.getW() );
00404 }
00405 
00406 inline const Quat select( const Quat & quat0, const Quat & quat1, bool select1 )
00407 {
00408     return Quat(
00409         ( select1 )? quat1.getX() : quat0.getX(),
00410         ( select1 )? quat1.getY() : quat0.getY(),
00411         ( select1 )? quat1.getZ() : quat0.getZ(),
00412         ( select1 )? quat1.getW() : quat0.getW()
00413     );
00414 }
00415 
00416 #ifdef _VECTORMATH_DEBUG
00417 
00418 inline void print( const Quat & quat )
00419 {
00420     printf( "( %f %f %f %f )\n", quat.getX(), quat.getY(), quat.getZ(), quat.getW() );
00421 }
00422 
00423 inline void print( const Quat & quat, const char * name )
00424 {
00425     printf( "%s: ( %f %f %f %f )\n", name, quat.getX(), quat.getY(), quat.getZ(), quat.getW() );
00426 }
00427 
00428 #endif
00429 
00430 } // namespace Aos
00431 } // namespace Vectormath
00432 
00433 #endif