00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
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
00030
00031
00032
00033 Quaternion ();
00034 Quaternion (Real fW, Real fX, Real fY, Real fZ);
00035 Quaternion (const Quaternion& rkQ);
00036
00037
00038 Quaternion (const Matrix3<Real>& rkRot);
00039
00040
00041 Quaternion (const Vector3<Real>& rkAxis, Real fAngle);
00042
00043
00044 Quaternion (const Vector3<Real> akRotColumn[3]);
00045
00046
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
00061 inline Quaternion& operator= (const Quaternion& rkQ);
00062
00063
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
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
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
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
00094 inline Real Length () const;
00095 inline Real SquaredLength () const;
00096 inline Real Dot (const Quaternion& rkQ) const;
00097 inline Real Normalize ();
00098 Quaternion Inverse () const;
00099 Quaternion Conjugate () const;
00100 Quaternion Exp () const;
00101 Quaternion Log () const;
00102
00103
00104 Vector3<Real> Rotate (const Vector3<Real>& rkVector) const;
00105
00106
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
00113 Quaternion& Intermediate (const Quaternion& rkQ0,
00114 const Quaternion& rkQ1, const Quaternion& rkQ2);
00115
00116
00117 Quaternion& Squad (Real fT, const Quaternion& rkQ0,
00118 const Quaternion& rkA0, const Quaternion& rkA1,
00119 const Quaternion& rkQ1);
00120
00121
00122
00123
00124
00125
00126 Quaternion& Align (const Vector3<Real>& rkV1, const Vector3<Real>& rkV2);
00127
00128
00129
00130
00131
00132 void DecomposeTwistTimesSwing (const Vector3<Real>& rkV1,
00133 Quaternion& rkTwist, Quaternion& rkSwing);
00134
00135
00136
00137
00138
00139 void DecomposeSwingTimesTwist (const Vector3<Real>& rkV1,
00140 Quaternion& rkSwing, Quaternion& rkTwist);
00141
00142
00143
00144
00145 Quaternion GetClosestX () const;
00146
00147
00148 Quaternion GetClosestY () const;
00149
00150
00151 Quaternion GetClosestZ () const;
00152
00153
00154 Quaternion GetClosestXY () const;
00155
00156
00157 Quaternion GetClosestYX () const;
00158
00159
00160 Quaternion GetClosestZX () const;
00161
00162
00163 Quaternion GetClosestXZ () const;
00164
00165
00166 Quaternion GetClosestYZ () const;
00167
00168
00169 Quaternion GetClosestZY () const;
00170
00171
00172 void FactorXYZ (Real& rfCx, Real& rfSx, Real& rfCy, Real& rfSy,
00173 Real& rfCz, Real& rfSz);
00174
00175
00176 void FactorXZY (Real& rfCx, Real& rfSx, Real& rfCz, Real& rfSz,
00177 Real& rfCy, Real& rfSy);
00178
00179
00180 void FactorYZX (Real& rfCy, Real& rfSy, Real& rfCz, Real& rfSz,
00181 Real& rfCx, Real& rfSx);
00182
00183
00184 void FactorYXZ (Real& rfCy, Real& rfSy, Real& rfCx, Real& rfSx,
00185 Real& rfCz, Real& rfSz);
00186
00187
00188 void FactorZXY (Real& rfCz, Real& rfSz, Real& rfCx, Real& rfSx,
00189 Real& rfCy, Real& rfSy);
00190
00191
00192 void FactorZYX (Real& rfCz, Real& rfSz, Real& rfCy, Real& rfSy,
00193 Real& rfCx, Real& rfSx);
00194
00195
00196 class Constraints
00197 {
00198 public:
00199 Constraints ()
00200 {
00201
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
00227
00228
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
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;
00248 Real m_fMaxAngle;
00249 Real m_fCosMinAngle;
00250 Real m_fSinMinAngle;
00251 Real m_fCosMaxAngle;
00252 Real m_fSinMaxAngle;
00253 Real m_fDiffCosMaxMin;
00254 Real m_fDiffSinMaxMin;
00255 Real m_fCosAvrAngle;
00256 Real m_fSinAvrAngle;
00257 };
00258
00259
00260 Quaternion GetClosestX (const Constraints& rkXCon) const;
00261
00262
00263 Quaternion GetClosestY (const Constraints& rkYCon) const;
00264
00265
00266 Quaternion GetClosestZ (const Constraints& rkZCon) const;
00267
00268
00269 Quaternion GetClosestXY (const Constraints& rkXCon,
00270 const Constraints& rkYCon) const;
00271
00272
00273 Quaternion GetClosestYX (const Constraints& rkYCon,
00274 const Constraints& rkXCon) const;
00275
00276
00277 Quaternion GetClosestZX (const Constraints& rkZCon,
00278 const Constraints& rkXCon) const;
00279
00280
00281 Quaternion GetClosestXZ (const Constraints& rkXCon,
00282 const Constraints& rkZCon) const;
00283
00284
00285 Quaternion GetClosestZY (const Constraints& rkZCon,
00286 const Constraints& rkYCon) const;
00287
00288
00289 Quaternion GetClosestYZ (const Constraints& rkYCon,
00290 const Constraints& rkZCon) const;
00291
00292
00293 WM4_FOUNDATION_ITEM static const Quaternion IDENTITY;
00294 WM4_FOUNDATION_ITEM static const Quaternion ZERO;
00295
00296 private:
00297
00298 int CompareArrays (const Quaternion& rkQ) const;
00299
00300
00301
00302
00303
00304 Quaternion GetClosest (int iAxis) const;
00305
00306
00307
00308
00309
00310 Quaternion GetClosest (int iAxis, const Constraints& rkCon) const;
00311
00312
00313 WM4_FOUNDATION_ITEM static int ms_iNext[3];
00314
00315
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