00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #pragma once
00024
00025 #include "cinder/CinderMath.h"
00026 #include "cinder/Vector.h"
00027 #include "cinder/Matrix.h"
00028
00029 namespace cinder {
00030
00031 template<typename T, typename Y>
00032 struct QUATCONV {
00033 typedef typename T::TYPE F;
00034 static F getW( const Y &v ) { return static_cast<F>( v.w ); }
00035 static F getX( const Y &v ) { return static_cast<F>( v.x ); }
00036 static F getY( const Y &v ) { return static_cast<F>( v.y ); }
00037 static F getZ( const Y &v ) { return static_cast<F>( v.z ); }
00038 };
00039
00040 template<typename T>
00041 class Quaternion
00042 {
00043 public:
00044 typedef T TYPE;
00045 typedef T value_type;
00046
00047 Vec3<T> v;
00048 T w;
00049
00050 Quaternion(): w( 1 ), v( 0, 0, 0 ){}
00051 template<typename FromT>
00052 Quaternion( const Quaternion<FromT>& q ) : w( static_cast<T>( q.w ) ), v( q.v ) {}
00053
00054 Quaternion( T aW, T x, T y, T z ): w( aW ), v( x, y, z ) {}
00055
00056 Quaternion( const Vec3<T> &axis, T radians ) { set( axis, radians ); }
00057 Quaternion( const Vec3<T> &from, const Vec3<T> &to ) { set( from, to ); }
00058
00059 Quaternion( T xRotation, T yRotation, T zRotation ) { set( xRotation, yRotation, zRotation ); }
00060 Quaternion( const Matrix33<T> &m ) { set( m ); }
00061 Quaternion( const Matrix44<T> &m ) { set( m ); }
00062 template<typename Y>
00063 explicit Quaternion( const Y &v )
00064 : w( QUATCONV<Quaternion<typename Quaternion::TYPE>,Y>::getW( v ) ), v( QUATCONV<typename Quaternion::TYPE,Y>::getX( v ), QUATCONV<typename Quaternion::TYPE,Y>::getY( v ), QUATCONV<typename Quaternion::TYPE,Y>::getZ( v ) )
00065 {}
00066
00067
00068 Vec3<T> getAxis() const
00069 {
00070 T cos_angle = w;
00071 T invLen = static_cast<T>( 1.0 ) / math<T>::sqrt( static_cast<T>( 1.0 ) - cos_angle * cos_angle );
00072
00073 return v * invLen;
00074 }
00075
00076
00077 T getAngle() const
00078 {
00079 T cos_angle = w;
00080 return math<T>::acos( cos_angle ) * 2;
00081 }
00082
00083 T getPitch() const
00084 {
00085 return math<T>::atan2( (T)2 * ( v.y * v.z + w * v.x ), w * w - v.x * v.x - v.y * v.y + v.z * v.z );
00086 }
00087
00088 T getYaw() const
00089 {
00090 return math<T>::sin( (T)-2 * ( v.x * v.z - w * v.y ) );
00091 }
00092
00093 T getRoll() const
00094 {
00095 return math<T>::atan2( (T)2 * ( v.x * v.y + w * v.z), w * w + v.x * v.x - v.y * v.y - v.z * v.z );
00096 }
00097
00098 T dot( const Quaternion<T> &quat ) const
00099 {
00100 return w * quat.w + v.dot( quat.v );
00101 }
00102
00103 T length() const
00104 {
00105 return (T)std::sqrt( w*w + v.lengthSquared() );
00106 }
00107
00108 T lengthSquared() const
00109 {
00110 return w * w + v.lengthSquared();
00111 }
00112
00113 Quaternion<T> inverse() const
00114 {
00115 T norm = w * w + v.x * v.x + v.y * v.y + v.z * v.z;
00116
00117
00118
00119
00120
00121 T normRecip = static_cast<T>( 1.0f ) / norm;
00122 return Quaternion<T>( normRecip * w, -normRecip * v.x, -normRecip * v.y, -normRecip * v.z );
00123 }
00124
00125 void normalize()
00126 {
00127 if( T len = length() ) {
00128 w /= len;
00129 v /= len;
00130 }
00131 else {
00132 w = static_cast<T>( 1.0 );
00133 v = Vec3<T>( 0, 0, 0 );
00134 }
00135 }
00136
00137 Quaternion<T> normalized() const
00138 {
00139 Quaternion<T> result = *this;
00140
00141 if( T len = length() ) {
00142 result.w /= len;
00143 result.v /= len;
00144 }
00145 else {
00146 result.w = static_cast<T>( 1.0 );
00147 result.v = Vec3<T>( 0, 0, 0 );
00148 }
00149
00150 return result;
00151 }
00152
00153
00154
00155 Quaternion<T> log() const
00156 {
00157 T theta = math<T>::acos( std::min( w, (T) 1.0 ) );
00158
00159 if( theta == 0 )
00160 return Quaternion<T>( v, 0 );
00161
00162 T sintheta = math<T>::sin( theta );
00163
00164 T k;
00165 if ( abs( sintheta ) < 1 && abs( theta ) >= 3.402823466e+38F * abs( sintheta ) )
00166 k = 1;
00167 else
00168 k = theta / sintheta;
00169
00170 return Quaternion<T>( (T)0, v.x * k, v.y * k, v.z * k );
00171 }
00172
00173
00174
00175
00176 Quaternion<T> exp() const
00177 {
00178 T theta = v.length();
00179 T sintheta = math<T>::sin( theta );
00180
00181 T k;
00182 if( abs( theta ) < 1 && abs( sintheta ) >= 3.402823466e+38F * abs( theta ) )
00183 k = 1;
00184 else
00185 k = sintheta / theta;
00186
00187 T costheta = math<T>::cos( theta );
00188
00189 return Quaternion<T>( costheta, v.x * k, v.y * k, v.z * k );
00190 }
00191
00192 Quaternion<T> inverted() const
00193 {
00194 T qdot = this->dot( *this );
00195 return Quaternion( -v / qdot, w / qdot );
00196 }
00197
00198 void invert()
00199 {
00200 T qdot = this->dot( *this );
00201 set( -v / qdot, w / qdot );
00202 }
00203
00204 void set( T aW, T x, T y, T z )
00205 {
00206 w = aW;
00207 v = Vec3<T>( x, y, z );
00208 }
00209
00210 #if 1
00211 void set( const Vec3<T> &from, const Vec3<T> &to )
00212 {
00213 Vec3<T> axis = from.cross( to );
00214
00215 set( from.dot( to ), axis.x, axis.y, axis.z );
00216 normalize();
00217
00218 w += static_cast<T>( 1.0 );
00219
00220 if( w <= EPSILON ) {
00221 if ( from.z * from.z > from.x * from.x ) {
00222 set( static_cast<T>( 0.0 ), static_cast<T>( 0.0 ), from.z, -from.y );
00223 }
00224 else {
00225 set( static_cast<T>( 0.0 ), from.y, -from.x, static_cast<T>( 0.0 ) );
00226 }
00227 }
00228
00229 normalize();
00230 }
00231 #else
00232 void set( const Vec3<T> &from, const Vec3<T> &to )
00233 {
00234 Vec3<T> f0 = from.normalized();
00235 Vec3<T> t0 = to.normalized();
00236
00237 if( f0.dot( t0 ) >= 0 ) {
00238 setRotationInternal (f0, t0, *this);
00239 }
00240 else {
00241
00242
00243
00244
00245 Vec3<T> h0 = (f0 + t0).normalized();
00246
00247 if( h0.dot( h0 ) != 0 ) {
00248 setRotationInternal( f0, h0, *this );
00249 Quaternion<T> q;
00250 setRotationInternal( h0, t0, q );
00251
00252 set( q.w*w - q.v.x*v.x - q.v.y*v.y - q.v.z*v.z,
00253 q.w*v.x + q.v.x*w + q.v.y*v.z - q.v.z*v.y,
00254 q.w*v.y + q.v.y*w + q.v.z*v.x - q.v.x*v.z,
00255 q.w*v.z + q.v.z*w + q.v.x*v.y - q.v.y*v.x );
00256
00257
00258 }
00259 else {
00260
00261
00262
00263
00264 w = T( 0 );
00265
00266 Vec3<T> f02 = f0 * f0;
00267
00268 if( ( f02.x <= f02.y ) && ( f02.x <= f02.z ) )
00269 v = f0.cross( Vec3<T>( 1, 0, 0 ) ).normalized();
00270 else if( f02.y <= f02.z )
00271 v = f0.cross( Vec3<T>( 0, 1, 0 ) ).normalized();
00272 else
00273 v = f0.cross( Vec3<T>( 0, 0, 1 ) ).normalized();
00274 }
00275 }
00276 }
00277
00278 static void setRotationInternal( const Vec3<T> &f0, const Vec3<T> &t0, Quaternion<T> &q )
00279 {
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294 Vec3<T> h0 = ( f0 + t0 ).normalized();
00295
00296
00297 q.w = f0.dot( h0 );
00298 q.v = f0.cross( h0 );
00299 }
00300 #endif
00301
00302 void set( const Vec3<T> &axis, T radians )
00303 {
00304 w = math<T>::cos( radians / 2 );
00305 v = axis.normalized() * math<T>::sin( radians / 2 );
00306 }
00307
00308
00309 void set( T xRotation, T yRotation, T zRotation )
00310 {
00311 zRotation *= T( 0.5 );
00312 yRotation *= T( 0.5 );
00313 xRotation *= T( 0.5 );
00314
00315
00316 T Cx = math<T>::cos( xRotation );
00317 T Sx = math<T>::sin( xRotation );
00318
00319 T Cy = math<T>::cos( yRotation );
00320 T Sy = math<T>::sin( yRotation );
00321
00322 T Cz = math<T>::cos( zRotation );
00323 T Sz = math<T>::sin( zRotation );
00324
00325
00326 w = Cx*Cy*Cz - Sx*Sy*Sz;
00327 v.x = Sx*Cy*Cz + Cx*Sy*Sz;
00328 v.y = Cx*Sy*Cz - Sx*Cy*Sz;
00329 v.z = Cx*Cy*Sz + Sx*Sy*Cx;
00330 }
00331
00332 void getAxisAngle( Vec3<T> *axis, T *radians ) const
00333 {
00334 T cos_angle = w;
00335 *radians = math<T>::acos( cos_angle ) * 2;
00336 T invLen = static_cast<T>( 1.0 ) / math<T>::sqrt( static_cast<T>( 1.0 ) - cos_angle * cos_angle );
00337
00338 axis->x = v.x * invLen;
00339 axis->y = v.y * invLen;
00340 axis->z = v.z * invLen;
00341 }
00342
00343 Matrix33<T> toMatrix33() const
00344 {
00345 Matrix33<T> mV;
00346 T xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
00347
00348 xs = v.x + v.x;
00349 ys = v.y + v.y;
00350 zs = v.z + v.z;
00351 wx = w * xs;
00352 wy = w * ys;
00353 wz = w * zs;
00354 xx = v.x * xs;
00355 xy = v.x * ys;
00356 xz = v.x * zs;
00357 yy = v.y * ys;
00358 yz = v.y * zs;
00359 zz = v.z * zs;
00360
00361 mV[0] = T( 1 ) - ( yy + zz );
00362 mV[3] = xy - wz;
00363 mV[6] = xz + wy;
00364
00365 mV[1] = xy + wz;
00366 mV[4] = T( 1 ) - ( xx + zz );
00367 mV[7] = yz - wx;
00368
00369 mV[2] = xz - wy;
00370 mV[5] = yz + wx;
00371 mV[8] = T( 1 ) - ( xx + yy );
00372
00373 return mV;
00374 }
00375
00376 Matrix44<T> toMatrix44() const
00377 {
00378 Matrix44<T> mV;
00379 T xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
00380
00381 xs = v.x + v.x;
00382 ys = v.y + v.y;
00383 zs = v.z + v.z;
00384 wx = w * xs;
00385 wy = w * ys;
00386 wz = w * zs;
00387 xx = v.x * xs;
00388 xy = v.x * ys;
00389 xz = v.x * zs;
00390 yy = v.y * ys;
00391 yz = v.y * zs;
00392 zz = v.z * zs;
00393
00394 mV[0] = T( 1 ) - ( yy + zz );
00395 mV[4] = xy - wz;
00396 mV[8] = xz + wy;
00397 mV[12] = 0;
00398
00399 mV[1] = xy + wz;
00400 mV[5] = T( 1 ) - ( xx + zz );
00401 mV[9] = yz - wx;
00402 mV[13] = 0;
00403
00404 mV[2] = xz - wy;
00405 mV[6] = yz + wx;
00406 mV[10] = T( 1 ) - ( xx + yy );
00407 mV[14] = 0;
00408
00409 mV[3] = 0;
00410 mV[7] = 0;
00411 mV[11] = 0;
00412 mV[15] = T( 1 );
00413
00414 return mV;
00415 }
00416
00417 operator Matrix44<T>() const { return toMatrix44(); }
00418
00419 Quaternion<T> lerp( T t, const Quaternion<T> &end ) const
00420 {
00421
00422 float cosTheta = dot( end );
00423
00424
00425 Quaternion<T> result = t * end;
00426
00427
00428 if( cosTheta >= EPSILON ) {
00429
00430 result += *this * ( static_cast<T>( 1.0 ) - t );
00431 }
00432 else {
00433
00434 result += *this * ( t - static_cast<T>( 1.0 ) );
00435 }
00436
00437 return result;
00438 }
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448 Quaternion<T> slerpShortestUnenforced( T t, const Quaternion<T> &end ) const
00449 {
00450 Quaternion<T> d = *this - end;
00451 T lengthD = math<T>::sqrt( this->dot( end ) );
00452
00453 Quaternion<T> st = *this + end;
00454 T lengthS = math<T>::sqrt( st.dot( st ) );
00455
00456 T a = 2 * math<T>::atan2( lengthD, lengthS );;
00457 T s = 1 - t;
00458
00459 Quaternion<T> q( *this * ( sinx_over_x( s * a ) / sinx_over_x( a ) * s ) +
00460 end * ( sinx_over_x( t * a ) / sinx_over_x( a ) * t ) );
00461
00462 return q.normalized();
00463 }
00464
00465 Quaternion<T> slerp( T t, const Quaternion<T> &end ) const
00466 {
00467
00468 T cosTheta = this->dot( end );
00469 T startInterp, endInterp;
00470
00471
00472 if( cosTheta >= EPSILON ) {
00473
00474 if( ( static_cast<T>( 1.0 ) - cosTheta ) > EPSILON ) {
00475
00476 T theta = math<T>::acos( cosTheta );
00477 T recipSinTheta = static_cast<T>( 1.0 ) / math<T>::sin( theta );
00478
00479 startInterp = math<T>::sin( ( static_cast<T>( 1.0 ) - t ) * theta ) * recipSinTheta;
00480 endInterp = math<T>::sin( t * theta ) * recipSinTheta;
00481 }
00482
00483 else {
00484
00485 startInterp = static_cast<T>( 1.0 ) - t;
00486 endInterp = t;
00487 }
00488 }
00489
00490 else {
00491
00492 if( ( static_cast<T>( 1.0 ) + cosTheta ) > EPSILON ) {
00493
00494 T theta = math<T>::acos( -cosTheta );
00495 T recipSinTheta = static_cast<T>( 1.0 ) / math<T>::sin( theta );
00496
00497 startInterp = math<T>::sin( ( t - static_cast<T>( 1.0 ) ) * theta ) * recipSinTheta;
00498 endInterp = math<T>::sin( t * theta ) * recipSinTheta;
00499 }
00500
00501 else {
00502
00503 startInterp = t - static_cast<T>( 1.0 );
00504 endInterp = t;
00505 }
00506 }
00507
00508 return *this * startInterp + end * endInterp;
00509 }
00510
00511
00512
00513
00514
00515
00516
00517 Quaternion<T> squadShortestEnforced( T t, const Quaternion<T> &qa, const Quaternion<T> &qb, const Quaternion<T> &q2 ) const
00518 {
00519 Quaternion<T> r1;
00520 if( this->dot( q2 ) >= 0 )
00521 r1 = this->slerpShortestUnenforced( t, q2 );
00522 else
00523 r1 = this->slerpShortestUnenforced( t, q2.inverted() );
00524
00525 Quaternion<T> r2;
00526 if( qa.dot( qb ) >= 0 )
00527 r2 = qa.slerpShortestUnenforced( t, qb );
00528 else
00529 r2 = qa.slerpShortestUnenforced( t, qb.inverted() );
00530
00531 if( r1.dot( r2 ) >= 0 )
00532 return r1.slerpShortestUnenforced( 2 * t * (1-t), r2 );
00533 else
00534 return r1.slerpShortestUnenforced( 2 * t * (1-t), r2.inverted() );
00535 }
00536
00537 Quaternion<T> squad( T t, const Quaternion<T> &qa, const Quaternion<T> &qb, const Quaternion<T> &q2 ) const
00538 {
00539 Quaternion<T> r1 = this->slerp( t, q2 );
00540 Quaternion<T> r2 = qa.slerp( t, qb );
00541 return r1.slerp( 2 * t * (1-t), r2 );
00542 }
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563 Quaternion<T> spline( T t, const Quaternion<T> &q1,
00564 const Quaternion<T> &q2, const Quaternion<T> &q3 ) const
00565 {
00566 Quaternion<T> qa = splineIntermediate( *this, q1, q2 );
00567 Quaternion<T> qb = splineIntermediate( q1, q2, q3 );
00568 Quaternion<T> result = q1.squadShortestEnforced( t, qa, qb, q2 );
00569
00570 return result;
00571 }
00572
00573 void set( const Matrix33<T> &m )
00574 {
00575
00576 T trace = m.trace();
00577 if ( trace > (T)0.0 )
00578 {
00579 T s = math<T>::sqrt( trace + (T)1.0 );
00580 w = s * (T)0.5;
00581 T recip = (T)0.5 / s;
00582 v.x = ( m.at(2,1) - m.at(1,2) ) * recip;
00583 v.y = ( m.at(0,2) - m.at(2,0) ) * recip;
00584 v.z = ( m.at(1,0) - m.at(0,1) ) * recip;
00585 }
00586 else
00587 {
00588 unsigned int i = 0;
00589 if( m.at(1,1) > m.at(0,0) )
00590 i = 1;
00591 if( m.at(2,2) > m.at(i,i) )
00592 i = 2;
00593 unsigned int j = ( i + 1 ) % 3;
00594 unsigned int k = ( j + 1 ) % 3;
00595 T s = math<T>::sqrt( m.at(i,i) - m.at(j,j) - m.at(k,k) + (T)1.0 );
00596 (*this)[i] = (T)0.5 * s;
00597 T recip = (T)0.5 / s;
00598 w = ( m.at(k,j) - m.at(j,k) ) * recip;
00599 (*this)[j] = ( m.at(j,i) + m.at(i,j) ) * recip;
00600 (*this)[k] = ( m.at(k,i) + m.at(i,k) ) * recip;
00601 }
00602 }
00603
00604 void set( const Matrix44<T> &m )
00605 {
00606
00607 T trace = m.trace();
00608 if ( trace > (T)0.0 )
00609 {
00610 T s = math<T>::sqrt( trace + (T)1.0 );
00611 w = s * (T)0.5;
00612 T recip = (T)0.5 / s;
00613 v.x = ( m.at(2,1) - m.at(1,2) ) * recip;
00614 v.y = ( m.at(0,2) - m.at(2,0) ) * recip;
00615 v.z = ( m.at(1,0) - m.at(0,1) ) * recip;
00616 }
00617 else
00618 {
00619 unsigned int i = 0;
00620 if( m.at(1,1) > m.at(0,0) )
00621 i = 1;
00622 if( m.at(2,2) > m.at(i,i) )
00623 i = 2;
00624 unsigned int j = ( i + 1 ) % 3;
00625 unsigned int k = ( j + 1 ) % 3;
00626 T s = math<T>::sqrt( m.at(i,i) - m.at(j,j) - m.at(k,k) + (T)1.0 );
00627 (*this)[i] = (T)0.5 * s;
00628 T recip = (T)0.5 / s;
00629 w = ( m.at(k,j) - m.at(j,k) ) * recip;
00630 (*this)[j] = ( m.at(j,i) + m.at(i,j) ) * recip;
00631 (*this)[k] = ( m.at(k,i) + m.at(i,k) ) * recip;
00632 }
00633 }
00634
00635
00636 Quaternion<T>& operator=( const Quaternion<T> &rhs )
00637 {
00638 v = rhs.v;
00639 w = rhs.w;
00640 return *this;
00641 }
00642
00643 template<typename FromT>
00644 Quaternion<T>& operator=( const Quaternion<FromT> &rhs )
00645 {
00646 v = rhs.v;
00647 w = static_cast<T>( rhs.w );
00648 return *this;
00649 }
00650
00651 const Quaternion<T> operator+( const Quaternion<T> &rhs ) const
00652 {
00653 const Quaternion<T>& lhs = *this;
00654 return Quaternion<T>( lhs.w + rhs.w, lhs.v.x + rhs.v.x, lhs.v.y + rhs.v.y, lhs.v.z + rhs.v.z );
00655 }
00656
00657
00658
00659 const Quaternion<T> operator*( const Quaternion<T> &rhs ) const
00660 {
00661 return Quaternion<T>( rhs.w*w - rhs.v.x*v.x - rhs.v.y*v.y - rhs.v.z*v.z,
00662 rhs.w*v.x + rhs.v.x*w + rhs.v.y*v.z - rhs.v.z*v.y,
00663 rhs.w*v.y + rhs.v.y*w + rhs.v.z*v.x - rhs.v.x*v.z,
00664 rhs.w*v.z + rhs.v.z*w + rhs.v.x*v.y - rhs.v.y*v.x );
00665 }
00666
00667 const Quaternion<T> operator*( T rhs ) const
00668 {
00669 return Quaternion<T>( w * rhs, v.x * rhs, v.y * rhs, v.z * rhs );
00670 }
00671
00672
00673 const Vec3<T> operator*( const Vec3<T> &vec )
00674 {
00675 T vMult = T( 2 ) * ( v.x * vec.x + v.y * vec.y + v.z * vec.z );
00676 T crossMult = T( 2 ) * w;
00677 T pMult = crossMult * w - T( 1 );
00678
00679 return Vec3<T>( pMult * vec.x + vMult * v.x + crossMult * ( v.y * vec.z - v.z * vec.y ),
00680 pMult * vec.y + vMult * v.y + crossMult * ( v.z * vec.x - v.x * vec.z ),
00681 pMult * vec.z + vMult * v.z + crossMult * ( v.x * vec.y - v.y * vec.x ) );
00682 }
00683
00684 const Quaternion<T> operator-( const Quaternion<T> &rhs ) const
00685 {
00686 const Quaternion<T>& lhs = *this;
00687 return Quaternion<T>( lhs.w - rhs.w, lhs.v.x - rhs.v.x, lhs.v.y - rhs.v.y, lhs.v.z - rhs.v.z );
00688 }
00689
00690 Quaternion<T>& operator+=( const Quaternion<T> &rhs )
00691 {
00692 w += rhs.w;
00693 v += rhs.v;
00694 return *this;
00695 }
00696
00697 Quaternion<T>& operator-=( const Quaternion<T>& rhs )
00698 {
00699 w -= rhs.w;
00700 v -= rhs.v;
00701 return *this;
00702 }
00703
00704 Quaternion<T>& operator*=( const Quaternion<T> &rhs )
00705 {
00706 Quaternion q = (*this) * rhs;
00707 v = q.v;
00708 w = q.w;
00709 return *this;
00710 }
00711
00712 Quaternion<T>& operator*=( T rhs )
00713 {
00714 w *= rhs;
00715 v *= rhs;
00716 return *this;
00717 }
00718
00719 bool operator==( const Quaternion<T> &rhs ) const
00720 {
00721 const Quaternion<T>& lhs = *this;
00722 return ( std::fabs(lhs.w - rhs.w) < EPSILON ) && lhs.v == rhs.v;
00723 }
00724
00725 bool operator!=( const Quaternion<T> &rhs ) const
00726 {
00727 return ! (*this == rhs);
00728 }
00729
00730 inline T& operator[]( unsigned int i ) { return (&v.x)[i]; }
00731 inline const T& operator[]( unsigned int i ) const { return (&v.x)[i]; }
00732
00733 static Quaternion<T> identity()
00734 {
00735 return Quaternion();
00736 }
00737
00738
00739 friend std::ostream& operator <<( std::ostream &oss, const Quaternion<T> &q )
00740 {
00741 oss << q.getAxis() << " @ " << q.getAngle() * ( (T)180 / M_PI ) << "deg";
00742 return oss;
00743 }
00744
00745 private:
00746
00747
00748
00749
00750
00751 static Quaternion<T> splineIntermediate( const Quaternion<T> &q0, const Quaternion<T> &q1, const Quaternion<T> &q2 )
00752 {
00753 Quaternion<T> q1inv = q1.inverted();
00754 Quaternion<T> c1 = q1inv * q2;
00755 Quaternion<T> c2 = q1inv * q0;
00756 Quaternion<T> c3 = ( c2.log() + c1.log() ) * (T)-0.25;
00757 Quaternion<T> qa = q1 * c3.exp();
00758 return qa.normalized();
00759 }
00760 };
00761
00762 template<typename T>
00763 inline Vec3<T> operator*( const Vec3<T> &vec, const Quaternion<T> &q )
00764 {
00765 T vMult = T( 2 ) * ( q.v.x * vec.x + q.v.y * vec.y + q.v.z * vec.z );
00766 T crossMult = T( 2 ) * q.w;
00767 T pMult = crossMult * q.w - T( 1 );
00768
00769 return Vec3<T>( pMult * vec.x + vMult * q.v.x + crossMult * ( q.v.y * vec.z - q.v.z * vec.y ),
00770 pMult * vec.y + vMult * q.v.y + crossMult * ( q.v.z * vec.x - q.v.x * vec.z ),
00771 pMult * vec.z + vMult * q.v.z + crossMult * ( q.v.x * vec.y - q.v.y * vec.x ) );
00772 }
00773
00774 typedef Quaternion<float> Quatf;
00775 typedef Quaternion<double> Quatd;
00776
00777 }