Cinder

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