AI_Quaternion.h

Go to the documentation of this file.
00001 #ifndef AI_QUATERNION_H
00002 #define AI_QUATERNION_H
00003 //-------------------------------------------------------------------
00015 #include <stdlib.h>
00016 #include <stdio.h>
00017 #include <math.h>
00018 #include "AI_Vector.h"
00019 
00020 class AI_Quaternion {
00021 public:
00022     float x,y,z,w;
00023 
00024     //-- constructors -----------------------------------------------
00025     AI_Quaternion()
00026         : x(0.0f), y(0.0f), z(0.0f), w(1.0f)
00027     {};
00028     AI_Quaternion(float _x, float _y, float _z, float _w)
00029         : x(_x), y(_y), z(_z), w(_w)
00030     {};
00031     AI_Quaternion(const AI_Quaternion& q)
00032         : x(q.x), y(q.y), z(q.z), w(q.w)
00033     {};
00034 
00035     //-- setting elements -------------------------------------------
00036     void set(float _x, float _y, float _z, float _w) {
00037         x = _x;
00038         y = _y;
00039         z = _z;
00040         w = _w;
00041     };
00042     void set(const AI_Quaternion& q) {
00043         x = q.x;
00044         y = q.y;
00045         z = q.z;
00046         w = q.w;
00047     };
00048 
00049     //-- misc operations --------------------------------------------
00050     void ident(void) {
00051         x = 0.0f;
00052         y = 0.0f;
00053         z = 0.0f;
00054         w = 1.0f;
00055     };
00056 
00057     void conjugate(void) {
00058         x = -x;
00059         y = -y;
00060         z = -z;
00061     };
00062 
00063     void scale(float s) {
00064         x *= s;
00065         y *= s;
00066         z *= s;
00067         w *= s;
00068     };
00069 
00070     float norm(void) {
00071         return x*x + y*y + z*z + w*w;
00072     };
00073 
00074     float magnitude(void) {
00075         float n = norm();
00076         if (n > 0.0f) return ai_sqrt(n);
00077         else          return 0.0f;
00078     };
00079 
00080     void invert(void) {
00081         float n = norm();
00082         if (n > 0.0f) scale(1.0f / norm());
00083         conjugate();
00084     };
00085 
00086     void normalize(void) {
00087         float l = magnitude();
00088         if (l > 0.0f) scale(1.0f / l);
00089         else          set(0.0f,0.0f,0.0f,1.0f);
00090     };
00091 
00092     //-- operators --------------------------------------------------
00093     bool operator==(const AI_Quaternion& q) {
00094         return ((x==q.x) && (y==q.y) && (z==q.z) && (w==q.w)) ? true : false;
00095     };
00096 
00097     bool operator!=(const AI_Quaternion& q) {
00098         return ((x!=q.x) || (y!=q.y) || (z!=q.z) || (w!=q.w)) ? true : false;
00099     };
00100 
00101     const AI_Quaternion& operator+=(const AI_Quaternion& q) {
00102         x += q.x;
00103         y += q.y;
00104         z += q.z;
00105         w += q.w;
00106         return *this;
00107     };
00108 
00109     const AI_Quaternion& operator-=(const AI_Quaternion& q) {
00110         x -= q.x;
00111         y -= q.y;
00112         z -= q.z;
00113         w -= q.w;
00114         return *this;
00115     };
00116 
00117     const AI_Quaternion& operator*=(const AI_Quaternion& q) {
00118         float qx = w*q.x + x*q.w + y*q.z - z*q.y;
00119         float qy = w*q.y + y*q.w + z*q.x - x*q.z;
00120         float qz = w*q.z + z*q.w + x*q.y - y*q.x;
00121         float qw = w*q.w - x*q.x - y*q.y - z*q.z;
00122         x = qx;
00123         y = qy;
00124         z = qz;
00125         w = qw;
00126         return *this;
00127     };
00128 
00130     AI_Vector3 rotate(const AI_Vector3& v) {
00131         AI_Quaternion q(v.x * w + v.z * y - v.y * z,
00132                      v.y * w + v.x * z - v.z * x,
00133                      v.z * w + v.y * x - v.x * y,
00134                      v.x * x + v.y * y + v.z * z);
00135 
00136         return AI_Vector3(w * q.x + x * q.w + y * q.z - z * q.y,
00137                        w * q.y + y * q.w + z * q.x - x * q.z,
00138                        w * q.z + z * q.w + x * q.y - y * q.x);
00139     };
00140 
00150     void set_from_axes( const AI_Vector3& from, const AI_Vector3& to )
00151     {
00152         AI_Vector3 c(from * to);
00153         set(c.x, c.y, c.z, from % to);
00154         w += 1.0f;      // reducing angle to halfangle
00155         if( w <= AI_TINY ) // angle close to PI
00156         {
00157             if ((from.z * from.z) > (from.x * from.x))
00158                 set(0, from.z, -from.y, w);
00159                 //from*AI_Vector3(1,0,0) 
00160             else 
00161                 set(from.y, -from.x, 0, w);
00162                 //from*AI_Vector3(0,0,1) 
00163         }
00164         normalize(); 
00165     }
00166 
00176     void set_from_axes2( const AI_Vector3& from, const AI_Vector3& to )
00177     {
00178         AI_Vector3 c(from * to);
00179         set(c.x, c.y, c.z, from % to);
00180         normalize();    // if "from" or "to" not unit, normalize quat
00181         w += 1.0f;      // reducing angle to halfangle
00182         if( w <= AI_TINY ) // angle close to PI
00183         {
00184             if ((from.z * from.z) > (from.x * from.x))
00185                 set(0, from.z, -from.y, w);
00186                 //from*AI_Vector3(1,0,0) 
00187             else 
00188                 set(from.y, -from.x, 0, w);
00189                 //from*AI_Vector3(0,0,1) 
00190         }
00191         normalize(); 
00192     }
00193 
00194     //-- convert from euler angles ----------------------------------
00195     void set_rotate_axis_angle(const AI_Vector3& v, float a) {
00196         float sin_a = ai_sin(a * 0.5f);
00197         float cos_a = ai_cos(a * 0.5f);
00198         x = v.x * sin_a;
00199         y = v.y * sin_a;
00200         z = v.z * sin_a;
00201         w = cos_a;
00202     };
00203 
00204     void set_rotate_x(float a) {
00205         float sin_a = ai_sin(a * 0.5f);
00206         float cos_a = ai_cos(a * 0.5f);
00207         x = sin_a;
00208         y = 0.0f;
00209         z = 0.0f;
00210         w = cos_a;
00211     };
00212 
00213     void set_rotate_y(float a) {
00214         float sin_a = ai_sin(a * 0.5f);
00215         float cos_a = ai_cos(a * 0.5f);
00216         x = 0.0f;
00217         y = sin_a;
00218         z = 0.0f;
00219         w = cos_a;
00220     };
00221 
00222     void set_rotate_z(float a) {
00223         float sin_a = ai_sin(a * 0.5f);
00224         float cos_a = ai_cos(a * 0.5f);
00225         x = 0.0f;
00226         y = 0.0f;
00227         z = sin_a;
00228         w = cos_a;
00229     };
00230 
00231     void set_rotate_xyz(float ax, float ay, float az) {
00232         AI_Quaternion qx, qy, qz;
00233         qx.set_rotate_x(ax);
00234         qy.set_rotate_y(ay);
00235         qz.set_rotate_z(az);
00236         *this = qx;
00237         *this *= qy;
00238         *this *= qz;
00239     };
00240 
00241     //--- fuzzy compare operators -----------------------------------
00242     bool isequal(const AI_Quaternion& v, float tol) const
00243     {
00244         if (fabs(v.x-x) > tol)      return false;
00245         else if (fabs(v.y-y) > tol) return false;
00246         else if (fabs(v.z-z) > tol) return false;
00247         else if (fabs(v.w-w) > tol) return false;
00248         return true;
00249     };
00250 
00251     //-- rotation interpolation, set this matrix to the -------------    
00252     //-- interpolated result of q0->q1 with l as interpolator -------
00253     void slerp(const AI_Quaternion& q0, const AI_Quaternion& q1, float l) 
00254     {
00255         float fScale1;
00256         float fScale2;
00257         AI_Quaternion A = q0;
00258         AI_Quaternion B = q1;
00259 
00260         // compute dot product, aka cos(theta):
00261         float fCosTheta = A.x*B.x + A.y*B.y + A.z*B.z + A.w*B.w;
00262 
00263         if (fCosTheta < 0.0f) 
00264         {
00265             // flip start AI_Quaternion
00266            A.x = -A.x; A.y = -A.y; A.z = -A.z; A.w = -A.w;
00267            fCosTheta = -fCosTheta;
00268         }
00269 
00270         if ((fCosTheta + 1.0f) > 0.05f) 
00271         {
00272             // If the quaternions are close, use linear interploation
00273             if ((1.0f - fCosTheta) < 0.05f) 
00274             {
00275                 fScale1 = 1.0f - l;
00276                 fScale2 = l;
00277             }
00278             else 
00279             { 
00280                 // Otherwise, do spherical interpolation
00281                 float fTheta    = ai_acos(fCosTheta);
00282                 float fSinTheta = ai_sin(fTheta);
00283                 fScale1 = ai_sin( fTheta * (1.0f-l) ) / fSinTheta;
00284                 fScale2 = ai_sin( fTheta * l ) / fSinTheta;
00285             }
00286         }
00287         else 
00288         {
00289             B.x = -A.y;
00290             B.y =  A.x;
00291             B.z = -A.w;
00292             B.w =  A.z;
00293             fScale1 = ai_sin( PI * (0.5f - l) );
00294             fScale2 = ai_sin( PI * l );
00295         }
00296 
00297         x = fScale1 * A.x + fScale2 * B.x;
00298         y = fScale1 * A.y + fScale2 * B.y;
00299         z = fScale1 * A.z + fScale2 * B.z;
00300         w = fScale1 * A.w + fScale2 * B.w;
00301     };
00302     
00303     void lerp(const AI_Quaternion& q0, const AI_Quaternion& q1, float l)
00304     {
00305         slerp(q0, q1, l);
00306     };
00307 };
00308 
00309 //--- global operators ----------------------------------------------
00310 static inline AI_Quaternion operator+(const AI_Quaternion& q0, const AI_Quaternion& q1) {
00311     return AI_Quaternion(q0.x+q1.x, q0.y+q1.y, q0.z+q1.z, q0.w+q1.w);
00312 };
00313 
00314 static inline AI_Quaternion operator-(const AI_Quaternion& q0, const AI_Quaternion& q1) {
00315     return AI_Quaternion(q0.x-q1.x, q0.y-q1.y, q0.z-q1.z, q0.w-q1.w);
00316 };
00317 
00318 static inline AI_Quaternion operator*(const AI_Quaternion& q0, const AI_Quaternion& q1) {
00319     return AI_Quaternion(q0.w*q1.x + q0.x*q1.w + q0.y*q1.z - q0.z*q1.y,
00320                       q0.w*q1.y + q0.y*q1.w + q0.z*q1.x - q0.x*q1.z,
00321                       q0.w*q1.z + q0.z*q1.w + q0.x*q1.y - q0.y*q1.x,
00322                       q0.w*q1.w - q0.x*q1.x - q0.y*q1.y - q0.z*q1.z);
00323 };
00324 
00325 
00326 
00327 
00328 //------------------------------------------------------------------------------
00331 template<>
00332 static inline
00333 void
00334 ai_lerp<AI_Quaternion>(AI_Quaternion & result, const AI_Quaternion & val0, const AI_Quaternion & val1, float lerpVal)
00335 {
00336     result.lerp(val0, val1, lerpVal);
00337 }
00338 
00339 
00340 //-------------------------------------------------------------------
00341 #endif