00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #ifndef _VECTORMATH_QUAT_AOS_CPP_H
00018 #define _VECTORMATH_QUAT_AOS_CPP_H
00019
00020
00021
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 }
00431 }
00432
00433 #endif