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 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 }
00410 }
00411
00412 #endif
00413