Wm4Quaternion.h

Go to the documentation of this file.
00001 // Wild Magic Source Code
00002 // David Eberly
00003 // http://www.geometrictools.com
00004 // Copyright (c) 1998-2008
00005 //
00006 // This library is free software; you can redistribute it and/or modify it
00007 // under the terms of the GNU Lesser General Public License as published by
00008 // the Free Software Foundation; either version 2.1 of the License, or (at
00009 // your option) any later version.  The license is available for reading at
00010 // either of the locations:
00011 //     http://www.gnu.org/copyleft/lgpl.html
00012 //     http://www.geometrictools.com/License/WildMagicLicense.pdf
00013 //
00014 // Version: 4.0.2 (2008/07/29)
00015 
00016 #ifndef WM4QUATERNION_H
00017 #define WM4QUATERNION_H
00018 
00019 #include "Wm4FoundationLIB.h"
00020 #include "Wm4Matrix3.h"
00021 
00022 namespace Wm4
00023 {
00024 
00025 template <class Real>
00026 class Quaternion
00027 {
00028 public:
00029     // A quaternion is q = w + x*i + y*j + z*k where (w,x,y,z) is not
00030     // necessarily a unit length vector in 4D.
00031 
00032     // construction
00033     Quaternion ();  // uninitialized
00034     Quaternion (Real fW, Real fX, Real fY, Real fZ);
00035     Quaternion (const Quaternion& rkQ);
00036 
00037     // quaternion for the input rotation matrix
00038     Quaternion (const Matrix3<Real>& rkRot);
00039 
00040     // quaternion for the rotation of the axis-angle pair
00041     Quaternion (const Vector3<Real>& rkAxis, Real fAngle);
00042 
00043     // quaternion for the rotation matrix with specified columns
00044     Quaternion (const Vector3<Real> akRotColumn[3]);
00045 
00046     // member access:  0 = w, 1 = x, 2 = y, 3 = z
00047     inline operator const Real* () const;
00048     inline operator Real* ();
00049     inline Real operator[] (int i) const;
00050     inline Real& operator[] (int i);
00051     inline Real W () const;
00052     inline Real& W ();
00053     inline Real X () const;
00054     inline Real& X ();
00055     inline Real Y () const;
00056     inline Real& Y ();
00057     inline Real Z () const;
00058     inline Real& Z ();
00059 
00060     // assignment
00061     inline Quaternion& operator= (const Quaternion& rkQ);
00062 
00063     // comparison
00064     bool operator== (const Quaternion& rkQ) const;
00065     bool operator!= (const Quaternion& rkQ) const;
00066     bool operator<  (const Quaternion& rkQ) const;
00067     bool operator<= (const Quaternion& rkQ) const;
00068     bool operator>  (const Quaternion& rkQ) const;
00069     bool operator>= (const Quaternion& rkQ) const;
00070 
00071     // arithmetic operations
00072     inline Quaternion operator+ (const Quaternion& rkQ) const;
00073     inline Quaternion operator- (const Quaternion& rkQ) const;
00074     inline Quaternion operator* (const Quaternion& rkQ) const;
00075     inline Quaternion operator* (Real fScalar) const;
00076     inline Quaternion operator/ (Real fScalar) const;
00077     inline Quaternion operator- () const;
00078 
00079     // arithmetic updates
00080     inline Quaternion& operator+= (const Quaternion& rkQ);
00081     inline Quaternion& operator-= (const Quaternion& rkQ);
00082     inline Quaternion& operator*= (Real fScalar);
00083     inline Quaternion& operator/= (Real fScalar);
00084 
00085     // conversion between quaternions, matrices, and axis-angle
00086     Quaternion& FromRotationMatrix (const Matrix3<Real>& rkRot);
00087     void ToRotationMatrix (Matrix3<Real>& rkRot) const;
00088     Quaternion& FromRotationMatrix (const Vector3<Real> akRotColumn[3]);
00089     void ToRotationMatrix (Vector3<Real> akRotColumn[3]) const;
00090     Quaternion& FromAxisAngle (const Vector3<Real>& rkAxis, Real fAngle);
00091     void ToAxisAngle (Vector3<Real>& rkAxis, Real& rfAngle) const;
00092 
00093     // functions of a quaternion
00094     inline Real Length () const;  // length of 4-tuple
00095     inline Real SquaredLength () const;  // squared length of 4-tuple
00096     inline Real Dot (const Quaternion& rkQ) const;  // dot product of 4-tuples
00097     inline Real Normalize ();  // make the 4-tuple unit length
00098     Quaternion Inverse () const;  // apply to non-zero quaternion
00099     Quaternion Conjugate () const;
00100     Quaternion Exp () const;  // apply to quaternion with w = 0
00101     Quaternion Log () const;  // apply to unit-length quaternion
00102 
00103     // rotation of a vector by a quaternion
00104     Vector3<Real> Rotate (const Vector3<Real>& rkVector) const;
00105 
00106     // spherical linear interpolation
00107     Quaternion& Slerp (Real fT, const Quaternion& rkP, const Quaternion& rkQ);
00108 
00109     Quaternion& SlerpExtraSpins (Real fT, const Quaternion& rkP,
00110         const Quaternion& rkQ, int iExtraSpins);
00111 
00112     // intermediate terms for spherical quadratic interpolation
00113     Quaternion& Intermediate (const Quaternion& rkQ0,
00114         const Quaternion& rkQ1, const Quaternion& rkQ2);
00115 
00116     // spherical quadratic interpolation
00117     Quaternion& Squad (Real fT, const Quaternion& rkQ0,
00118         const Quaternion& rkA0, const Quaternion& rkA1,
00119         const Quaternion& rkQ1);
00120 
00121     // Compute a quaternion that rotates unit-length vector V1 to unit-length
00122     // vector V2.  The rotation is about the axis perpendicular to both V1 and
00123     // V2, with angle of that between V1 and V2.  If V1 and V2 are parallel,
00124     // any axis of rotation will do, such as the permutation (z2,x2,y2), where
00125     // V2 = (x2,y2,z2).
00126     Quaternion& Align (const Vector3<Real>& rkV1, const Vector3<Real>& rkV2);
00127 
00128     // Decompose a quaternion into q = q_twist * q_swing, where q is 'this'
00129     // quaternion.  If V1 is the input axis and V2 is the rotation of V1 by
00130     // q, q_swing represents the rotation about the axis perpendicular to
00131     // V1 and V2 (see Quaternion::Align), and q_twist is a rotation about V1.
00132     void DecomposeTwistTimesSwing (const Vector3<Real>& rkV1,
00133         Quaternion& rkTwist, Quaternion& rkSwing);
00134 
00135     // Decompose a quaternion into q = q_swing * q_twist, where q is 'this'
00136     // quaternion.  If V1 is the input axis and V2 is the rotation of V1 by
00137     // q, q_swing represents the rotation about the axis perpendicular to
00138     // V1 and V2 (see Quaternion::Align), and q_twist is a rotation about V1.
00139     void DecomposeSwingTimesTwist (const Vector3<Real>& rkV1,
00140         Quaternion& rkSwing, Quaternion& rkTwist);
00141 
00142     // *** Find closest quaternions with unconstrained angles.
00143 
00144     // Closest quaternion of the form (cx + sx*i).
00145     Quaternion GetClosestX () const;
00146 
00147     // Closest quaternion of the form (cy + sy*j).
00148     Quaternion GetClosestY () const;
00149 
00150     // Closest quaternion of the form (cz + sz*k).
00151     Quaternion GetClosestZ () const;
00152 
00153     // Closest quaternion of the form (cx + sx*i)*(cy + sy*j).
00154     Quaternion GetClosestXY () const;
00155 
00156     // Closest quaternion of the form (cy + sy*j)*(cx + sx*i).
00157     Quaternion GetClosestYX () const;
00158 
00159     // Closest quaternion of the form (cz + sz*k)*(cx + sx*i).
00160     Quaternion GetClosestZX () const;
00161 
00162     // Closest quaternion of the form (cx + sx*i)*(cz + sz*k).
00163     Quaternion GetClosestXZ () const;
00164 
00165     // Closest quaternion of the form (cy + sy*j)*(cz + sz*k).
00166     Quaternion GetClosestYZ () const;
00167 
00168     // Closest quaternion of the form (cz + sz*k)*(cy + sy*j).
00169     Quaternion GetClosestZY () const;
00170 
00171     // Factor to (cx + sx*i)*(cy + sy*j)*(cz + sz*k).
00172     void FactorXYZ (Real& rfCx, Real& rfSx, Real& rfCy, Real& rfSy,
00173         Real& rfCz, Real& rfSz);
00174 
00175     // Factor to (cx + sx*i)*(cz + sz*k)*(cy + sy*j).
00176     void FactorXZY (Real& rfCx, Real& rfSx, Real& rfCz, Real& rfSz,
00177         Real& rfCy, Real& rfSy);
00178 
00179     // Factor to (cy + sy*j)*(cz + sz*k)*(cx + sx*i).
00180     void FactorYZX (Real& rfCy, Real& rfSy, Real& rfCz, Real& rfSz,
00181         Real& rfCx, Real& rfSx);
00182 
00183     // Factor to (cy + sy*j)*(cx + sx*i)*(cz + sz*k).
00184     void FactorYXZ (Real& rfCy, Real& rfSy, Real& rfCx, Real& rfSx,
00185         Real& rfCz, Real& rfSz);
00186 
00187     // Factor to (cz + sz*k)*(cx + sx*i)*(cy + sy*j).
00188     void FactorZXY (Real& rfCz, Real& rfSz, Real& rfCx, Real& rfSx,
00189         Real& rfCy, Real& rfSy);
00190 
00191     // Factor to (cz + sz*k)*(cy + sy*j)*(cx + sx*i).
00192     void FactorZYX (Real& rfCz, Real& rfSz, Real& rfCy, Real& rfSy,
00193         Real& rfCx, Real& rfSx);
00194 
00195     // *** Find closest quaternions with constrained angles.
00196     class Constraints
00197     {
00198     public:
00199         Constraints ()
00200         {
00201             // Members are uninitialized.
00202         }
00203 
00204         Constraints (Real fMinAngle, Real fMaxAngle)
00205         {
00206             SetAngles(fMinAngle,fMaxAngle);
00207         }
00208 
00209         void SetAngles (Real fMinAngle, Real fMaxAngle)
00210         {
00211             m_fMinAngle = fMinAngle;
00212             m_fMaxAngle = fMaxAngle;
00213             m_fCosMinAngle = Math<Real>::Cos(m_fMinAngle);
00214             m_fSinMinAngle = Math<Real>::Sin(m_fMinAngle);
00215             m_fCosMaxAngle = Math<Real>::Cos(m_fMaxAngle);
00216             m_fSinMaxAngle = Math<Real>::Sin(m_fMaxAngle);
00217             m_fDiffCosMaxMin = m_fCosMaxAngle - m_fCosMinAngle;
00218             m_fDiffSinMaxMin = m_fSinMaxAngle - m_fSinMinAngle;
00219             Real fAvrAngle = ((Real)0.5)*(m_fMinAngle + m_fMaxAngle);
00220             m_fCosAvrAngle = Math<Real>::Cos(fAvrAngle);
00221             m_fSinAvrAngle = Math<Real>::Sin(fAvrAngle);
00222         }
00223 
00224         bool IsValid (Real fX, Real fY) const
00225         {
00226             // (x,y) must be unit-length.
00227 
00228             // Test whether (x,y) satisfies the constraints.
00229             Real fXm = fX - m_fCosMinAngle;
00230             Real fYm = fY - m_fSinMinAngle;
00231             if (fXm*m_fDiffSinMaxMin >= fYm*m_fDiffCosMaxMin)
00232             {
00233                 return true;
00234             }
00235 
00236             // Test whether (-x,-y) satisfies the constraints.
00237             Real fXp = fX + m_fCosMinAngle;
00238             Real fYp = fY + m_fSinMinAngle;
00239             if (fXp*m_fDiffSinMaxMin <= fYp*m_fDiffCosMaxMin)
00240             {
00241                 return true;
00242             }
00243 
00244             return false;
00245         }
00246 
00247         Real m_fMinAngle;       // in [-PI/2,PI/2]
00248         Real m_fMaxAngle;       // in [m_fMinAngle/2,PI/2]
00249         Real m_fCosMinAngle;    // = cos(m_fMinAngle)
00250         Real m_fSinMinAngle;    // = sin(m_fMinAngle)
00251         Real m_fCosMaxAngle;    // = cos(m_fMaxAngle)
00252         Real m_fSinMaxAngle;    // = sin(m_fMaxAngle)
00253         Real m_fDiffCosMaxMin;  // = cos(m_fMaxAngle) - cos(m_fMinAngle)
00254         Real m_fDiffSinMaxMin;  // = sin(m_fMaxAngle) - sin(m_fMinAngle)
00255         Real m_fCosAvrAngle;    // = cos((m_fMinAngle + m_fMaxAngle)/2)
00256         Real m_fSinAvrAngle;    // = sin((m_fMinAngle + mM_faxAngle)/2)
00257     };
00258 
00259     // Closest constrained quaternion of the form (cx + sx*i).
00260     Quaternion GetClosestX (const Constraints& rkXCon) const;
00261 
00262     // Closest constrained quaternion of the form (cy + sy*j).
00263     Quaternion GetClosestY (const Constraints& rkYCon) const;
00264 
00265     // Closest constrained quaternion of the form (cz + sz*k).
00266     Quaternion GetClosestZ (const Constraints& rkZCon) const;
00267 
00268     // Closest constrained quaternion of the form (cx + sx*i)*(cy + sy*j).
00269     Quaternion GetClosestXY (const Constraints& rkXCon,
00270         const Constraints& rkYCon) const;
00271 
00272     // Closest constrained quaternion of the form (cy + sy*j)*(cx + sx*i).
00273     Quaternion GetClosestYX (const Constraints& rkYCon,
00274         const Constraints& rkXCon) const;
00275 
00276     // Closest constrained quaternion of the form (cz + sz*k)*(cx + sx*i).
00277     Quaternion GetClosestZX (const Constraints& rkZCon,
00278         const Constraints& rkXCon) const;
00279 
00280     // Closest constrained quaternion of the form (cx + sx*i)*(cz + sz*k).
00281     Quaternion GetClosestXZ (const Constraints& rkXCon,
00282         const Constraints& rkZCon) const;
00283 
00284     // Closest constrained quaternion of the form (cz + sz*k)*(cy + sy*j).
00285     Quaternion GetClosestZY (const Constraints& rkZCon,
00286         const Constraints& rkYCon) const;
00287 
00288     // Closest constrained quaternion of the form (cy + sy*j)*(cz + sz*k).
00289     Quaternion GetClosestYZ (const Constraints& rkYCon,
00290         const Constraints& rkZCon) const;
00291 
00292     // special values
00293     WM4_FOUNDATION_ITEM static const Quaternion IDENTITY;
00294     WM4_FOUNDATION_ITEM static const Quaternion ZERO;
00295 
00296 private:
00297     // support for comparisons
00298     int CompareArrays (const Quaternion& rkQ) const;
00299 
00300     // Closest unconstrained quaternion of the form:
00301     //   (cx + sx*i) when iAxis = 1,
00302     //   (cy + sy*j) when iAxis = 2,
00303     //   (cz + sz*k) when iAxis = 3
00304     Quaternion GetClosest (int iAxis) const;
00305 
00306     // Closest constrained quaternion of the form:
00307     //   (cx + sx*i) when iAxis = 1,
00308     //   (cy + sy*j) when iAxis = 2,
00309     //   (cz + sz*k) when iAxis = 3
00310     Quaternion GetClosest (int iAxis, const Constraints& rkCon) const;
00311 
00312     // support for FromRotationMatrix
00313     WM4_FOUNDATION_ITEM static int ms_iNext[3];
00314 
00315     // support for closest quaternions
00316     WM4_FOUNDATION_ITEM static Real ms_fTolerance;
00317     WM4_FOUNDATION_ITEM static Real ms_fRootTwo;
00318     WM4_FOUNDATION_ITEM static Real ms_fRootHalf;
00319 
00320     Real m_afTuple[4];
00321 };
00322 
00323 template <class Real>
00324 inline Quaternion<Real> operator* (Real fScalar, const Quaternion<Real>& rkQ);
00325 
00326 #include "Wm4Quaternion.inl"
00327 
00328 typedef Quaternion<float> Quaternionf;
00329 typedef Quaternion<double> Quaterniond;
00330 
00331 }
00332 
00333 #endif

Generated on Fri Feb 13 13:58:10 2009 for meshmorph by  doxygen 1.5.1