src/vectormath/scalar/quat_aos.h

   1 /*
   2    Copyright (C) 2009 Sony Computer Entertainment Inc.
   3    All rights reserved.
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 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.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14
  15 */
  16
  17 #ifndef _VECTORMATH_QUAT_AOS_CPP_H
  18 #define _VECTORMATH_QUAT_AOS_CPP_H
  19
  20 //-----------------------------------------------------------------------------
  21 // Definitions
  22
  23 #ifndef _VECTORMATH_INTERNAL_FUNCTIONS
  24 #define _VECTORMATH_INTERNAL_FUNCTIONS
  25
  26 #endif
  27
  28 namespace Vectormath {
  29 namespace Aos {
  30
  31 inline Quat::Quat( const Quat & quat )
  32 {
  33     mX = quat.mX;
  34     mY = quat.mY;
  35     mZ = quat.mZ;
  36     mW = quat.mW;
  37 }
  38
  39 inline Quat::Quat( float _x, float _y, float _z, float _w )
  40 {
  41     mX = _x;
  42     mY = _y;
  43     mZ = _z;
  44     mW = _w;
  45 }
  46
  47 inline Quat::Quat( const Vector3 & xyz, float _w )
  48 {
  49     this->setXYZ( xyz );
  50     this->setW( _w );
  51 }
  52
  53 inline Quat::Quat( const Vector4 & vec )
  54 {
  55     mX = vec.getX();
  56     mY = vec.getY();
  57     mZ = vec.getZ();
  58     mW = vec.getW();
  59 }
  60
  61 inline Quat::Quat( float scalar )
  62 {
  63     mX = scalar;
  64     mY = scalar;
  65     mZ = scalar;
  66     mW = scalar;
  67 }
  68
  69 inline const Quat Quat::identity( )
  70 {
  71     return Quat( 0.0f, 0.0f, 0.0f, 1.0f );
  72 }
  73
  74 inline const Quat lerp( float t, const Quat & quat0, const Quat & quat1 )
  75 {
  76     return ( quat0 + ( ( quat1 - quat0 ) * t ) );
  77 }
  78
  79 inline const Quat slerp( float t, const Quat & unitQuat0, const Quat & unitQuat1 )
  80 {
  81     Quat start;
  82     float recipSinAngle, scale0, scale1, cosAngle, angle;
  83     cosAngle = dot( unitQuat0, unitQuat1 );
  84     if ( cosAngle < 0.0f ) {
  85         cosAngle = -cosAngle;
  86         start = ( -unitQuat0 );
  87     } else {
  88         start = unitQuat0;
  89     }
  90     if ( cosAngle < _VECTORMATH_SLERP_TOL ) {
  91         angle = acosf( cosAngle );
  92         recipSinAngle = ( 1.0f / sinf( angle ) );
  93         scale0 = ( sinf( ( ( 1.0f - t ) * angle ) ) * recipSinAngle );
  94         scale1 = ( sinf( ( t * angle ) ) * recipSinAngle );
  95     } else {
  96         scale0 = ( 1.0f - t );
  97         scale1 = t;
  98     }
  99     return ( ( start * scale0 ) + ( unitQuat1 * scale1 ) );
 100 }
 101
 102 inline const Quat squad( float t, const Quat & unitQuat0, const Quat & unitQuat1, const Quat & unitQuat2, const Quat & unitQuat3 )
 103 {
 104     Quat tmp0, tmp1;
 105     tmp0 = slerp( t, unitQuat0, unitQuat3 );
 106     tmp1 = slerp( t, unitQuat1, unitQuat2 );
 107     return slerp( ( ( 2.0f * t ) * ( 1.0f - t ) ), tmp0, tmp1 );
 108 }
 109
 110 inline void loadXYZW( Quat & quat, const float * fptr )
 111 {
 112     quat = Quat( fptr[0], fptr[1], fptr[2], fptr[3] );
 113 }
 114
 115 inline void storeXYZW( const Quat & quat, float * fptr )
 116 {
 117     fptr[0] = quat.getX();
 118     fptr[1] = quat.getY();
 119     fptr[2] = quat.getZ();
 120     fptr[3] = quat.getW();
 121 }
 122
 123 inline Quat & Quat::operator =( const Quat & quat )
 124 {
 125     mX = quat.mX;
 126     mY = quat.mY;
 127     mZ = quat.mZ;
 128     mW = quat.mW;
 129     return *this;
 130 }
 131
 132 inline Quat & Quat::setXYZ( const Vector3 & vec )
 133 {
 134     mX = vec.getX();
 135     mY = vec.getY();
 136     mZ = vec.getZ();
 137     return *this;
 138 }
 139
 140 inline const Vector3 Quat::getXYZ( ) const
 141 {
 142     return Vector3( mX, mY, mZ );
 143 }
 144
 145 inline Quat & Quat::setX( float _x )
 146 {
 147     mX = _x;
 148     return *this;
 149 }
 150
 151 inline float Quat::getX( ) const
 152 {
 153     return mX;
 154 }
 155
 156 inline Quat & Quat::setY( float _y )
 157 {
 158     mY = _y;
 159     return *this;
 160 }
 161
 162 inline float Quat::getY( ) const
 163 {
 164     return mY;
 165 }
 166
 167 inline Quat & Quat::setZ( float _z )
 168 {
 169     mZ = _z;
 170     return *this;
 171 }
 172
 173 inline float Quat::getZ( ) const
 174 {
 175     return mZ;
 176 }
 177
 178 inline Quat & Quat::setW( float _w )
 179 {
 180     mW = _w;
 181     return *this;
 182 }
 183
 184 inline float Quat::getW( ) const
 185 {
 186     return mW;
 187 }
 188
 189 inline Quat & Quat::setElem( int idx, float value )
 190 {
 191     *(&mX + idx) = value;
 192     return *this;
 193 }
 194
 195 inline float Quat::getElem( int idx ) const
 196 {
 197     return *(&mX + idx);
 198 }
 199
 200 inline float & Quat::operator []( int idx )
 201 {
 202     return *(&mX + idx);
 203 }
 204
 205 inline float Quat::operator []( int idx ) const
 206 {
 207     return *(&mX + idx);
 208 }
 209
 210 inline const Quat Quat::operator +( const Quat & quat ) const
 211 {
 212     return Quat(
 213         ( mX + quat.mX ),
 214         ( mY + quat.mY ),
 215         ( mZ + quat.mZ ),
 216         ( mW + quat.mW )
 217     );
 218 }
 219
 220 inline const Quat Quat::operator -( const Quat & quat ) const
 221 {
 222     return Quat(
 223         ( mX - quat.mX ),
 224         ( mY - quat.mY ),
 225         ( mZ - quat.mZ ),
 226         ( mW - quat.mW )
 227     );
 228 }
 229
 230 inline const Quat Quat::operator *( float scalar ) const
 231 {
 232     return Quat(
 233         ( mX * scalar ),
 234         ( mY * scalar ),
 235         ( mZ * scalar ),
 236         ( mW * scalar )
 237     );
 238 }
 239
 240 inline Quat & Quat::operator +=( const Quat & quat )
 241 {
 242     *this = *this + quat;
 243     return *this;
 244 }
 245
 246 inline Quat & Quat::operator -=( const Quat & quat )
 247 {
 248     *this = *this - quat;
 249     return *this;
 250 }
 251
 252 inline Quat & Quat::operator *=( float scalar )
 253 {
 254     *this = *this * scalar;
 255     return *this;
 256 }
 257
 258 inline const Quat Quat::operator /( float scalar ) const
 259 {
 260     return Quat(
 261         ( mX / scalar ),
 262         ( mY / scalar ),
 263         ( mZ / scalar ),
 264         ( mW / scalar )
 265     );
 266 }
 267
 268 inline Quat & Quat::operator /=( float scalar )
 269 {
 270     *this = *this / scalar;
 271     return *this;
 272 }
 273
 274 inline const Quat Quat::operator -( ) const
 275 {
 276     return Quat(
 277         -mX,
 278         -mY,
 279         -mZ,
 280         -mW
 281     );
 282 }
 283
 284 inline const Quat operator *( float scalar, const Quat & quat )
 285 {
 286     return quat * scalar;
 287 }
 288
 289 inline float dot( const Quat & quat0, const Quat & quat1 )
 290 {
 291     float result;
 292     result = ( quat0.getX() * quat1.getX() );
 293     result = ( result + ( quat0.getY() * quat1.getY() ) );
 294     result = ( result + ( quat0.getZ() * quat1.getZ() ) );
 295     result = ( result + ( quat0.getW() * quat1.getW() ) );
 296     return result;
 297 }
 298
 299 inline float norm( const Quat & quat )
 300 {
 301     float result;
 302     result = ( quat.getX() * quat.getX() );
 303     result = ( result + ( quat.getY() * quat.getY() ) );
 304     result = ( result + ( quat.getZ() * quat.getZ() ) );
 305     result = ( result + ( quat.getW() * quat.getW() ) );
 306     return result;
 307 }
 308
 309 inline float length( const Quat & quat )
 310 {
 311     return ::sqrtf( norm( quat ) );
 312 }
 313
 314 inline const Quat normalize( const Quat & quat )
 315 {
 316     float lenSqr, lenInv;
 317     lenSqr = norm( quat );
 318     lenInv = ( 1.0f / sqrtf( lenSqr ) );
 319     return Quat(
 320         ( quat.getX() * lenInv ),
 321         ( quat.getY() * lenInv ),
 322         ( quat.getZ() * lenInv ),
 323         ( quat.getW() * lenInv )
 324     );
 325 }
 326
 327 inline const Quat Quat::rotation( const Vector3 & unitVec0, const Vector3 & unitVec1 )
 328 {
 329     float cosHalfAngleX2, recipCosHalfAngleX2;
 330     cosHalfAngleX2 = sqrtf( ( 2.0f * ( 1.0f + dot( unitVec0, unitVec1 ) ) ) );
 331     recipCosHalfAngleX2 = ( 1.0f / cosHalfAngleX2 );
 332     return Quat( ( cross( unitVec0, unitVec1 ) * recipCosHalfAngleX2 ), ( cosHalfAngleX2 * 0.5f ) );
 333 }
 334
 335 inline const Quat Quat::rotation( float radians, const Vector3 & unitVec )
 336 {
 337     float s, c, angle;
 338     angle = ( radians * 0.5f );
 339     s = sinf( angle );
 340     c = cosf( angle );
 341     return Quat( ( unitVec * s ), c );
 342 }
 343
 344 inline const Quat Quat::rotationX( float radians )
 345 {
 346     float s, c, angle;
 347     angle = ( radians * 0.5f );
 348     s = sinf( angle );
 349     c = cosf( angle );
 350     return Quat( s, 0.0f, 0.0f, c );
 351 }
 352
 353 inline const Quat Quat::rotationY( float radians )
 354 {
 355     float s, c, angle;
 356     angle = ( radians * 0.5f );
 357     s = sinf( angle );
 358     c = cosf( angle );
 359     return Quat( 0.0f, s, 0.0f, c );
 360 }
 361
 362 inline const Quat Quat::rotationZ( float radians )
 363 {
 364     float s, c, angle;
 365     angle = ( radians * 0.5f );
 366     s = sinf( angle );
 367     c = cosf( angle );
 368     return Quat( 0.0f, 0.0f, s, c );
 369 }
 370
 371 inline const Quat Quat::operator *( const Quat & quat ) const
 372 {
 373     return Quat(
 374         ( ( ( ( mW * quat.mX ) + ( mX * quat.mW ) ) + ( mY * quat.mZ ) ) - ( mZ * quat.mY ) ),
 375         ( ( ( ( mW * quat.mY ) + ( mY * quat.mW ) ) + ( mZ * quat.mX ) ) - ( mX * quat.mZ ) ),
 376         ( ( ( ( mW * quat.mZ ) + ( mZ * quat.mW ) ) + ( mX * quat.mY ) ) - ( mY * quat.mX ) ),
 377         ( ( ( ( mW * quat.mW ) - ( mX * quat.mX ) ) - ( mY * quat.mY ) ) - ( mZ * quat.mZ ) )
 378     );
 379 }
 380
 381 inline Quat & Quat::operator *=( const Quat & quat )
 382 {
 383     *this = *this * quat;
 384     return *this;
 385 }
 386
 387 inline const Vector3 rotate( const Quat & quat, const Vector3 & vec )
 388 {
 389     float tmpX, tmpY, tmpZ, tmpW;
 390     tmpX = ( ( ( quat.getW() * vec.getX() ) + ( quat.getY() * vec.getZ() ) ) - ( quat.getZ() * vec.getY() ) );
 391     tmpY = ( ( ( quat.getW() * vec.getY() ) + ( quat.getZ() * vec.getX() ) ) - ( quat.getX() * vec.getZ() ) );
 392     tmpZ = ( ( ( quat.getW() * vec.getZ() ) + ( quat.getX() * vec.getY() ) ) - ( quat.getY() * vec.getX() ) );
 393     tmpW = ( ( ( quat.getX() * vec.getX() ) + ( quat.getY() * vec.getY() ) ) + ( quat.getZ() * vec.getZ() ) );
 394     return Vector3(
 395         ( ( ( ( tmpW * quat.getX() ) + ( tmpX * quat.getW() ) ) - ( tmpY * quat.getZ() ) ) + ( tmpZ * quat.getY() ) ),
 396         ( ( ( ( tmpW * quat.getY() ) + ( tmpY * quat.getW() ) ) - ( tmpZ * quat.getX() ) ) + ( tmpX * quat.getZ() ) ),
 397         ( ( ( ( tmpW * quat.getZ() ) + ( tmpZ * quat.getW() ) ) - ( tmpX * quat.getY() ) ) + ( tmpY * quat.getX() ) )
 398     );
 399 }
 400
 401 inline const Quat conj( const Quat & quat )
 402 {
 403     return Quat( -quat.getX(), -quat.getY(), -quat.getZ(), quat.getW() );
 404 }
 405
 406 inline const Quat select( const Quat & quat0, const Quat & quat1, bool select1 )
 407 {
 408     return Quat(
 409         ( select1 )? quat1.getX() : quat0.getX(),
 410         ( select1 )? quat1.getY() : quat0.getY(),
 411         ( select1 )? quat1.getZ() : quat0.getZ(),
 412         ( select1 )? quat1.getW() : quat0.getW()
 413     );
 414 }
 415
 416 #ifdef _VECTORMATH_DEBUG
 417
 418 inline void print( const Quat & quat )
 419 {
 420     printf( "( %f %f %f %f )\n", quat.getX(), quat.getY(), quat.getZ(), quat.getW() );
 421 }
 422
 423 inline void print( const Quat & quat, const char * name )
 424 {
 425     printf( "%s: ( %f %f %f %f )\n", name, quat.getX(), quat.getY(), quat.getZ(), quat.getW() );
 426 }
 427
 428 #endif
 429
 430 } // namespace Aos
 431 } // namespace Vectormath
 432
 433 #endif