Cinder

  • Main Page
  • Related Pages
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

include/cinder/Quaternion.h

Go to the documentation of this file.
00001 /*
00002  Copyright (c) 2010, The Barbarian Group
00003  All rights reserved.
00004 
00005  Redistribution and use in source and binary forms, with or without modification, are permitted provided that
00006  the following conditions are met:
00007 
00008     * Redistributions of source code must retain the above copyright notice, this list of conditions and
00009     the following disclaimer.
00010     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
00011     the following disclaimer in the documentation and/or other materials provided with the distribution.
00012 
00013  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
00014  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
00015  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
00016  ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
00017  TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
00018  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00019  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00020  POSSIBILITY OF SUCH DAMAGE.
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  
00046     Vec3<T>     v; // axisOfRotation.normalized() * sin( angleOfRotation / 2 )
00047     T           w; // cos( angleOfRotation / 2 )
00048 
00049     Quaternion(): w( 1 ), v( 0, 0, 0 ){} // default constructor is identity quat
00050     template<typename FromT>
00051     Quaternion( const Quaternion<FromT>& q ) : w( static_cast<T>( q.w ) ), v( q.v ) {}
00052 
00053     Quaternion( T aW, T x, T y, T z ): w( aW ), v( x, y, z ) {} 
00054     // construct from axis-angle
00055     Quaternion( const Vec3<T> &axis, T radians ) { set( axis, radians ); } 
00056     Quaternion( const Vec3<T> &from, const Vec3<T> &to ) { set( from, to ); }
00057     // create from euler angles in radians expressed in ZYX rotation order
00058     Quaternion( T xRotation, T yRotation, T zRotation ) { set( xRotation, yRotation, zRotation ); }
00059     Quaternion( const Matrix44<T> &m ) { set( m ); }
00060     template<typename Y>
00061     explicit Quaternion( const Y &v )
00062         : 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 ) )
00063     {}
00064     
00065     // get axis-angle representation's axis
00066     Vec3<T> getAxis() const
00067     {
00068         T cos_angle = w;
00069         T invLen = static_cast<T>( 1.0 ) / math<T>::sqrt( static_cast<T>( 1.0 ) - cos_angle * cos_angle );
00070 
00071         return v * invLen;
00072     }
00073     
00074     // get axis-angle representation's angle in radians
00075     T getAngle() const
00076     {
00077         T cos_angle = w;
00078         return math<T>::acos( cos_angle ) * 2;
00079     }
00080 
00081     T getPitch() const
00082     {
00083         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 );
00084     }
00085 
00086     T getYaw() const
00087     {
00088         return math<T>::sin( (T)-2 * ( v.x * v.z - w * v.y ) );
00089     }
00090 
00091     T getRoll() const
00092     {
00093         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 );
00094     }
00095     
00096     T dot( const Quaternion<T> &quat ) const
00097     {
00098         return w * quat.w + v.dot( quat.v );
00099     }
00100     
00101     T length() const
00102     {
00103         return (T)std::sqrt( w*w + v.lengthSquared() ); 
00104     }
00105 
00106     T lengthSquared() const
00107     {
00108         return w * w + v.lengthSquared();   
00109     }
00110 
00111     Quaternion<T> inverse() const
00112     {
00113         T norm = w * w + v.x * v.x + v.y * v.y + v.z * v.z;
00114         // if we're the zero quaternion, just return identity
00115         /*if( ! ( math<T>::abs( norm ) < EPSILON_VALUE ) ) {
00116             return identity();
00117         }*/
00118 
00119         T normRecip = static_cast<T>( 1.0f ) / norm;
00120         return Quaternion<T>( normRecip * w, -normRecip * v.x, -normRecip * v.y, -normRecip * v.z );
00121     }
00122 
00123     void normalize()
00124     {
00125         if( T len = length() ) {
00126             w /= len;
00127             v /= len;
00128         }
00129         else {
00130             w = static_cast<T>( 1.0 );
00131             v = Vec3<T>( 0, 0, 0 );
00132         }
00133     }
00134 
00135     Quaternion<T> normalized() const
00136     {
00137         Quaternion<T> result = *this;
00138     
00139         if( T len = length() ) {
00140             result.w /= len;
00141             result.v /= len;
00142         }
00143         else {
00144             result.w = static_cast<T>( 1.0 );
00145             result.v = Vec3<T>( 0, 0, 0 );
00146         }
00147         
00148         return result;
00149     }
00150 
00151     // For unit quaternion, from Advanced Animation and 
00152     // Rendering Techniques by Watt and Watt, Page 366:
00153     Quaternion<T> log() const
00154     {
00155         T theta = math<T>::acos( std::min( w, (T) 1.0 ) );
00156 
00157         if( theta == 0 )
00158             return Quaternion<T>( v, 0 );
00159         
00160         T sintheta = math<T>::sin( theta );
00161         
00162         T k;
00163         if ( abs( sintheta ) < 1 && abs( theta ) >= 3.402823466e+38F * abs( sintheta ) )
00164             k = 1;
00165         else
00166             k = theta / sintheta;
00167 
00168         return Quaternion<T>( (T)0, v.x * k, v.y * k, v.z * k );
00169     }
00170 
00171     // For pure quaternion (zero scalar part):
00172     // from Advanced Animation and Rendering
00173     // Techniques by Watt and Watt, Page 366:
00174     Quaternion<T> exp() const
00175     {
00176         T theta = v.length();
00177         T sintheta = math<T>::sin( theta );
00178         
00179         T k;
00180         if( abs( theta ) < 1 && abs( sintheta ) >= 3.402823466e+38F * abs( theta ) )
00181             k = 1;
00182         else
00183             k = sintheta / theta;
00184 
00185         T costheta = math<T>::cos( theta );
00186 
00187         return Quaternion<T>( costheta, v.x * k, v.y * k, v.z * k );
00188     }
00189 
00190     Quaternion<T> inverted() const
00191     {
00192         T qdot = this->dot( *this );
00193         return Quaternion( -v / qdot, w / qdot );
00194     }
00195 
00196     void invert()
00197     {
00198         T qdot = this->dot( *this );
00199         set( -v / qdot, w / qdot );     
00200     }
00201 
00202     void set( T aW, T x, T y, T z )
00203     {
00204         w = aW;
00205         v = Vec3<T>( x, y, z );
00206     }
00207 
00208 #if 1
00209     void set( const Vec3<T> &from, const Vec3<T> &to )
00210     {
00211         Vec3<T> axis = from.cross( to );
00212         
00213         set( from.dot( to ), axis.x, axis.y, axis.z );
00214         normalize();
00215 
00216         w += static_cast<T>( 1.0 );
00217 
00218         if( w <= EPSILON ) {
00219             if ( from.z * from.z > from.x * from.x ) {
00220                 set( static_cast<T>( 0.0 ), static_cast<T>( 0.0 ), from.z, -from.y );
00221             }
00222             else {
00223                 set( static_cast<T>( 0.0 ), from.y, -from.x, static_cast<T>( 0.0 ) );
00224             }
00225         }
00226         
00227         normalize();
00228     }
00229 #else
00230     void set( const Vec3<T> &from, const Vec3<T> &to )
00231     {
00232         Vec3<T> f0 = from.normalized();
00233         Vec3<T> t0 = to.normalized();
00234 
00235         if( f0.dot( t0 ) >= 0 ) { // The rotation angle is less than or equal to pi/2.
00236             setRotationInternal (f0, t0, *this);
00237         }
00238         else {
00239             // The angle is greater than pi/2.  After computing h0,
00240             // which is halfway between f0 and t0, we rotate first
00241             // from f0 to h0, then from h0 to t0.
00242 
00243             Vec3<T> h0 = (f0 + t0).normalized();
00244 
00245             if( h0.dot( h0 ) != 0 ) {
00246                 setRotationInternal( f0, h0, *this );
00247                 Quaternion<T> q;
00248                 setRotationInternal( h0, t0, q );
00249                 
00250                 set( q.w*w - q.v.x*v.x - q.v.y*v.y - q.v.z*v.z,
00251                     q.w*v.x + q.v.x*w + q.v.y*v.z - q.v.z*v.y,
00252                     q.w*v.y + q.v.y*w + q.v.z*v.x - q.v.x*v.z,
00253                     q.w*v.z + q.v.z*w + q.v.x*v.y - q.v.y*v.x );
00254                 
00255                 //*this *= q;
00256             }
00257             else {
00258                 // f0 and t0 point in exactly opposite directions.
00259                 // Pick an arbitrary axis that is orthogonal to f0,
00260                 // and rotate by pi.
00261 
00262                 w = T( 0 );
00263 
00264                 Vec3<T> f02 = f0 * f0;
00265 
00266                 if( ( f02.x <= f02.y ) && ( f02.x <= f02.z ) )
00267                     v = f0.cross( Vec3<T>( 1, 0, 0 ) ).normalized();
00268                 else if( f02.y <= f02.z )
00269                     v = f0.cross( Vec3<T>( 0, 1, 0 ) ).normalized();
00270                 else
00271                     v = f0.cross( Vec3<T>( 0, 0, 1 ) ).normalized();
00272             }
00273         }
00274     }
00275 
00276     static void setRotationInternal( const Vec3<T> &f0, const Vec3<T> &t0, Quaternion<T> &q )
00277     {
00278         //
00279         // The following is equivalent to setAxisAngle(n,2*phi),
00280         // where the rotation axis, is orthogonal to the f0 and
00281         // t0 vectors, and 2*phi is the angle between f0 and t0.
00282         //
00283         // This function is called by setRotation(), above; it assumes
00284         // that f0 and t0 are normalized and that the angle between
00285         // them is not much greater than pi/2.  This function becomes
00286         // numerically inaccurate if f0 and t0 point into nearly
00287         // opposite directions.
00288         //
00289 
00290         // Find a normalized vector, h0, that is half way between f0 and t0.
00291         // The angle between f0 and h0 is phi.
00292         Vec3<T> h0 = ( f0 + t0 ).normalized();
00293 
00294         // Store the rotation axis and rotation angle.
00295         q.w = f0.dot( h0 ); //  f0 ^ h0 == cos (phi)
00296         q.v = f0.cross( h0 );   // (f0 % h0).length() == sin (phi)
00297     }
00298 #endif
00299 
00300     void set( const Vec3<T> &axis, T radians )
00301     {
00302         w = math<T>::cos( radians / 2 );
00303         v = axis.normalized() * math<T>::sin( radians / 2 );
00304     }
00305 
00306     // assumes ZYX rotation order and radians
00307     void set( T xRotation, T yRotation, T zRotation )
00308     {
00309         zRotation *= T( 0.5 );
00310         yRotation *= T( 0.5 );
00311         xRotation *= T( 0.5 );
00312 
00313         // get sines and cosines of half angles
00314         T Cx = math<T>::cos( xRotation );
00315         T Sx = math<T>::sin( xRotation );
00316 
00317         T Cy = math<T>::cos( yRotation );
00318         T Sy = math<T>::sin( yRotation );
00319 
00320         T Cz = math<T>::cos( zRotation );
00321         T Sz = math<T>::sin( zRotation );
00322 
00323         // multiply it out
00324         w = Cx*Cy*Cz - Sx*Sy*Sz;
00325         v.x = Sx*Cy*Cz + Cx*Sy*Sz;
00326         v.y = Cx*Sy*Cz - Sx*Cy*Sz;
00327         v.z = Cx*Cy*Sz + Sx*Sy*Cx;
00328     }
00329 
00330     void getAxisAngle( Vec3<T> *axis, T *radians ) const
00331     {
00332         T cos_angle = w;
00333         *radians = math<T>::acos( cos_angle ) * 2;
00334         T invLen = static_cast<T>( 1.0 ) / math<T>::sqrt( static_cast<T>( 1.0 ) - cos_angle * cos_angle );
00335 
00336         axis->x = v.x * invLen;
00337         axis->y = v.y * invLen;
00338         axis->z = v.z * invLen;
00339     }
00340 
00341     Matrix44<T> toMatrix44() const
00342     {
00343         Matrix44<T> mV;
00344         T xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
00345 
00346         xs = v.x + v.x;   
00347         ys = v.y + v.y;
00348         zs = v.z + v.z;
00349         wx = w * xs;
00350         wy = w * ys;
00351         wz = w * zs;
00352         xx = v.x * xs;
00353         xy = v.x * ys;
00354         xz = v.x * zs;
00355         yy = v.y * ys;
00356         yz = v.y * zs;
00357         zz = v.z * zs;
00358 
00359         mV[0] = T( 1 ) - ( yy + zz );
00360         mV[4] = xy - wz;
00361         mV[8] = xz + wy;
00362         mV[12] = 0;
00363         
00364         mV[1] = xy + wz;
00365         mV[5] = T( 1 ) - ( xx + zz );
00366         mV[9] = yz - wx;
00367         mV[13] = 0;
00368         
00369         mV[2] = xz - wy;
00370         mV[6] = yz + wx;
00371         mV[10] = T( 1 ) - ( xx + yy );
00372         mV[14] = 0;
00373 
00374         mV[3] = 0;
00375         mV[7] = 0;
00376         mV[11] = 0;
00377         mV[15] = T( 1 );
00378         
00379         return mV;
00380     }
00381 
00382     operator Matrix44<T>() const { return toMatrix44(); }
00383 
00384     Quaternion<T> lerp( T t, const Quaternion<T> &end ) const
00385     {
00386         // get cos of "angle" between quaternions
00387         float cosTheta = dot( end );
00388 
00389         // initialize result
00390         Quaternion<T> result = t * end;
00391 
00392         // if "angle" between quaternions is less than 90 degrees
00393         if( cosTheta >= EPSILON ) {
00394             // use standard interpolation
00395             result += *this * ( static_cast<T>( 1.0 ) - t );
00396         }
00397         else {
00398             // otherwise, take the shorter path
00399             result += *this * ( t - static_cast<T>( 1.0 ) );
00400         }
00401         
00402         return result;
00403     }
00404 
00405     // This method does *not* interpolate along the shortest
00406     // arc between q1 and q2.  If you desire interpolation
00407     // along the shortest arc, and q1.dot( q2 ) is negative, then
00408     // consider flipping the second quaternion explicitly.
00409     //
00410     // NOTE: the difference between this and slerp isn't clear, but we're using
00411     // the Don Hatch / ilmbase squad code which explicity requires this impl. of slerp
00412     // so I'm leaving it for now
00413     Quaternion<T> slerpShortestUnenforced( T t, const Quaternion<T> &end ) const
00414     {
00415         Quaternion<T> d = *this - end;
00416         T lengthD = math<T>::sqrt( this->dot( end ) );
00417 
00418         Quaternion<T> st = *this + end;
00419         T lengthS = math<T>::sqrt( st.dot( st ) );
00420     
00421         T a = 2 * math<T>::atan2( lengthD, lengthS );;
00422         T s = 1 - t;
00423 
00424         Quaternion<T> q( *this * ( sinx_over_x( s * a ) / sinx_over_x( a ) * s ) +
00425                 end * ( sinx_over_x( t * a ) / sinx_over_x( a ) * t ) );
00426 
00427         return q.normalized();
00428     }
00429 
00430     Quaternion<T> slerp( T t, const Quaternion<T> &end ) const
00431     {
00432         // get cosine of "angle" between quaternions
00433         T cosTheta = this->dot( end );
00434         T startInterp, endInterp;
00435 
00436         // if "angle" between quaternions is less than 90 degrees
00437         if( cosTheta >= EPSILON ) {
00438             // if angle is greater than zero
00439             if( ( static_cast<T>( 1.0 ) - cosTheta ) > EPSILON ) {
00440                 // use standard slerp
00441                 T theta = math<T>::acos( cosTheta );
00442                 T recipSinTheta = static_cast<T>( 1.0 ) / math<T>::sin( theta );
00443 
00444                 startInterp = math<T>::sin( ( static_cast<T>( 1.0 ) - t ) * theta ) * recipSinTheta;
00445                 endInterp = math<T>::sin( t * theta ) * recipSinTheta;
00446             }
00447             // angle is close to zero
00448             else {
00449                 // use linear interpolation
00450                 startInterp = static_cast<T>( 1.0 ) - t;
00451                 endInterp = t;
00452             }
00453         }
00454         // otherwise, take the shorter route
00455         else {
00456             // if angle is less than 180 degrees
00457             if( ( static_cast<T>( 1.0 ) + cosTheta ) > EPSILON ) {
00458                 // use slerp w/negation of start quaternion
00459                 T theta = math<T>::acos( -cosTheta );
00460                 T recipSinTheta = static_cast<T>( 1.0 ) / math<T>::sin( theta );
00461 
00462                 startInterp = math<T>::sin( ( t - static_cast<T>( 1.0 ) ) * theta ) * recipSinTheta;
00463                 endInterp = math<T>::sin( t * theta ) * recipSinTheta;
00464             }
00465             // angle is close to 180 degrees
00466             else {
00467                 // use lerp w/negation of start quaternion
00468                 startInterp = t - static_cast<T>( 1.0 );
00469                 endInterp = t;
00470             }
00471         }
00472         
00473         return *this * startInterp + end * endInterp;
00474     }
00475 
00476     // Spherical Quadrangle Interpolation -
00477     // from Advanced Animation and Rendering
00478     // Techniques by Watt and Watt, Page 366:
00479     // It constructs a spherical cubic interpolation as 
00480     // a series of three spherical linear interpolations 
00481     // of a quadrangle of unit quaternions. 
00482     Quaternion<T> squadShortestEnforced( T t, const Quaternion<T> &qa, const Quaternion<T> &qb, const Quaternion<T> &q2 ) const
00483     {
00484         Quaternion<T> r1;
00485         if( this->dot( q2 ) >= 0 )
00486             r1 = this->slerpShortestUnenforced( t, q2 );
00487         else
00488             r1 = this->slerpShortestUnenforced( t, q2.inverted() );
00489         
00490         Quaternion<T> r2;
00491         if( qa.dot( qb ) >= 0 )
00492             r2 = qa.slerpShortestUnenforced( t, qb );
00493         else
00494             r2 = qa.slerpShortestUnenforced( t, qb.inverted() );
00495         
00496         if( r1.dot( r2 ) >= 0 )
00497             return r1.slerpShortestUnenforced( 2 * t * (1-t), r2 );
00498         else
00499             return r1.slerpShortestUnenforced( 2 * t * (1-t), r2.inverted() );
00500     }
00501 
00502     Quaternion<T> squad( T t, const Quaternion<T> &qa, const Quaternion<T> &qb, const Quaternion<T> &q2 ) const
00503     {
00504         Quaternion<T> r1 = this->slerp( t, q2 );    
00505         Quaternion<T> r2 = qa.slerp( t, qb );
00506         return r1.slerp( 2 * t * (1-t), r2 );
00507     }
00508 
00509     // Spherical Cubic Spline Interpolation -
00510     // from Advanced Animation and Rendering
00511     // Techniques by Watt and Watt, Page 366:
00512     // A spherical curve is constructed using three
00513     // spherical linear interpolations of a quadrangle
00514     // of unit quaternions: q1, qa, qb, q2.
00515     // Given a set of quaternion keys: q0, q1, q2, q3,
00516     // this routine does the interpolation between
00517     // q1 and q2 by constructing two intermediate
00518     // quaternions: qa and qb. The qa and qb are 
00519     // computed by the intermediate function to 
00520     // guarantee the continuity of tangents across
00521     // adjacent cubic segments. The qa represents in-tangent
00522     // for q1 and the qb represents the out-tangent for q2.
00523     // 
00524     // The q1 q2 is the cubic segment being interpolated. 
00525     // The q0 is from the previous adjacent segment and q3 is 
00526     // from the next adjacent segment. The q0 and q3 are used
00527     // in computing qa and qb.
00528     Quaternion<T> spline( T t, const Quaternion<T> &q1,
00529                const Quaternion<T> &q2, const Quaternion<T> &q3 ) const
00530     {
00531         Quaternion<T> qa = splineIntermediate( *this, q1, q2 );
00532         Quaternion<T> qb = splineIntermediate( q1, q2, q3 );
00533         Quaternion<T> result = q1.squadShortestEnforced( t, qa, qb, q2 );
00534 
00535         return result;
00536     }
00537 
00538     void set( const Matrix44<T> &m )
00539     {
00540         T trace =  m.m[0] + m.m[5] + m.m[10];
00541         if ( trace > (T)0.0 )
00542         {
00543             T s = math<T>::sqrt( trace + (T)1.0 );
00544             w = s * (T)0.5;
00545             T recip = (T)0.5 / s;
00546             v.x = ( m.at(2,1) - m.at(1,2) ) * recip;
00547             v.y = ( m.at(0,2) - m.at(2,0) ) * recip;
00548             v.z = ( m.at(1,0) - m.at(0,1) ) * recip;
00549         }
00550         else
00551         {
00552             unsigned int i = 0;
00553             if( m.at(1,1) > m.at(0,0) )
00554                 i = 1;
00555             if( m.at(2,2) > m.at(i,i) )
00556                 i = 2;
00557             unsigned int j = ( i + 1 ) % 3;
00558             unsigned int k = ( j + 1 ) % 3;
00559             T s = math<T>::sqrt( m.at(i,i) - m.at(j,j) - m.at(k,k) + (T)1.0 );
00560             (*this)[i] = (T)0.5 * s;
00561             T recip = (T)0.5 / s;
00562             w = ( m.at(k,j) - m.at(j,k) ) * recip;
00563             (*this)[j] = ( m.at(j,i) + m.at(i,j) ) * recip;
00564             (*this)[k] = ( m.at(k,i) + m.at(i,k) ) * recip;
00565         }
00566     }
00567 
00568     // Operators
00569     Quaternion<T>& operator=( const Quaternion<T> &rhs )
00570     {
00571         v = rhs.v;
00572         w = rhs.w;
00573         return *this;
00574     }
00575 
00576     template<typename FromT>
00577     Quaternion<T>& operator=( const Quaternion<FromT> &rhs )
00578     {
00579         v = rhs.v;
00580         w = static_cast<T>( rhs.w );
00581         return *this;
00582     }
00583 
00584     const Quaternion<T> operator+( const Quaternion<T> &rhs ) const
00585     {
00586         const Quaternion<T>& lhs = *this;
00587         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 );  
00588     }
00589 
00590     // post-multiply operator, similar to matrices, but different from Shoemake
00591     // Concatenates 'rhs' onto 'this'
00592     const Quaternion<T> operator*( const Quaternion<T> &rhs ) const
00593     {
00594         return Quaternion<T>( rhs.w*w - rhs.v.x*v.x - rhs.v.y*v.y - rhs.v.z*v.z,
00595                    rhs.w*v.x + rhs.v.x*w + rhs.v.y*v.z - rhs.v.z*v.y,
00596                    rhs.w*v.y + rhs.v.y*w + rhs.v.z*v.x - rhs.v.x*v.z,
00597                    rhs.w*v.z + rhs.v.z*w + rhs.v.x*v.y - rhs.v.y*v.x );
00598     }
00599 
00600     const Quaternion<T> operator*( T rhs ) const 
00601     { 
00602         return Quaternion<T>( w * rhs, v.x * rhs, v.y * rhs, v.z * rhs );
00603     }
00604 
00605     // transform a vector by the quaternion
00606     const Vec3<T> operator*( const Vec3<T> &vec )
00607     {
00608         T vMult = T( 2 ) * ( v.x * vec.x + v.y * vec.y + v.z * vec.z );
00609         T crossMult = T( 2 ) * w;
00610         T pMult = crossMult * w - T( 1 );
00611 
00612         return Vec3<T>( pMult * vec.x + vMult * v.x + crossMult * ( v.y * vec.z - v.z * vec.y ),
00613                         pMult * vec.y + vMult * v.y + crossMult * ( v.z * vec.x - v.x * vec.z ),
00614                         pMult * vec.z + vMult * v.z + crossMult * ( v.x * vec.y - v.y * vec.x ) );
00615     }
00616 
00617     const Quaternion<T> operator-( const Quaternion<T> &rhs ) const
00618     {
00619         const Quaternion<T>& lhs = *this;
00620         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 );
00621     }
00622 
00623     Quaternion<T>& operator+=( const Quaternion<T> &rhs )
00624     {
00625         w += rhs.w;
00626         v += rhs.v;
00627         return *this;
00628     }
00629 
00630     Quaternion<T>& operator-=( const Quaternion<T>& rhs )
00631     {
00632         w -= rhs.w;
00633         v -= rhs.v;
00634         return *this;
00635     }
00636 
00637     Quaternion<T>& operator*=( const Quaternion<T> &rhs )
00638     {
00639         Quaternion q = (*this) * rhs;
00640         v = q.v;
00641         w = q.w;
00642         return *this;
00643     }
00644 
00645     Quaternion<T>& operator*=( T rhs )
00646     {
00647         w *= rhs;
00648         v *= rhs;
00649         return *this;
00650     }
00651 
00652     bool operator==( const Quaternion<T> &rhs ) const
00653     {
00654         const Quaternion<T>& lhs = *this;
00655         return ( std::fabs(lhs.w - rhs.w) < EPSILON ) && lhs.v == rhs.v;
00656     }
00657 
00658     bool operator!=( const Quaternion<T> &rhs ) const
00659     {
00660         return ! (*this == rhs);
00661     }
00662 
00663     inline T& operator[]( unsigned int i ) { return (&v.x)[i]; }
00664     inline const T& operator[]( unsigned int i ) const { return (&v.x)[i]; }
00665 
00666     static Quaternion<T> identity()
00667     {
00668         return Quaternion();
00669     }
00670 
00671     // Output
00672     friend std::ostream& operator <<( std::ostream &oss, const Quaternion<T> &q )
00673     {
00674         oss << q.getAxis() << " @ " << q.getAngle() * ( (T)180 / M_PI ) << "deg";
00675         return oss;
00676     }
00677 
00678  private:
00679     // From advanced Animation and Rendering
00680     // Techniques by Watt and Watt, Page 366:
00681     // computing the inner quadrangle 
00682     // points (qa and qb) to guarantee tangent
00683     // continuity.
00684     static Quaternion<T> splineIntermediate( const Quaternion<T> &q0, const Quaternion<T> &q1, const Quaternion<T> &q2 )
00685     {
00686         Quaternion<T> q1inv = q1.inverted();
00687         Quaternion<T> c1 = q1inv * q2;
00688         Quaternion<T> c2 = q1inv * q0;
00689         Quaternion<T> c3 = ( c2.log() + c1.log() ) * (T)-0.25;
00690         Quaternion<T> qa = q1 * c3.exp();
00691         return qa.normalized();
00692     }
00693 };
00694 
00695 template<typename T>
00696 inline Vec3<T> operator*( const Vec3<T> &vec, const Quaternion<T> &q )
00697 {
00698     T vMult = T( 2 ) * ( q.v.x * vec.x + q.v.y * vec.y + q.v.z * vec.z );
00699     T crossMult = T( 2 ) * q.w;
00700     T pMult = crossMult * q.w - T( 1 );
00701 
00702     return Vec3<T>( pMult * vec.x + vMult * q.v.x + crossMult * ( q.v.y * vec.z - q.v.z * vec.y ),
00703                     pMult * vec.y + vMult * q.v.y + crossMult * ( q.v.z * vec.x - q.v.x * vec.z ),
00704                     pMult * vec.z + vMult * q.v.z + crossMult * ( q.v.x * vec.y - q.v.y * vec.x ) );
00705 }
00706 
00707 typedef Quaternion<float>   Quatf;
00708 typedef Quaternion<double>  Quatd;
00709 
00710 } // namespace cinder