# 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