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     typedef T   value_type;
00046  
00047     Vec3<T>     v; // axisOfRotation.normalized() * sin( angleOfRotation / 2 )
00048     T           w; // cos( angleOfRotation / 2 )
00049 
00050     Quaternion(): w( 1 ), v( 0, 0, 0 ){} // default constructor is identity quat
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     // construct from axis-angle
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     // create from Euler angles in radians expressed in ZYX rotation order
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     // get axis-angle representation's axis
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     // get axis-angle representation's angle in radians
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         // if we're the zero quaternion, just return identity
00117         /*if( ! ( math<T>::abs( norm ) < EPSILON_VALUE ) ) {
00118             return identity();
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     // For unit quaternion, from Advanced Animation and 
00154     // Rendering Techniques by Watt and Watt, Page 366:
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     // For pure quaternion (zero scalar part):
00174     // from Advanced Animation and Rendering
00175     // Techniques by Watt and Watt, Page 366:
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 ) { // The rotation angle is less than or equal to pi/2.
00238             setRotationInternal (f0, t0, *this);
00239         }
00240         else {
00241             // The angle is greater than pi/2.  After computing h0,
00242             // which is halfway between f0 and t0, we rotate first
00243             // from f0 to h0, then from h0 to t0.
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                 //*this *= q;
00258             }
00259             else {
00260                 // f0 and t0 point in exactly opposite directions.
00261                 // Pick an arbitrary axis that is orthogonal to f0,
00262                 // and rotate by pi.
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         // The following is equivalent to setAxisAngle(n,2*phi),
00282         // where the rotation axis, is orthogonal to the f0 and
00283         // t0 vectors, and 2*phi is the angle between f0 and t0.
00284         //
00285         // This function is called by setRotation(), above; it assumes
00286         // that f0 and t0 are normalized and that the angle between
00287         // them is not much greater than pi/2.  This function becomes
00288         // numerically inaccurate if f0 and t0 point into nearly
00289         // opposite directions.
00290         //
00291 
00292         // Find a normalized vector, h0, that is half way between f0 and t0.
00293         // The angle between f0 and h0 is phi.
00294         Vec3<T> h0 = ( f0 + t0 ).normalized();
00295 
00296         // Store the rotation axis and rotation angle.
00297         q.w = f0.dot( h0 ); //  f0 ^ h0 == cos (phi)
00298         q.v = f0.cross( h0 );   // (f0 % h0).length() == sin (phi)
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     // assumes ZYX rotation order and radians
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         // get sines and cosines of half angles
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         // multiply it out
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         // get cos of "angle" between quaternions
00422         float cosTheta = dot( end );
00423 
00424         // initialize result
00425         Quaternion<T> result = t * end;
00426 
00427         // if "angle" between quaternions is less than 90 degrees
00428         if( cosTheta >= EPSILON ) {
00429             // use standard interpolation
00430             result += *this * ( static_cast<T>( 1.0 ) - t );
00431         }
00432         else {
00433             // otherwise, take the shorter path
00434             result += *this * ( t - static_cast<T>( 1.0 ) );
00435         }
00436         
00437         return result;
00438     }
00439 
00440     // This method does *not* interpolate along the shortest
00441     // arc between q1 and q2.  If you desire interpolation
00442     // along the shortest arc, and q1.dot( q2 ) is negative, then
00443     // consider flipping the second quaternion explicitly.
00444     //
00445     // NOTE: the difference between this and slerp isn't clear, but we're using
00446     // the Don Hatch / ilmbase squad code which explicity requires this impl. of slerp
00447     // so I'm leaving it for now
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         // get cosine of "angle" between quaternions
00468         T cosTheta = this->dot( end );
00469         T startInterp, endInterp;
00470 
00471         // if "angle" between quaternions is less than 90 degrees
00472         if( cosTheta >= EPSILON ) {
00473             // if angle is greater than zero
00474             if( ( static_cast<T>( 1.0 ) - cosTheta ) > EPSILON ) {
00475                 // use standard slerp
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             // angle is close to zero
00483             else {
00484                 // use linear interpolation
00485                 startInterp = static_cast<T>( 1.0 ) - t;
00486                 endInterp = t;
00487             }
00488         }
00489         // otherwise, take the shorter route
00490         else {
00491             // if angle is less than 180 degrees
00492             if( ( static_cast<T>( 1.0 ) + cosTheta ) > EPSILON ) {
00493                 // use slerp w/negation of start quaternion
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             // angle is close to 180 degrees
00501             else {
00502                 // use lerp w/negation of start quaternion
00503                 startInterp = t - static_cast<T>( 1.0 );
00504                 endInterp = t;
00505             }
00506         }
00507         
00508         return *this * startInterp + end * endInterp;
00509     }
00510 
00511     // Spherical Quadrangle Interpolation -
00512     // from Advanced Animation and Rendering
00513     // Techniques by Watt and Watt, Page 366:
00514     // It constructs a spherical cubic interpolation as 
00515     // a series of three spherical linear interpolations 
00516     // of a quadrangle of unit quaternions. 
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     // Spherical Cubic Spline Interpolation -
00545     // from Advanced Animation and Rendering
00546     // Techniques by Watt and Watt, Page 366:
00547     // A spherical curve is constructed using three
00548     // spherical linear interpolations of a quadrangle
00549     // of unit quaternions: q1, qa, qb, q2.
00550     // Given a set of quaternion keys: q0, q1, q2, q3,
00551     // this routine does the interpolation between
00552     // q1 and q2 by constructing two intermediate
00553     // quaternions: qa and qb. The qa and qb are 
00554     // computed by the intermediate function to 
00555     // guarantee the continuity of tangents across
00556     // adjacent cubic segments. The qa represents in-tangent
00557     // for q1 and the qb represents the out-tangent for q2.
00558     // 
00559     // The q1 q2 is the cubic segment being interpolated. 
00560     // The q0 is from the previous adjacent segment and q3 is 
00561     // from the next adjacent segment. The q0 and q3 are used
00562     // in computing qa and qb.
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         //T trace = m.m[0] + m.m[4] + m.m[8];
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         //T trace = m.m[0] + m.m[5] + m.m[10];
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     // Operators
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     // post-multiply operator, similar to matrices, but different from Shoemake
00658     // Concatenates 'rhs' onto 'this'
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     // transform a vector by the quaternion
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     // Output
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     // From advanced Animation and Rendering
00747     // Techniques by Watt and Watt, Page 366:
00748     // computing the inner quadrangle 
00749     // points (qa and qb) to guarantee tangent
00750     // continuity.
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 } // namespace cinder