Main Page   Modules   Class Hierarchy   Alphabetical List   Compound List   File List   Compound Members   File Members   Related Pages  

quatimpl.hpp

00001 /***********************************************************************
00002 
00003   quatimpl.hpp
00004 
00005   A quaternion template class 
00006 
00007   -------------------------------------------------------------------
00008 
00009   Feb 1998, Paul Rademacher (rademach@cs.unc.edu)
00010   Oct 2000, Bill Baxter
00011 
00012   Modification History:
00013   - See main header file, quat.hpp
00014 
00015 ************************************************************************/
00016 
00017 #include "quat.hpp"
00018 #include <math.h>
00019 #include <assert.h>
00020 
00021 
00022 //============================================================================
00023 // CONSTRUCTORS
00024 //============================================================================
00025 
00026 template <class Type>
00027 Quat<Type>::Quat( void )
00028 {
00029   // do nothing so default construction is fast
00030 }
00031 
00032 template <class Type>
00033 Quat<Type>::Quat( Type _s, Type x, Type y, Type z )
00034 {
00035   s = _s;
00036   v.Set( x, y, z );
00037 }
00038 template <class Type>
00039 Quat<Type>::Quat( Type x, Type y, Type z )
00040 {
00041   s = 0.0;
00042   v.Set( x, y, z );
00043 }
00044 
00045 template <class Type>
00046 Quat<Type>::Quat( const qvec& _v, Type _s )
00047 {
00048   Set( _v, _s );
00049 }
00050 
00051 template <class Type>
00052 Quat<Type>::Quat( Type _s, const qvec& _v )
00053 {
00054   Set( _v, _s );
00055 }
00056 
00057 
00058 template <class Type>
00059 Quat<Type>::Quat( const Type *d )
00060 {
00061   s = *d++;
00062   v.Set(d);
00063 }
00064 
00065 template <class Type>
00066 Quat<Type>::Quat( const Quat &q )
00067 {
00068   s = q.s;
00069   v = q.v;
00070 }
00071 
00072 //============================================================================
00073 // SETTERS
00074 //============================================================================
00075 
00076 template <class Type>
00077 void Quat<Type>::Set( Type _s, Type x, Type y, Type z )
00078 {
00079   s = _s;
00080   v.Set(x,y,z);
00081 }
00082 template <class Type>
00083 void Quat<Type>::Set( Type x, Type y, Type z )
00084 {
00085   s = 0.0;
00086   v.Set(x,y,z);
00087 }
00088 
00089 template <class Type>
00090 void Quat<Type>::Set( const qvec& _v, Type _s )
00091 {
00092   s = _s;
00093   v = _v;
00094 }
00095 template <class Type>
00096 void Quat<Type>::Set( Type _s, const qvec& _v )
00097 {
00098   s = _s;
00099   v = _v;
00100 }
00101 
00102 
00103 //============================================================================
00104 // OPERATORS
00105 //============================================================================
00106 
00107 template <class Type>
00108 Quat<Type>& Quat<Type>::operator = (const Quat& q)
00109 { 
00110   v = q.v;  s = q.s; return *this; 
00111 }
00112 
00113 template <class Type>
00114 Quat<Type>& Quat<Type>::operator += ( const Quat &q )
00115 {
00116   v += q.v; s += q.s; return *this;
00117 }
00118 
00119 template <class Type>
00120 Quat<Type>& Quat<Type>::operator -= ( const Quat &q )
00121 {
00122   v -= q.v; s -= q.s; return *this;
00123 }
00124 
00125 template <class Type>
00126 Quat<Type> &Quat<Type>::operator *= ( const Type d ) 
00127 {
00128   v *= d; s *= d; return *this;
00129 }
00130 
00131 template <class Type>
00132 Quat<Type> &Quat<Type>::operator *= ( const Quat& q ) 
00133 {
00134 #if 0
00135   // Quaternion multiplication with 
00136   // temporary object construction minimized (hopefully)
00137   Type ns = s*q.s - v*q.v;
00138   qvec nv(v^q.v);
00139   v *= q.s;
00140   v += nv;
00141   nv.Set(s*q.v);
00142   v += nv;
00143   s = ns;
00144   return *this;
00145 #else
00146   // optimized (12 mults, and no compiler-generated temp objects)
00147   Type A, B, C, D, E, F, G, H;
00148 
00149   A = (s   + v.x)*(q.s   + q.v.x);
00150   B = (v.z - v.y)*(q.v.y - q.v.z);
00151   C = (s   - v.x)*(q.v.y + q.v.z); 
00152   D = (v.y + v.z)*(q.s   - q.v.x);
00153   E = (v.x + v.z)*(q.v.x + q.v.y);
00154   F = (v.x - v.z)*(q.v.x - q.v.y);
00155   G = (s   + v.y)*(q.s   - q.v.z);
00156   H = (s   - v.y)*(q.s   + q.v.z);
00157 
00158   v.x = A - (E + F + G + H) * Type(0.5); 
00159   v.y = C + (E - F + G - H) * Type(0.5); 
00160   v.z = D + (E - F - G + H) * Type(0.5);
00161   s = B + (-E - F + G + H) * Type(0.5);
00162 
00163   return *this;
00164 #endif
00165 }
00166 
00167 template <class Type>
00168 Quat<Type> &Quat<Type>::operator /= ( const Type d )
00169 {
00170   Type r = Type(1.0)/d;
00171   v *= r;
00172   s *= r;
00173   return *this;
00174 }
00175 
00176 template <class Type>
00177 Type &Quat<Type>::operator [] ( int i)
00178 {
00179   switch (i) {
00180     case 0: return s;
00181     case 1: return v.x;
00182     case 2: return v.y;
00183     case 3: return v.z;
00184   }
00185   assert(false);
00186   return s;
00187 }
00188 
00189 //============================================================================
00190 // SPECIAL FUNCTIONS
00191 //============================================================================
00192 template <class Type>
00193 inline Type Quat<Type>::Length( void ) const
00194 {
00195   return Type( sqrt( v*v + s*s ) );
00196 }
00197 
00198 template <class Type>
00199 inline Type Quat<Type>::LengthSqr( void ) const
00200 {
00201   return Norm();
00202 }
00203 
00204 template <class Type>
00205 inline Type Quat<Type>::Norm( void ) const
00206 {
00207   return v*v + s*s;
00208 }
00209 
00210 template <class Type>
00211 inline Quat<Type>& Quat<Type>::Normalize( void )
00212 {
00213   *this *= Type(1.0) / Type(sqrt(v*v + s*s));
00214   return *this;
00215 }
00216 
00217 template <class Type>
00218 inline Quat<Type>& Quat<Type>::Invert( void )
00219 {
00220   Type scale = Type(1.0)/Norm();
00221   v *= -scale;
00222   s *= scale;
00223   return *this;
00224 }
00225 
00226 template <class Type>
00227 inline Quat<Type>& Quat<Type>::Conjugate( void )
00228 {
00229   v.x = -v.x;
00230   v.y = -v.y;
00231   v.z = -v.z;
00232   return *this;
00233 }
00234 
00235 
00236 //----------------------------------------------------------------------------
00237 // Xform
00238 //----------------------------------------------------------------------------
00239 // Transform a vector by this quaternion using q * v * q^-1
00240 //----------------------------------------------------------------------------
00241 template <class Type>
00242 Quat<Type>::qvec Quat<Type>::Xform( const qvec &vec ) const
00243 {
00244   /* copy vector into temp quaternion for multiply      */
00245   Quat  vecQuat(vec);
00246   /* invert multiplier  */
00247   Quat  inverse(*this);
00248   inverse.Invert();
00249 
00250   /* do q * vec * q(inv)        */
00251   Quat  tempVecQuat(*this * vecQuat);
00252   tempVecQuat *= inverse;
00253 
00254   /* return vector part */
00255   return tempVecQuat.v;
00256 }
00257 
00258 //----------------------------------------------------------------------------
00259 // Log
00260 //----------------------------------------------------------------------------
00261 // Natural log of quat
00262 //----------------------------------------------------------------------------
00263 template <class Type>
00264 Quat<Type> &Quat<Type>::Log(void)
00265 {
00266   Type theta, scale;
00267 
00268   scale = v.Length();
00269   theta = ATan2(scale, s);
00270 
00271   if (scale > 0.0)
00272     scale = theta/scale;
00273 
00274   v *= scale;
00275   s = 0.0;
00276   return *this;
00277 }
00278 
00279 //----------------------------------------------------------------------------
00280 // Exp
00281 //----------------------------------------------------------------------------
00282 // e to the quat: e^quat 
00283 // -- assuming scalar part 0
00284 //----------------------------------------------------------------------------
00285 template <class Type>
00286 Quat<Type> &Quat<Type>::Exp(void)
00287 {
00288   Type scale;
00289   Type theta = v.Length();
00290 
00291   if (theta > FUDGE()) {
00292     scale = Sin(theta)/theta ;
00293     v *= scale;
00294   }
00295 
00296   s = Cos(theta) ; 
00297   return *this;
00298 }
00299 
00300 
00301 //----------------------------------------------------------------------------
00302 // SetAngle (radians)
00303 //----------------------------------------------------------------------------
00304 template <class Type>
00305 void Quat<Type>::SetAngle( Type f )
00306 {
00307   qvec axis(GetAxis());
00308   f *= Type(0.5);
00309   s = Cos( f );
00310   v = axis * Sin( f );
00311 }
00312 
00313 //----------------------------------------------------------------------------
00314 // ScaleAngle 
00315 //----------------------------------------------------------------------------
00316 template <class Type>
00317 inline void  Quat<Type>::ScaleAngle( Type f )
00318 {
00319   SetAngle( f * GetAngle() );
00320 }
00321 
00322 //----------------------------------------------------------------------------
00323 // GetAngle (radians)
00324 //----------------------------------------------------------------------------
00325 // get rot angle in radians.  Assumes s is between -1 and 1, which will always
00326 // be the case for unit quaternions.
00327 //----------------------------------------------------------------------------
00328 template <class Type>
00329 inline Type Quat<Type>::GetAngle( void ) const
00330 {
00331   return ( Type(2.0) * ACos( s ) );
00332 }
00333 
00334 //----------------------------------------------------------------------------
00335 // GetAxis
00336 //----------------------------------------------------------------------------
00337 template <class Type>
00338 Quat<Type>::qvec Quat<Type>::GetAxis( void ) const
00339 {
00340   Type scale;
00341 
00342   scale = Sin( acos( s ) ) ;
00343   if ( scale < FUDGE() && scale > -FUDGE() )
00344     return qvec( 0.0, 0.0, 0.0 );
00345   else
00346     return  v / scale;
00347 }
00348 
00349 //---------------------------------------------------------------------------- 
00350 // Print
00351 //---------------------------------------------------------------------------- 
00352 template <class Type>
00353 inline void Quat<Type>::Print( ) const
00354 {
00355   printf( "(%3.2f, <%3.2f %3.2f %3.2f>)\n", s, v.x, v.y, v.z );
00356 }       
00357 
00358 
00359 //============================================================================
00360 // CONVERSIONS
00361 //============================================================================
00362 
00363 template <class Type>
00364 Mat44<Type>& Quat<Type>::ToMat( Mat44<Type>& dest ) const
00365 {
00366   Type t, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
00367   qvec  a, c, b, d;
00368   
00369   t  = Type(2.0) / (v*v + s*s);
00370   const Type ONE(1.0);
00371   
00372   xs = v.x*t;   ys = v.y*t;   zs = v.z*t;
00373   wx = s*xs;    wy = s*ys;    wz = s*zs;
00374   xx = v.x*xs;  xy = v.x*ys;  xz = v.x*zs;
00375   yy = v.y*ys;  yz = v.y*zs;  zz = v.z*zs;
00376   
00377   dest.Set( ONE-(yy+zz), xy-wz,       xz+wy,       0.0,
00378             xy+wz,       ONE-(xx+zz), yz-wx,       0.0,
00379             xz-wy,       yz+wx,       ONE-(xx+yy), 0.0,
00380             0.0,         0.0,         0.0,         ONE );
00381   
00382   return dest;
00383 }
00384 
00385 template <class Type>
00386 Mat33<Type>&  Quat<Type>::ToMat( Mat33<Type>& dest ) const
00387 {
00388   Type t, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
00389   qvec  a, c, b, d;
00390   
00391   t  = Type(2.0) / Norm();
00392   const Type ONE(1.0);
00393   
00394   xs = v.x*t;   ys = v.y*t;   zs = v.z*t;
00395   wx = s*xs;    wy = s*ys;    wz = s*zs;
00396   xx = v.x*xs;  xy = v.x*ys;  xz = v.x*zs;
00397   yy = v.y*ys;  yz = v.y*zs;  zz = v.z*zs;
00398   
00399   dest.Set( ONE-(yy+zz), xy-wz,       xz+wy,       
00400             xy+wz,       ONE-(xx+zz), yz-wx,       
00401             xz-wy,       yz+wx,       ONE-(xx+yy) );
00402 
00403   return dest;
00404 }
00405 
00406 //----------------------------------------------------------------------------
00407 // FromMat
00408 //----------------------------------------------------------------------------
00409 // Convert rotation matrix to quaternion
00410 // Results will be bad if matrix is not (very close to) orthonormal
00411 // Modified from gamasutra.com article:
00412 // http://www.gamasutra.com/features/programming/19980703/quaternions_07.htm
00413 //----------------------------------------------------------------------------
00414 template <class Type>
00415 Quat<Type>&  Quat<Type>::FromMat( const Mat44<Type>& m )
00416 {
00417   Type tr = m.Trace();
00418 
00419   // check the diagonal
00420   if (tr > 0.0) {
00421     Type scale = Type( sqrt (tr) );
00422     s = scale * Type(0.5);
00423     scale = Type(0.5) / scale;
00424     v.x = (m(1,2) - m(2,1)) * scale;
00425     v.y = (m(2,0) - m(0,2)) * scale;
00426     v.z = (m(0,1) - m(1,0)) * scale;
00427   } else {              
00428     // diagonal is negative or zero
00429     int i, j, k;
00430     i = 0;
00431     if (m(1,1) > m(0,0)) i = 1;
00432     if (m(2,2) > m(i,i)) i = 2;
00433     int nxt[3] = {1, 2, 0};
00434     j = nxt[i];
00435     k = nxt[j];
00436 
00437     Type scale = Type( sqrt (Type(1.0) + m(i,i) - (m(j,j) + m(k,k)) ) );
00438       
00439     v[i] = scale * Type(0.5);
00440             
00441     if (scale != 0.0) scale = Type(0.5) / scale;
00442 
00443     s = (m(j,k) - m(k,j)) * scale;
00444     v[j] = (m(i,j) + m(j,i)) * scale;
00445     v[k] = (m(i,k) + m(k,i)) * scale;
00446   }
00447   return *this;
00448 }
00449 
00450 template <class Type>
00451 Quat<Type>&  Quat<Type>::FromMat( const Mat33<Type>& m )
00452 {
00453   Type tr = m.Trace();
00454 
00455   // check the diagonal
00456   if (tr > 0.0) {
00457     Type scale = Type( sqrt (tr + Type(1.0)) );
00458     s = scale * Type(0.5);
00459     scale = Type(0.5) / scale;
00460     v.x = (m(1,2) - m(2,1)) * scale;
00461     v.y = (m(2,0) - m(0,2)) * scale;
00462     v.z = (m(0,1) - m(1,0)) * scale;
00463   } else {              
00464     // diagonal is negative or zero
00465     int i, j, k;
00466     i = 0;
00467     if (m(1,1) > m(0,0)) i = 1;
00468     if (m(2,2) > m(i,i)) i = 2;
00469     int nxt[3] = {1, 2, 0};
00470     j = nxt[i];
00471     k = nxt[j];
00472 
00473     Type scale = Type( sqrt (Type(1.0) + m(i,i) - (m(j,j) + m(k,k)) ) );
00474       
00475     v[i] = scale * Type(0.5);
00476             
00477     if (scale != 0.0) scale = Type(0.5) / scale;
00478 
00479     s = (m(j,k) - m(k,j)) * scale;
00480     v[j] = (m(i,j) + m(j,i)) * scale;
00481     v[k] = (m(i,k) + m(k,i)) * scale;
00482   }
00483   return *this;
00484 }
00485 
00486 //----------------------------------------------------------------------------
00487 // ToAngleAxis (radians)
00488 //----------------------------------------------------------------------------
00489 // Convert to angle & axis representation
00490 //----------------------------------------------------------------------------
00491 template <class Type>
00492 void Quat<Type>::ToAngleAxis( Type &angle, qvec &axis ) const
00493 {
00494   Type cinv = ACos( s );
00495   angle = Type(2.0) * cinv;
00496 
00497   Type scale;
00498 
00499   scale = Sin( cinv );
00500   if ( scale < FUDGE() && scale > -FUDGE() )
00501     axis = qvec::ZERO;
00502   else {
00503     axis = v;
00504     axis /= scale;
00505   }
00506 }
00507 
00508 //----------------------------------------------------------------------------
00509 // FromAngleAxis (radians)
00510 //----------------------------------------------------------------------------
00511 // Convert to quat from angle & axis representation
00512 //----------------------------------------------------------------------------
00513 template <class Type>
00514 Quat<Type>& Quat<Type>::FromAngleAxis( Type angle, const qvec &axis )
00515 {
00516   /* normalize vector */
00517   Type length = axis.Length();
00518 
00519   /* if zero vector passed in, just set to identity quaternion */
00520   if ( length < FUDGE() )
00521   {
00522     *this = IDENTITY();
00523     return *this;
00524   }
00525   length = Type(1.0)/length;
00526   angle *= 0.5;
00527   v = axis;
00528   v *= length;
00529   v *= Sin(angle);
00530 
00531   s = Cos(angle);
00532   return *this;
00533 }
00534 
00535 //----------------------------------------------------------------------------
00536 // FromTwoVecs
00537 //----------------------------------------------------------------------------
00538 // Return the quat that rotates vector a into vector b
00539 //----------------------------------------------------------------------------
00540 template <class Type>
00541 Quat<Type>& Quat<Type>::FromTwoVecs(const qvec &a, const qvec& b)
00542 {
00543   qvec u1(a);
00544   qvec u2(b);
00545   double theta ;                                     /* angle of rotation about axis */
00546   double theta_complement ;
00547   double crossProductMagnitude ;
00548 
00549 
00550   // Normalize both vectors and take cross product to get rotation axis. 
00551   u1.Normalize();
00552   u2.Normalize();
00553   qvec axis( u1 ^ u2 );
00554 
00555 
00556   // | u1 X u2 | = |u1||u2|sin(theta)
00557   //
00558   // Since u1 and u2 are normalized, 
00559   //
00560   //  theta = arcsin(|axis|)
00561   crossProductMagnitude = axis.Length();
00562 
00563   // Occasionally, even though the vectors are normalized, the
00564   // magnitude will be calculated to be slightly greater than one.  If
00565   // this happens, just set it to 1 or asin() will barf.
00566   if( crossProductMagnitude > Type(1.0) )
00567     crossProductMagnitude = Type(1.0) ;
00568 
00569   // Take arcsin of magnitude of rotation axis to compute rotation
00570   // angle.  Since crossProductMagnitude=[0,1], we will have
00571   // theta=[0,pi/2].
00572   theta = ASin( crossProductMagnitude ) ;
00573   theta_complement = Type(3.14159265358979323846) - theta ;
00574 
00575   // If cos(theta) < 0, use complement of theta as rotation angle.
00576   if( u1 * u2 < 0.0 )
00577   {
00578     double tmp = theta;
00579     theta = theta_complement ;
00580     theta_complement = tmp;
00581   }
00582 
00583   // if angle is 0, just return identity quaternion
00584   if( theta < FUDGE() )
00585   {
00586     *this = IDENTITY();
00587   }
00588   else
00589   {
00590     if( theta_complement < FUDGE() )
00591     {
00592       // The two vectors are opposed.  Find some arbitrary axis vector.
00593       // First try cross product with x-axis if u1 not parallel to x-axis.
00594       if( (u1.y*u1.y + u1.z*u1.z) >= FUDGE() )
00595       {
00596         axis.Set( 0.0, u1.z, -u1.y ) ;
00597       }
00598       else
00599       {
00600         // u1 is parallel to to x-axis.  Use z-axis as axis of rotation.
00601         axis.Set(0.0, 0.0, 1.0);
00602       }
00603     }
00604 
00605     axis.Normalize();
00606     FromAngleAxis(Type(theta), axis);
00607     Normalize();
00608   }
00609   return *this;  
00610 }
00611 
00612 //----------------------------------------------------------------------------
00613 // FromEuler
00614 //----------------------------------------------------------------------------
00615 // converts 3 euler angles (in radians) to a quaternion
00616 //
00617 // angles are in radians; Assumes roll is rotation about X, pitch is
00618 // rotation about Y, yaw is about Z.  (So thinking of
00619 // Z as up) Assumes order of yaw, pitch, roll applied as follows:
00620 //
00621 //          p' = roll( pitch( yaw(p) ) )
00622 //
00623 // Where yaw, pitch, and roll are defined in the BODY coordinate sys.
00624 // In other words these are ZYX-relative (or XYZ-fixed) Euler Angles.
00625 //
00626 // For a complete Euler angle implementation that handles all 24 angle
00627 // sets, see "Euler Angle Conversion" by Ken Shoemake, in "Graphics
00628 // Gems IV", Academic Press, 1994
00629 //----------------------------------------------------------------------------
00630 template <class Type>
00631 Quat<Type>& Quat<Type>::FromEuler(Type yaw, Type pitch, Type roll)
00632 {
00633   Type  cosYaw, sinYaw, cosPitch, sinPitch, cosRoll, sinRoll;
00634   Type  half_roll, half_pitch, half_yaw;
00635 
00636   /* put angles into radians and divide by two, since all angles in formula
00637    *  are (angle/2)
00638    */
00639   const Type HALF(0.5);
00640   half_yaw   = yaw   * HALF;
00641   half_pitch = pitch * HALF;
00642   half_roll  = roll  * HALF;
00643 
00644   cosYaw = Cos(half_yaw);
00645   sinYaw = Sin(half_yaw);
00646 
00647   cosPitch = Cos(half_pitch);
00648   sinPitch = Sin(half_pitch);
00649 
00650   cosRoll = Cos(half_roll);
00651   sinRoll = Sin(half_roll);
00652 
00653   Type cpcy = cosPitch * cosYaw;
00654   Type spsy = sinPitch * sinYaw;
00655 
00656   v.x = sinRoll * cpcy              - cosRoll * spsy;
00657   v.y = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
00658   v.z = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
00659   s   = cosRoll * cpcy              + sinRoll * spsy;
00660 
00661   return *this;
00662 }
00663 
00664 //----------------------------------------------------------------------------
00665 // ToEuler
00666 //----------------------------------------------------------------------------
00667 // converts a quaternion to  3 euler angles (in radians)
00668 //
00669 // See FromEuler for details of which set of Euler Angles are returned
00670 //----------------------------------------------------------------------------
00671 template <class Type>
00672 void Quat<Type>::ToEuler(Type& yaw, Type& pitch, Type& roll) const
00673 {
00674   // This is probably wrong
00675   Mat33<Type> M;
00676   ToMat(M);
00677   const int i = 0, j = 1, k = 2;
00678   double cy = sqrt(M(i,i)*M(i,i) + M(i,j)*M(i,j));
00679   if (cy > FUDGE()) {
00680     roll = ATan2(M(j,k), M(k,k));
00681     pitch = ATan2(-M(i,k), cy);
00682     yaw = ATan2(M(i,j), M(i,i));
00683   } else {
00684     roll = ATan2(-M(k,j), M(j,j));
00685     pitch = ATan2(-M(i,k), cy);
00686     yaw = 0;
00687   }
00688 }
00689 
00690 //============================================================================
00691 // QUAT FRIENDS
00692 //============================================================================
00693 
00694 
00695 template <class Type>
00696 Quat<Type> operator + (const Quat<Type> &a, const Quat<Type> &b)
00697 {
00698   return Quat<Type>( a.s+b.s, a.v+b.v );
00699 }
00700 
00701 template <class Type>
00702 Quat<Type> operator - (const Quat<Type> &a, const Quat<Type> &b)
00703 {
00704   return Quat<Type>( a.s-b.s, a.v-b.v );
00705 }
00706 
00707 template <class Type>
00708 Quat<Type> operator - (const Quat<Type> &a )
00709 {
00710   return Quat<Type>( -a.s, -a.v );
00711 }
00712 
00713 template <class Type>
00714 Quat<Type> operator * ( const Quat<Type> &a, const Quat<Type> &b)
00715 {
00716 #if 0
00717   // 16 mults
00718   return Quat<Type>( a.s*b.s - a.v*b.v, a.s*b.v + b.s*a.v + a.v^b.v );
00719 #else 
00720   // optimized (12 mults, and no compiler-generated temp objects)
00721   Type A, B, C, D, E, F, G, H;
00722 
00723   A = (a.s   + a.v.x)*(b.s   + b.v.x);
00724   B = (a.v.z - a.v.y)*(b.v.y - b.v.z);
00725   C = (a.s   - a.v.x)*(b.v.y + b.v.z); 
00726   D = (a.v.y + a.v.z)*(b.s   - b.v.x);
00727   E = (a.v.x + a.v.z)*(b.v.x + b.v.y);
00728   F = (a.v.x - a.v.z)*(b.v.x - b.v.y);
00729   G = (a.s   + a.v.y)*(b.s   - b.v.z);
00730   H = (a.s   - a.v.y)*(b.s   + b.v.z);
00731 
00732   return Quat<Type>(
00733     B + (-E - F + G + H) * Type(0.5),
00734     A - (E + F + G + H) * Type(0.5), 
00735     C + (E - F + G - H) * Type(0.5), 
00736     D + (E - F - G + H) * Type(0.5));
00737 #endif
00738 }
00739 
00740 template <class Type>
00741 Quat<Type> operator * ( const Quat<Type> &a, const Type t)
00742 {
00743   return Quat<Type>( a.v * t, a.s * t );
00744 }
00745 
00746 template <class Type>
00747 Quat<Type> operator * ( const Type t, const Quat<Type> &a )
00748 {
00749   return Quat<Type>( a.v * t, a.s * t );
00750 }
00751 template <class Type>
00752 Quat<Type> operator / ( const Quat<Type> &a, const Type t )
00753 {
00754   return Quat<Type>( a.v / t, a.s / t );
00755 }
00756 
00757 template <class Type>
00758 bool operator == (const Quat<Type> &a, const Quat<Type> &b)
00759 {
00760   return (a.s == b.s && a.v == b.v);
00761 }
00762 template <class Type>
00763 bool operator != (const Quat<Type> &a, const Quat<Type> &b)
00764 {
00765   return (a.s != b.s || a.v != b.v);
00766 }
00767 
00768 
00769 //============================================================================
00770 // UTILS
00771 //============================================================================
00772 template <class Type>
00773 inline Type Quat<Type>::DEG2RAD(Type d)
00774 {
00775   return d * Type(0.0174532925199432957692369076848861);
00776 }
00777 template <class Type>
00778 inline Type Quat<Type>::RAD2DEG(Type d)
00779 {
00780   return d * Type(57.2957795130823208767981548141052);
00781 }
00782 
00783 template <class Type>
00784 inline Type Quat<Type>::Sin(double d)  { return Type(sin(d)); }
00785 template <class Type>
00786 inline Type Quat<Type>::Cos(double d)  { return Type(cos(d)); }
00787 template <class Type>
00788 inline Type Quat<Type>::ACos(double d) { return Type(acos(d)); }
00789 template <class Type>
00790 inline Type Quat<Type>::ASin(double d) { return Type(asin(d)); }
00791 template <class Type>
00792 inline Type Quat<Type>::ATan(double d) { return Type(atan(d)); }
00793 template <class Type>
00794 inline Type Quat<Type>::ATan2(double n, double d) {return Type(atan2(n,d));}
00795 
00796 template <class Type>
00797 inline const Quat<Type> Quat<Type>::ZERO() {return Quat(0,0,0,0); }
00798 template <class Type>
00799 inline const Quat<Type> Quat<Type>::IDENTITY() {return Quat(1,0,0,0); }
00800 
00801 template<class Type>
00802 inline const Type Quat<Type>::FUDGE()    { return 1e-6; }
00803 template<>
00804 inline const double Quat<double>::FUDGE() { return 1e-10; }
00805 
00806 //----------------------------------------------------------------------------
00807 // QuatSlerp
00808 //----------------------------------------------------------------------------
00809 template <class Type>
00810 Quat<Type>& QuatSlerp(
00811   Quat<Type> &dest, 
00812   const Quat<Type> &from, const Quat<Type> &to, Type t )
00813 {
00814   Quat<Type> to1;
00815   double omega, cosom, sinom, scale0, scale1;
00816 
00817   /* calculate cosine */
00818   cosom = from.v * to.v + from.s + to.s;
00819 
00820   /* Adjust signs (if necessary) so we take shorter path */
00821   if ( cosom < 0.0 ) {
00822     cosom = -cosom;
00823     to1 = -to;
00824   }
00825   else
00826   {
00827     to1 = to;
00828   }
00829 
00830   /* Calculate coefficients */
00831   if ((1.0 - cosom) > Quat<Type>::FUDGE ) {
00832     /* standard case (slerp) */
00833     omega = acos( cosom );
00834     sinom = sin( omega );
00835     scale0 = sin((1.0 - t) * omega) / sinom;
00836     scale1 = sin(t * omega) / sinom;
00837   }
00838   else {
00839     /* 'from' and 'to' are very close - just do linear interpolation */
00840     scale0 = 1.0 - t;
00841     scale1 = t;      
00842   }
00843 
00844   dest = from;
00845   dest *= Type(scale0);
00846   dest += Type(scale1) * to1;
00847   return dest;
00848 }
00849 
00850 // This version creates more temporary objects
00851 template <class Type>
00852 inline Quat<Type> QuatSlerp(
00853   const Quat<Type>& from, const Quat<Type>& to, Type t )
00854 {
00855   Quat<Type> q;
00856   return QuatSlerp(q, from, to, t);
00857 }
00858 
00859 

Generated at Fri Oct 12 15:12:21 2001 for GLVU by doxygen1.2.10 written by Dimitri van Heesch, © 1997-2001