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