VisFlowRenderer: C:/Users/weinma/Development/VisFlowRenderer/VisFlowRenderer/vmmlib/include/vmmlib/quaternion.hpp Source File

00001 /*
00002 * VMMLib - Vector & Matrix Math Lib
00003 *
00004 * @author Philip Schlegel
00005 * @author Jonas Boesch
00006 * @author Julius Natrup
00007 *
00008 */
00009 
00010 #ifndef __VMML__QUATERNION__HPP__
00011 #define __VMML__QUATERNION__HPP__
00012 
00013 #include <vmmlib/vector.hpp>
00014 #include <vmmlib/matrix.hpp>
00015 #include <vmmlib/details.hpp>
00016 
00017 #include <vmmlib/exception.hpp>
00018 #include <vmmlib/vmmlib_config.hpp>
00019 
00020 #include <algorithm>
00021 #include <cassert>
00022 #include <cmath>
00023 #include <cstdlib>
00024 #include <iomanip>
00025 #include <iostream>
00026 #include <limits>
00027 
00028 
00029 // - declaration - //
00030 
00031 #define QUATERNION_TRACE_EPSILON 1e-5
00032 
00033 namespace vmml
00034 {
00035 
00036 template < typename float_t >
00037 class quaternion
00038 {
00039 public:
00040     // accessors
00041         inline float_t& operator()( size_t position );
00042         inline const float_t& operator()( size_t position ) const;
00043         inline float_t& operator[]( size_t position );
00044         inline const float_t& operator[]( size_t position ) const;
00045         inline float_t& at( size_t position );
00046         inline const float_t& at( size_t position ) const;
00047 
00048     inline float_t& x();
00049     inline const float_t& x() const;
00050     inline float_t& y();
00051     inline const float_t& y() const;
00052     inline float_t& z();
00053     inline const float_t& z() const;
00054     inline float_t& w();
00055     inline const float_t& w() const;
00056 
00057         //constructors
00058         quaternion(); // warning: components NOT initialised (for performance)
00059         quaternion( float_t x, float_t y, float_t z, float_t w );
00060         quaternion( const vector< 3, float_t >& xyz , float_t w );
00061     // initializes the quaternion with xyz, sets w to zero
00062         quaternion( const vector< 3, float_t >& xyz );
00063     
00064         // use rotation matrices only!
00065         quaternion( const matrix< 3, 3, float_t >& rotationMatrix );
00066     // only uses the rotational part ( top-left 3x3 )
00067         quaternion( const matrix< 4, 4, float_t >& matrix );
00068         
00069     void zero();
00070     void identity();
00071 
00072     template< size_t DIM >
00073     void fromRotationMatrix( const matrix< DIM, DIM, float_t >& rotationMatrix );
00074     
00075         void set( float_t ww, float_t xx, float_t yy, float_t zz);
00076 
00077         const quaternion& operator=( const quaternion& a );
00078 
00079     // copies the values in array a to *this
00080     // a must be a 4-component float_t c-style array
00081         void operator=( float_t* a );
00082     void copyFrom1DimCArray( const float_t* a );
00083     
00084         bool operator==( const float_t& a ) const;
00085         bool operator!=( const float_t& a ) const;
00086         
00087         bool operator==( const quaternion& a ) const;
00088         bool operator!=( const quaternion& a ) const;
00089 
00090         bool isAkin( const quaternion& a, 
00091                                  const float_t& delta = std::numeric_limits< float_t >::epsilon() );
00092                                         
00093 
00094         void conjugate();
00095         quaternion getConjugate() const;
00096 
00097         float_t abs() const;
00098         float_t absSquared() const;
00099         
00100         float_t normalize();
00101         quaternion getNormalized() const;
00102 
00103         quaternion negate() const;
00104         quaternion operator-() const;
00105 
00106     //
00107         // quaternion/quaternion operations
00108         //
00109     quaternion operator+( const quaternion< float_t >& a ) const;
00110         quaternion operator-( const quaternion< float_t >& a ) const;
00111         // caution: a * q != q * a in general
00112         quaternion operator*( const quaternion< float_t >& a ) const;
00113         void operator+=( const quaternion< float_t >& a );
00114         void operator-=( const quaternion< float_t >& a );
00115         // caution: a *= q != q *= a in general
00116         void operator*=( const quaternion< float_t >& a );
00117 
00118     //
00119         // quaternion/scalar operations
00120     //
00121         quaternion operator*( float_t a ) const;
00122         quaternion operator/( float_t a ) const;
00123         
00124         void operator*=( float_t a );
00125         void operator/=( float_t a );
00126         
00127     //
00128         //quaternion/vector operations
00129         //
00130     quaternion operator+( const vector< 3, float_t >& a ) const;
00131         quaternion operator-( const vector< 3, float_t >& a ) const;
00132         quaternion operator*( const vector< 3, float_t >& a ) const;
00133 
00134         void operator+=( const vector< 3, float_t >& a );
00135         void operator-=( const vector< 3, float_t >& a );
00136         void operator*=( const vector< 3, float_t >& a );
00137         
00138     // vec3 = this x b
00139         vector< 3, float_t > cross( const quaternion< float_t >& b ) const;
00140 
00141         float_t dot( const quaternion< float_t >& a ) const;
00142         static float_t dot( const quaternion< float_t >& a, const quaternion< float_t >& b );
00143         
00144         // returns multiplicative inverse
00145         quaternion getInverse();
00146         
00147         void normal( const quaternion& aa, const quaternion& bb, const quaternion& cc,  const quaternion& dd );
00148         quaternion normal( const quaternion& aa, const quaternion& bb, const quaternion& cc );
00149         
00150         // to combine two rotations, multiply the respective quaternions before using rotate 
00151         // instead of rotating twice for increased performance, but be aware of non-commutativity!
00152         void rotate( float_t theta, const vector< 3, float_t >& a );
00153         void rotatex( float_t theta );
00154         void rotatey( float_t theta );
00155         void rotatez( float_t theta );
00156         quaternion rotate( float_t theta, vector< 3, float_t >& axis, const vector< 3, float_t >& a );
00157         quaternion rotatex( float_t theta, const vector< 3, float_t >& a );
00158         quaternion rotatey( float_t theta, const vector< 3, float_t >& a );
00159         quaternion rotatez( float_t theta, const vector< 3, float_t >& a );
00160         
00161         quaternion slerp( float_t a, const quaternion< float_t >& p, const quaternion& q );
00162         
00163         float_t getMinComponent();
00164         float_t getMaxComponent();
00165   
00166     matrix< 3, 3, float_t > getRotationMatrix() const;
00167 
00168     template< size_t DIM >
00169     void getRotationMatrix( matrix< DIM, DIM, float_t >& result ) const;
00170         
00171         friend std::ostream& operator << ( std::ostream& os, const quaternion& q )
00172         {
00173                 const std::ios::fmtflags flags =    os.flags();
00174                 const int                                prec  =    os.precision();
00175                 
00176                 os. setf( std::ios::right, std::ios::adjustfield );
00177                 os.precision( 5 );
00178                 os << "[" 
00179             << std::setw(10) << "w: " << q.w() << " " 
00180             << std::setw(10) << "xyz: " << q.x() << " " 
00181             << std::setw(10) << q.y() << " " 
00182             << std::setw(10) << q.z() 
00183             << " ]";
00184                 os.precision( prec );
00185                 os.setf( flags );
00186                 return os;
00187         };
00188     
00189     //storage -> layout x,y,z,w 
00190     float_t array[ 4 ]
00191     #ifndef VMMLIB_DONT_FORCE_ALIGNMENT
00192         #ifdef _GCC
00193             __attribute__((aligned(16)))
00194         #endif
00195     #endif
00196     ;
00197 
00198         static const quaternion ZERO;
00199         static const quaternion IDENTITY;
00200         static const quaternion QUATERI;
00201         static const quaternion QUATERJ;
00202         static const quaternion QUATERK;
00203 
00204 }; // class quaternion
00205 
00206 #ifndef VMMLIB_NO_TYPEDEFS
00207 typedef quaternion< float >  quaternionf;
00208 typedef quaternion< double > quaterniond;
00209 #endif
00210 
00211 // - implementation - //
00212 
00213 template < typename float_t >
00214 const quaternion< float_t > quaternion< float_t >::ZERO( 0, 0, 0, 0 );
00215 
00216 template < typename float_t >
00217 const quaternion< float_t > quaternion< float_t >::IDENTITY( 0, 0, 0, 1 );
00218 
00219 template < typename float_t >
00220 const quaternion< float_t > quaternion< float_t >::QUATERI( 1, 0, 0, 0 );
00221 
00222 template < typename float_t >
00223 const quaternion< float_t > quaternion< float_t >::QUATERJ( 0, 1, 0, 0 );
00224 
00225 template < typename float_t >
00226 const quaternion< float_t > quaternion< float_t >::QUATERK( 0, 0, 1, 0 );
00227 
00228 
00229 template < typename float_t >
00230 quaternion< float_t >::quaternion()
00231 {
00232     // intentionally left empty
00233 }
00234 
00235 
00236 
00237 template < typename float_t >
00238 quaternion< float_t >::quaternion( float_t x_, float_t y_, float_t z_, float_t w_ )
00239 {
00240     x() = x_;
00241     y() = y_;
00242     z() = z_;
00243     w() = w_;
00244 }
00245 
00246 
00247 
00248 template < typename float_t >
00249 quaternion< float_t >::quaternion(
00250     const vector< 3, float_t >& xyz,
00251     float_t w_ 
00252     )
00253 {
00254         w() = w_;
00255     memcpy( array, xyz.array, 3 * sizeof( float_t ) );
00256 }
00257 
00258 
00259 
00260 
00261 template < typename float_t >
00262 quaternion< float_t >::quaternion( const vector< 3, float_t >& xyz )
00263 {
00264         w() = 0.0;
00265     memcpy( array, xyz.array, 3 * sizeof( float_t ) );
00266 }
00267 
00268 
00269 
00270 
00271 template < typename float_t >
00272 quaternion< float_t >::quaternion( const matrix< 3, 3, float_t >& M )
00273 {
00274     fromRotationMatrix< 3 >( M );
00275 }
00276 
00277 
00278 template < typename float_t >
00279 quaternion< float_t >::quaternion( const matrix< 4, 4, float_t >& M )
00280 {
00281     fromRotationMatrix< 4 >( M );
00282 }
00283 
00284 
00285 
00286 template < typename float_t >
00287 template< size_t DIM >
00288 void
00289 quaternion< float_t >::
00290 fromRotationMatrix( const matrix< DIM, DIM, float_t >& M )
00291 {
00292     float_t trace = M( 0, 0 ) + M( 1, 1 ) + M( 2,2 ) + 1.0;
00293     
00294     // very small traces may introduce a big numerical error
00295     if( trace > QUATERNION_TRACE_EPSILON )
00296     {
00297         float_t s = 0.5 / details::getSquareRoot( trace );
00298         x() = M( 2, 1 ) - M( 1, 2 );
00299         x() *= s;
00300 
00301         y() = M( 0, 2 ) - M( 2, 0 );
00302         y() *= s;
00303 
00304         z() = M( 1, 0 ) - M( 0, 1 );
00305         z() *= s;
00306 
00307         w() = 0.25 / s;
00308     }
00309     else 
00310     {
00311         // 0, 0 is largest
00312         if ( M( 0, 0 ) > M( 1, 1 ) && M( 0, 0 ) > M( 2, 2 ) )
00313         {
00314             float_t s = 0.5 / details::getSquareRoot( 1.0 + M( 0, 0 ) - M( 1, 1 ) - M( 2, 2 ) );
00315             x() = 0.25 / s;
00316 
00317             y() = M( 0,1 ) + M( 1,0 );
00318             y() *= s;
00319 
00320             z() = M( 0,2 ) + M( 2,0 );
00321             z() *= s;
00322             
00323             w() = M( 1,2 ) - M( 2,1 );
00324             w() *= s;
00325         }
00326         else
00327         {
00328             // 1, 1 is largest
00329             if ( M( 1, 1 ) > M( 2, 2 ) )
00330             {
00331                 float_t s = 0.5 / details::getSquareRoot( 1.0 + M( 1,1 ) - M( 0,0 ) - M( 2,2 ) );
00332                 x() = M( 0,1 ) + M( 1,0 );
00333                 x() *= s;
00334 
00335                 y() = 0.25 / s;
00336 
00337                 z() = M( 1,2 ) + M( 2,1 );
00338                 z() *= s;
00339 
00340                 w() = M( 0,2 ) - M( 2,0 );
00341                 w() *= s;
00342             }
00343             // 2, 2 is largest
00344             else 
00345             {
00346                 float_t s = 0.5 / details::getSquareRoot( 1.0 + M( 2,2 ) - M( 0,0 ) - M( 1,1 ) );
00347                 x() = M( 0,2 ) + M( 2,0 );
00348                 x() *= s;
00349 
00350                 y() = M( 1,2 ) + M( 2,1 );
00351                 y() *= s;
00352 
00353                 z() = 0.25 / s;
00354 
00355                 w() = M( 0,1 ) - M( 1,0 );
00356                 w() *= s;
00357             }
00358         }
00359     }
00360 }
00361 
00362 
00363 
00364 template < typename float_t >
00365 void
00366 quaternion< float_t >::zero()
00367 {
00368     memcpy( array, ZERO.array, 4 * sizeof( float_t ) );
00369 }
00370 
00371 
00372 
00373 template < typename float_t >
00374 void
00375 quaternion< float_t >::identity()
00376 {
00377     memcpy( array, IDENTITY.array, 4 * sizeof( float_t ) );
00378 }
00379 
00380 
00381 
00382 template < typename float_t >
00383 void
00384 quaternion< float_t >::set( float_t xx, float_t yy, float_t zz, float_t ww )
00385 {
00386         x() = xx;
00387         y() = yy;
00388         z() = zz;
00389         w() = ww;
00390 }
00391 
00392 
00393 
00394 template < typename float_t >
00395 void
00396 quaternion< float_t >::copyFrom1DimCArray( const float_t* a )
00397 {
00398     memcpy( array, a, 4 * sizeof( float_t ) );
00399 }
00400 
00401 
00402 
00403 template < typename float_t >
00404 inline void
00405 quaternion< float_t >::operator=( float_t* a )
00406 {
00407     copyFrom1DimCArray( a );
00408 }
00409 
00410 
00411 
00412 template < typename float_t >
00413 const quaternion< float_t >&
00414 quaternion < float_t >::operator=( const quaternion< float_t >& a )
00415 {
00416     memcpy( array, a.array, 4 * sizeof( float_t ) );
00417         return *this;
00418 }
00419 
00420 
00421 
00422 template < typename float_t >
00423 bool
00424 quaternion< float_t >::operator==( const float_t& a ) const
00425 {
00426         return ( w() == a && x() == 0 && y() == 0 && z() == 0 );
00427 }
00428 
00429 
00430 
00431 template < typename float_t >
00432 bool quaternion< float_t >::operator!=( const float_t& a ) const
00433 {
00434         return ( w() != a || x() != 0 || y() != 0 || z() != 0 );
00435 }
00436 
00437 
00438 
00439 template < typename float_t >
00440 bool
00441 quaternion< float_t >::operator==( const quaternion& a ) const
00442 {
00443         return ( 
00444         w() == a.w() && 
00445         x() == a.x() &&
00446         y() == a.y() &&
00447         z() == a.z()
00448         );
00449 }
00450 
00451 
00452 
00453 template < typename float_t >
00454 bool
00455 quaternion< float_t >::operator!=( const quaternion& a ) const
00456 {
00457         return ! this->operator==( a );
00458 }
00459 
00460 
00461 
00462 template < typename float_t >
00463 bool
00464 quaternion< float_t >::isAkin( const quaternion& a, const float_t& delta )
00465 {
00466         if( fabsf( w() - a.w() ) > delta || 
00467         fabsf( x() - a.x() ) > delta || 
00468                 fabsf( y() - a.y() ) > delta ||
00469         fabsf( z() - a.z() ) > delta
00470         )
00471         return false;
00472         return true;
00473 }
00474 
00475 
00476 
00477 template < typename float_t >
00478 inline float_t&
00479 quaternion< float_t >::x()
00480 {
00481     return array[ 0 ];
00482 }
00483 
00484 
00485 
00486 template < typename float_t >
00487 inline const float_t&
00488 quaternion< float_t >::x() const
00489 {
00490     return array[ 0 ];
00491 }
00492 
00493 
00494 
00495 template < typename float_t >
00496 inline float_t&
00497 quaternion< float_t >::y()
00498 {
00499     return array[ 1 ];
00500 }
00501 
00502 
00503 
00504 template < typename float_t >
00505 inline const float_t&
00506 quaternion< float_t >::y() const
00507 {
00508     return array[ 1 ];
00509 }
00510 
00511 
00512 
00513 template < typename float_t >
00514 inline float_t&
00515 quaternion< float_t >::z()
00516 {
00517     return array[ 2 ];
00518 }
00519 
00520 
00521 
00522 template < typename float_t >
00523 inline const float_t&
00524 quaternion< float_t >::z() const
00525 {
00526     return array[ 2 ];
00527 }
00528 
00529 
00530 
00531 template < typename float_t >
00532 inline float_t&
00533 quaternion< float_t >::w()
00534 {
00535     return array[ 3 ];
00536 }
00537 
00538 
00539 
00540 template < typename float_t >
00541 inline const float_t&
00542 quaternion< float_t >::w() const
00543 {
00544     return array[ 3 ];
00545 }
00546 
00547 
00548 
00549 template < typename float_t >
00550 inline float_t&
00551 quaternion< float_t >::at( size_t index )
00552 {
00553     #ifdef VMMLIB_SAFE_ACCESSORS
00554     if ( index >= 4 )
00555     {
00556         VMMLIB_ERROR( "at() - index out of bounds", VMMLIB_HERE );
00557     }
00558     #endif
00559     return array[ index ];
00560 
00561 }
00562 
00563 
00564 
00565 template < typename float_t >
00566 inline const float_t&
00567 quaternion< float_t >::at( size_t index ) const
00568 {
00569     #ifdef VMMLIB_SAFE_ACCESSORS
00570     if ( index >= 4 )
00571     {
00572         VMMLIB_ERROR( "at() - index out of bounds", VMMLIB_HERE );
00573     }
00574     #endif
00575     return array[ index ];
00576 }
00577 
00578 
00579 
00580 template < typename float_t >
00581 inline float_t&
00582 quaternion< float_t >::operator[]( size_t index )
00583 {
00584     return at( index );
00585 }
00586 
00587 
00588 
00589 template < typename float_t >
00590 inline const float_t&
00591 quaternion< float_t >::operator[]( size_t index ) const
00592 {
00593     return at( index );
00594 }
00595 
00596 
00597 
00598 template < typename float_t >
00599 inline float_t&
00600 quaternion< float_t >::operator()( size_t index )
00601 {
00602     return at( index );
00603 }
00604 
00605         
00606 
00607 template < typename float_t >
00608 inline const float_t&
00609 quaternion< float_t >::operator()( size_t index ) const
00610 {
00611     return at( index );
00612 }
00613 
00614 
00615 
00616 template < typename float_t >
00617 void quaternion< float_t >::conjugate() 
00618 {
00619     x() = -x();
00620     y() = -y();
00621     z() = -z();
00622 }
00623 
00624 
00625 
00626 template < typename float_t >
00627 quaternion< float_t > quaternion< float_t >::getConjugate() const
00628 {
00629         return quaternion< float_t > ( -x(), -y(), -z(), w() );
00630 }
00631 
00632 
00633 
00634 template < typename float_t >
00635 float_t
00636 quaternion< float_t >::abs() const
00637 {
00638         return details::getSquareRoot( absSquared() );
00639 }
00640 
00641 
00642 
00643 template < typename float_t >
00644 float_t quaternion< float_t >::absSquared() const
00645 {
00646         return x() * x() + y() * y() + z() * z() + w() * w();
00647 }
00648 
00649 
00650 
00651 template < typename float_t >
00652 quaternion< float_t > quaternion< float_t >::getInverse()
00653 {
00654         quaternion< float_t > q( *this );
00655     q.conjugate();
00656     
00657         float_t tmp = absSquared();
00658         tmp = 1.0f / tmp;
00659         return q * tmp;
00660 }
00661 
00662 
00663 
00664 template < typename float_t >
00665 float_t quaternion< float_t >::normalize()
00666 {
00667         float_t length = abs();
00668         if( length == 0.0 )
00669                 return 0.0;
00670         length = 1.0f / length;
00671     this->operator*=( length );
00672         return length;
00673 }
00674 
00675 
00676 
00677 template < typename float_t >
00678 quaternion< float_t >
00679 quaternion< float_t >::getNormalized() const
00680 {
00681     quaternion< float_t > normalized_quaternion( *this );
00682     normalized_quaternion.normalize();
00683         return normalized_quaternion;
00684 }
00685 
00686 
00687 
00688 //
00689 // quaternion/quaternion operations
00690 //
00691 
00692 template < typename float_t >
00693 quaternion< float_t >
00694 quaternion< float_t >::operator+( const quaternion< float_t >& a ) const
00695 {
00696         return quaternion( x() + a.x(), y() + a.y(), z() + a.z(), w() + a.w() );
00697 }
00698 
00699 
00700 
00701 template < typename float_t >
00702 quaternion< float_t >
00703 quaternion< float_t >::operator-( const quaternion< float_t >& a ) const
00704 {
00705         return quaternion( x() - a.x(), y() - a.y(), z() - a.z(), w() - a.w() );
00706 }
00707 
00708 
00709 
00710 // returns Grasssmann product
00711 template < typename float_t >
00712 quaternion< float_t >
00713 quaternion< float_t >::operator*( const quaternion< float_t >& a ) const
00714 {
00715     quaternion< float_t > ret( *this );
00716     ret *= a;
00717     return ret;
00718 }
00719 
00720 
00721 
00722 // Grassmann product
00723 template < typename float_t >
00724 void
00725 quaternion< float_t >::operator*=( const quaternion< float_t >& q )
00726 {
00727     #if 0
00728     quaternion< float_t > orig( *this );
00729         x() = orig.w() * a.x() + orig.x() * a.w() + orig.y() * a.z() - orig.z() * a.y();
00730         y() = orig.w() * a.y() + orig.y() * a.w() + orig.z() * a.x() - orig.x() * a.z();
00731         z() = orig.w() * a.z() + orig.z() * a.w() + orig.x() * a.y() - orig.y() * a.x();
00732         w() = orig.w() * a.w() - orig.x() * a.x() - orig.y() * a.y() - orig.z() * a.z();
00733     #else
00734 
00735     // optimized version, 7 less mul, but 15 more add/subs
00736     // after Henrik Engstrom, from a gamedev.net article.
00737 
00738     const float_t& a = array[ 3 ];
00739     const float_t& b = array[ 0 ];
00740     const float_t& c = array[ 1 ];
00741     const float_t& d = array[ 2 ];
00742     const float_t& x = q.array[ 3 ];
00743     const float_t& y = q.array[ 0 ];
00744     const float_t& z = q.array[ 1 ];
00745     const float_t& w = q.array[ 2 ];
00746 
00747     const float_t tmp_00 = (d - c) * (z - w);
00748     const float_t tmp_01 = (a + b) * (x + y);
00749     const float_t tmp_02 = (a - b) * (z + w);
00750     const float_t tmp_03 = (c + d) * (x - y);
00751     const float_t tmp_04 = (d - b) * (y - z);
00752     const float_t tmp_05 = (d + b) * (y + z);
00753     const float_t tmp_06 = (a + c) * (x - w);
00754     const float_t tmp_07 = (a - c) * (x + w);
00755     const float_t tmp_08 = tmp_05 + tmp_06 + tmp_07;
00756     const float_t tmp_09 = 0.5 * (tmp_04 + tmp_08);
00757     
00758     array[ 3 ] = tmp_00 + tmp_09 - tmp_05;
00759     array[ 0 ] = tmp_01 + tmp_09 - tmp_08;
00760     array[ 1 ] = tmp_02 + tmp_09 - tmp_07;
00761     array[ 2 ] = tmp_03 + tmp_09 - tmp_06;   
00762     
00763     #endif
00764 }
00765 
00766 
00767 
00768 
00769 
00770 template < typename float_t >
00771 quaternion< float_t > 
00772 quaternion< float_t >::operator-() const
00773 {
00774         return quaternion( -x(), -y(), -z(), -w() );
00775 }
00776 
00777 
00778 
00779 template < typename float_t >
00780 void
00781 quaternion< float_t >::operator+=( const quaternion< float_t >& a )
00782 {
00783     array[ 0 ] += a.array[ 0 ];
00784     array[ 1 ] += a.array[ 1 ];
00785     array[ 2 ] += a.array[ 2 ];
00786     array[ 3 ] += a.array[ 3 ];
00787 }       
00788 
00789 
00790 
00791 template < typename float_t >
00792 void
00793 quaternion< float_t >::operator-=( const quaternion< float_t >& a )
00794 {
00795     array[ 0 ] -= a.array[ 0 ];
00796     array[ 1 ] -= a.array[ 1 ];
00797     array[ 2 ] -= a.array[ 2 ];
00798     array[ 3 ] -= a.array[ 3 ];
00799 }
00800 
00801 
00802 
00803 //
00804 // quaternion/scalar operations         
00805 //
00806 
00807 template < typename float_t >
00808 quaternion< float_t >
00809 quaternion< float_t >::operator*( const float_t a ) const
00810 {
00811         return quaternion( x() * a, y() * a, z() * a, w() * a );
00812 }
00813 
00814 
00815         
00816 template < typename float_t >
00817 quaternion< float_t >
00818 quaternion< float_t >::operator/( float_t a ) const
00819 {  
00820     if ( a == 0.0 )
00821     {
00822         VMMLIB_ERROR( "Division by zero.", VMMLIB_HERE );
00823     }
00824         a = 1.0 / a;
00825         return quaternion( x() * a, y() * a, z() * a, w() * a );
00826 }
00827 
00828 
00829 
00830 template < typename float_t >
00831 void
00832 quaternion< float_t >::operator*=( float_t a ) 
00833 {
00834         array[ 0 ] *= a;
00835         array[ 1 ] *= a;
00836         array[ 2 ] *= a;
00837         array[ 3 ] *= a;
00838 }       
00839         
00840 
00841 
00842 template < typename float_t >
00843 void
00844 quaternion< float_t >::operator/=( float_t a ) 
00845 {
00846     if ( a == 0.0 )
00847     {
00848         VMMLIB_ERROR( "Division by zero", VMMLIB_HERE );
00849     }
00850         a = 1.0f / a;
00851     this->operator*=( a );
00852 }
00853 
00854 
00855 //quaternion/vector operations
00856 
00857 template < typename float_t >
00858 quaternion< float_t >
00859 quaternion< float_t >::operator+( const vector< 3, float_t >& a ) const
00860 {
00861         return quaternion( x() + a.x(), y() + a.y(), z() + a.z(), w() );
00862 }
00863 
00864 
00865 
00866 template < typename float_t >
00867 quaternion< float_t >
00868 quaternion< float_t >::operator-( const vector< 3, float_t >& a ) const
00869 {
00870         return quaternion( w(), x() - a.x(), y() - a.y(), z() - a.z() );
00871 }
00872 
00873 
00874 
00875 template < typename float_t >
00876 quaternion< float_t >
00877 quaternion< float_t >::operator*( const vector< 3, float_t >& a ) const
00878 {
00879         return quaternion( -x() * a.x() - y() * a.y() - z() * a.z(),
00880                                                 w() * a.x() + y() * a.z() - z() * a.y(), 
00881                                                 w() * a.y() + z() * a.x() - x() * a.z(),
00882                                                 w() * a.z() + x() * a.y() - y() * a.x()  );
00883 }
00884 
00885 
00886 
00887 template < typename float_t >
00888 void
00889 quaternion< float_t >::operator+=( const vector< 3, float_t >& xyz )
00890 {
00891         x() += xyz.x();
00892         y() += xyz.y();
00893         y() += xyz.z();
00894 }
00895 
00896 
00897 
00898 template < typename float_t >
00899 void
00900 quaternion< float_t >::operator-=( const vector< 3, float_t >& xyz )
00901 {
00902         x() -= xyz.x();
00903         y() -= xyz.y();
00904         z() -= xyz.z();
00905         return *this;
00906 }
00907 
00908 
00909 
00910 template < typename float_t >
00911 void
00912 quaternion< float_t >::operator*=(const vector< 3, float_t >& a )
00913 {
00914     float_t x = x();
00915     float_t y = y();
00916     float_t z = z();
00917     float_t w = w();
00918     
00919         x() = w * a.x() + y * a.z() - z * a.y();
00920         y() = w * a.y() + z * a.x() - x * a.z();
00921         z() = w * a.z() + x * a.y() - y * a.x();
00922     w() = -x * a.x() - y * a.y() - z * a.z();
00923 }
00924         
00925 
00926 
00927 
00928 template < typename float_t >
00929 vector< 3, float_t > quaternion< float_t >::cross( const quaternion< float_t >& bb ) const
00930 {
00931         vector< 3, float_t > result;
00932 
00933     result.array[ 0 ] = y() * bb.z() - z() * bb.y(); 
00934     result.array[ 1 ] = z() * bb.x() - x() * bb.z(); 
00935     result.array[ 2 ] = x() * bb.y() - y() * bb.x(); 
00936 
00937     return result;
00938 }
00939 
00940 
00941 
00942 template < typename float_t >
00943 float_t quaternion< float_t >::dot( const quaternion< float_t >& a ) const
00944 {
00945         return w() * a.w() + x() * a.x() + y() * a.y() + z() * a.z();
00946 }
00947 
00948 
00949 
00950 template < typename float_t >
00951 float_t quaternion< float_t >::
00952 dot( const quaternion< float_t >& a, const quaternion< float_t >& b )
00953 {
00954         return a.w() * b.w() + a.x() * b.x() + a.y() * b.y() + a.z() * b.z();
00955 }
00956 
00957 
00958 
00959 template < typename float_t >
00960 void quaternion< float_t >::normal( const quaternion< float_t >& aa,
00961                                                           const quaternion< float_t >& bb,
00962                                                           const quaternion< float_t >& cc,
00963                                                           const quaternion< float_t >& dd )
00964 {
00965         //right hand system, CCW triangle
00966         const quaternion< float_t > quat_t = bb - aa;
00967         const quaternion< float_t > quat_u = cc - aa;
00968         const quaternion< float_t > quat_v = dd - aa;
00969         cross( quat_t, quat_u, quat_v );
00970         normalize();
00971 }
00972 
00973 
00974 
00975 template < typename float_t >
00976 quaternion< float_t > quaternion< float_t >::normal( const quaternion< float_t >& aa,
00977                                                                                  const quaternion< float_t >& bb,
00978                                                                                  const quaternion< float_t >& cc )
00979 {
00980         quaternion< float_t > tmp;
00981         tmp.normal( *this, aa, bb, cc );
00982         return tmp;
00983 }
00984 
00985 
00986 // to combine two rotations, multiply the respective quaternions before using rotate 
00987 // instead of rotating twice for increased performance, but be aware of non-commutativity!
00988 // (the first rotation quaternion has to be the first factor)
00989 template< typename float_t >
00990 quaternion< float_t >
00991 quaternion< float_t >::rotate( float_t theta, vector< 3, float_t >& axis, const vector< 3, float_t >& a )
00992 {
00993         quaternion< float_t > p = a;
00994         float_t alpha = theta / 2;
00995         quaternion< float_t > q = cos( alpha ) + ( sin( alpha ) * axis.normalize() );
00996         return q * p * q.invert();
00997 }
00998 
00999 
01000 
01001 template< typename float_t >
01002 quaternion< float_t > quaternion< float_t >::rotatex( float_t theta, const vector< 3, float_t >& a )
01003 {
01004         quaternion< float_t > p = a;
01005         float_t alpha = theta / 2;
01006         quaternion< float_t > q = cos( alpha ) + ( sin( alpha ) *  QUATERI );
01007         return q * p * q.invert();
01008 }
01009 
01010 
01011 
01012 template< typename float_t >
01013 quaternion< float_t > quaternion< float_t >::rotatey( float_t theta, const vector< 3, float_t >& a )
01014 {
01015         quaternion< float_t > p = a;
01016         float_t alpha = theta / 2;
01017         quaternion< float_t > q = cos( alpha ) + ( sin( alpha ) *  QUATERJ );
01018         return q * p * q.invert();
01019 }
01020 
01021 
01022 
01023 template< typename float_t >
01024 quaternion< float_t > quaternion< float_t >::rotatez( float_t theta, const vector< 3, float_t >& a )
01025 {
01026         quaternion< float_t > p = a;
01027         float_t alpha = theta / 2;
01028         quaternion< float_t > q = cos( alpha ) + ( sin( alpha ) *  QUATERK );
01029         return q * p * q.invert();
01030 }
01031 
01032 
01033 
01034 template < typename float_t >
01035 float_t quaternion< float_t >::getMinComponent()
01036 {
01037         float_t m = std::min( w(), x() );
01038         m = std::min( m, y() );
01039         m = std::min( m, z() );
01040         return m;
01041 }
01042 
01043 
01044 
01045 template < typename float_t >
01046 float_t quaternion< float_t >::getMaxComponent()
01047 {
01048         float_t m = std::max( w(), x() );
01049         m = std::max( m(), y() );
01050         m = std::max( m(), z() );
01051         return m;
01052 }
01053 
01054 
01055 
01056 template < typename float_t >
01057 matrix< 3, 3, float_t >
01058 quaternion< float_t >::getRotationMatrix() const
01059 {
01060     matrix< 3, 3, float_t > result;
01061     getRotationMatrix< 3 >( result );
01062     return result;
01063 }
01064 
01065 
01066 
01067 template < typename float_t >
01068 template< size_t DIM >
01069 void
01070 quaternion< float_t >::getRotationMatrix( matrix< DIM, DIM, float_t >& M ) const
01071 {
01072     float_t w2 = w() * w();
01073     float_t x2 = x() * x();
01074     float_t y2 = y() * y();
01075     float_t z2 = z() * z();
01076     float_t wx = w() * x();
01077     float_t wy = w() * y();
01078     float_t wz = w() * z();
01079     float_t xy = x() * y();
01080     float_t xz = x() * z();
01081     float_t yz = y() * z();
01082 
01083     M( 0, 0 ) = w2 + x2 - y2 - z2;
01084     M( 0, 1 ) = 2. * (xy - wz);
01085     M( 0, 2 ) = 2. * (xz + wy);
01086     M( 1, 0 ) = 2. * (xy + wz);
01087     M( 1, 1 ) = w2 - x2 + y2 - z2;
01088     M( 1, 2 ) = 2. * (yz - wx);
01089     M( 2, 0 ) = 2. * (xz - wy);
01090     M( 2, 1 ) = 2. * (yz + wx);
01091     M( 2, 2 ) = w2 - x2 - y2 + z2;
01092 
01093 }
01094 
01095 
01096 
01097 template < typename float_t >
01098 quaternion< float_t > quaternion< float_t >::
01099 slerp( float_t a, const quaternion< float_t >& p, const quaternion< float_t >& q )
01100 {
01101         p = p.normalize();
01102         q = q.normalize();
01103         float_t cosine = p.dot(q);
01104         quaternion< float_t > quat_t;
01105         
01106         // check if inverted rotation is needed
01107         if ( cosine < 0.0 )
01108         {
01109                 cosine = -cosine;
01110                 quat_t = -q;
01111         }
01112         else
01113         {
01114                 quat_t = q;
01115         }
01116         
01117         if( cosine.abs() < 1 - 1e-13 )
01118         {
01119                 // standard slerp
01120                 float_t sine = sqrt( 1. - ( cosine * cosine ) );
01121                 float_t angle = atan2( sine, cosine );
01122                 float_t coeff1 = sin( 1.0 - a ) * angle / sine;
01123                 float_t coeff2 = sin( a * angle ) / sine;
01124                 return coeff1 * p + coeff2 * quat_t;
01125         }
01126         else
01127         {
01128                 // linear interpolation for very small angles  
01129                 quaternion< float_t > quat_u = ( 1. - a ) * p + a * quat_t;
01130                 quat_u.normalize();
01131                 return quat_u;
01132         }
01133 }
01134 
01135 }
01136 #endif