GenericMathTemplateLibrary: QuatOps.h Source File

Go to the documentation of this file.
00001 // GMTL is (C) Copyright 2001-2010 by Allen Bierbaum
00002 // Distributed under the GNU Lesser General Public License 2.1 with an
00003 // addendum covering inlined code. (See accompanying files LICENSE and
00004 // LICENSE.addendum or http://www.gnu.org/copyleft/lesser.txt)
00005 
00006 #ifndef _GMTL_QUAT_OPS_H_
00007 #define _GMTL_QUAT_OPS_H_
00008 
00009 #include <gmtl/Math.h>
00010 #include <gmtl/Quat.h>
00011 
00012 namespace gmtl
00013 {
00025    template <typename DATA_TYPE>
00026    Quat<DATA_TYPE>& mult( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00027    {
00028       // Here is the easy to understand equation: (grassman product)
00029       // scalar_component = q1.s * q2.s - dot(q1.v, q2.v)
00030       // vector_component = q2.v * q1.s + q1.v * q2.s + cross(q1.v, q2.v)
00031 
00032       // Here is another version (euclidean product, just FYI)...
00033       // scalar_component = q1.s * q2.s + dot(q1.v, q2.v)
00034       // vector_component = q2.v * q1.s - q1.v * q2.s - cross(q1.v, q2.v)
00035 
00036       // Here it is, using vector algebra (grassman product)
00037       /*
00038       const float& w1( q1[Welt] ), w2( q2[Welt] );
00039       Vec3 v1( q1[Xelt], q1[Yelt], q1[Zelt] ), v2( q2[Xelt], q2[Yelt], q2[Zelt] );
00040 
00041       float w = w1 * w2 - v1.dot( v2 );
00042       Vec3 v = (w1 * v2) + (w2 * v1) + v1.cross( v2 );
00043 
00044       vec[Welt] = w;
00045       vec[Xelt] = v[0];
00046       vec[Yelt] = v[1];
00047       vec[Zelt] = v[2];
00048       */
00049 
00050       // Here is the same, only expanded... (grassman product)
00051       Quat<DATA_TYPE> temporary; // avoid aliasing problems...
00052       temporary[Xelt] = q1[Welt]*q2[Xelt] + q1[Xelt]*q2[Welt] + q1[Yelt]*q2[Zelt] - q1[Zelt]*q2[Yelt];
00053       temporary[Yelt] = q1[Welt]*q2[Yelt] + q1[Yelt]*q2[Welt] + q1[Zelt]*q2[Xelt] - q1[Xelt]*q2[Zelt];
00054       temporary[Zelt] = q1[Welt]*q2[Zelt] + q1[Zelt]*q2[Welt] + q1[Xelt]*q2[Yelt] - q1[Yelt]*q2[Xelt];
00055       temporary[Welt] = q1[Welt]*q2[Welt] - q1[Xelt]*q2[Xelt] - q1[Yelt]*q2[Yelt] - q1[Zelt]*q2[Zelt];
00056 
00057       // use a temporary, in case q1 or q2 is the same as self.
00058       result[Xelt] = temporary[Xelt];
00059       result[Yelt] = temporary[Yelt];
00060       result[Zelt] = temporary[Zelt];
00061       result[Welt] = temporary[Welt];
00062 
00063       // don't normalize, because it might not be rotation arithmetic we're doing
00064       // (only rotation quats have unit length)
00065       return result;
00066    }
00067 
00074    template <typename DATA_TYPE>
00075    Quat<DATA_TYPE> operator*( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00076    {
00077       // (grassman product - see mult() for discussion)
00078       // don't normalize, because it might not be rotation arithmetic we're doing
00079       // (only rotation quats have unit length)
00080       return Quat<DATA_TYPE>( q1[Welt]*q2[Xelt] + q1[Xelt]*q2[Welt] + q1[Yelt]*q2[Zelt] - q1[Zelt]*q2[Yelt],
00081                               q1[Welt]*q2[Yelt] + q1[Yelt]*q2[Welt] + q1[Zelt]*q2[Xelt] - q1[Xelt]*q2[Zelt],
00082                               q1[Welt]*q2[Zelt] + q1[Zelt]*q2[Welt] + q1[Xelt]*q2[Yelt] - q1[Yelt]*q2[Xelt],
00083                               q1[Welt]*q2[Welt] - q1[Xelt]*q2[Xelt] - q1[Yelt]*q2[Yelt] - q1[Zelt]*q2[Zelt] );
00084    }
00085 
00090    template <typename DATA_TYPE>
00091    Quat<DATA_TYPE>& operator*=( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q2 )
00092    {
00093       return mult( result, result, q2 );
00094    }
00095 
00101    template <typename DATA_TYPE>
00102    Quat<DATA_TYPE>& negate( Quat<DATA_TYPE>& result )
00103    {
00104       result[0] = -result[0];
00105       result[1] = -result[1];
00106       result[2] = -result[2];
00107       result[3] = -result[3];
00108       return result;
00109    }
00110 
00116    template <typename DATA_TYPE>
00117    Quat<DATA_TYPE> operator-( const Quat<DATA_TYPE>& quat )
00118    {
00119       return Quat<DATA_TYPE>( -quat[0], -quat[1], -quat[2], -quat[3] );
00120    }
00121 
00126    template <typename DATA_TYPE>
00127    Quat<DATA_TYPE>& mult( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q, DATA_TYPE s )
00128    {
00129       result[0] = q[0] * s;
00130       result[1] = q[1] * s;
00131       result[2] = q[2] * s;
00132       result[3] = q[3] * s;
00133       return result;
00134    }
00135 
00140    template <typename DATA_TYPE>
00141    Quat<DATA_TYPE> operator*( const Quat<DATA_TYPE>& q, DATA_TYPE s )
00142    {
00143       Quat<DATA_TYPE> temporary;
00144       return mult( temporary, q, s );
00145    }
00146 
00151    template <typename DATA_TYPE>
00152    Quat<DATA_TYPE>& operator*=( Quat<DATA_TYPE>& q, DATA_TYPE s )
00153    {
00154       return mult( q, q, s );
00155    }
00156 
00161    template <typename DATA_TYPE>
00162    Quat<DATA_TYPE>& div( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q1, Quat<DATA_TYPE> q2 )
00163    {
00164       // multiply q1 by the multiplicative inverse of the quaternion
00165       return mult( result, q1, invert( q2 ) );
00166    }
00167 
00172    template <typename DATA_TYPE>
00173    Quat<DATA_TYPE> operator/( const Quat<DATA_TYPE>& q1, Quat<DATA_TYPE> q2 )
00174    {
00175       return q1 * invert( q2 );
00176    }
00177 
00182    template <typename DATA_TYPE>
00183    Quat<DATA_TYPE>& operator/=( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q2 )
00184    {
00185       return div( result, result, q2 );
00186    }
00187 
00192    template <typename DATA_TYPE>
00193    Quat<DATA_TYPE>& div( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q, DATA_TYPE s )
00194    {
00195       result[0] = q[0] / s;
00196       result[1] = q[1] / s;
00197       result[2] = q[2] / s;
00198       result[3] = q[3] / s;
00199       return result;
00200    }
00201 
00206    template <typename DATA_TYPE>
00207    Quat<DATA_TYPE> operator/( const Quat<DATA_TYPE>& q, DATA_TYPE s )
00208    {
00209       Quat<DATA_TYPE> temporary;
00210       return div( temporary, q, s );
00211    }
00212 
00217    template <typename DATA_TYPE>
00218    Quat<DATA_TYPE>& operator/=( const Quat<DATA_TYPE>& q, DATA_TYPE s )
00219    {
00220       return div( q, q, s );
00221    }
00222 
00226    template <typename DATA_TYPE>
00227    Quat<DATA_TYPE>& add( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00228    {
00229       result[0] = q1[0] + q2[0];
00230       result[1] = q1[1] + q2[1];
00231       result[2] = q1[2] + q2[2];
00232       result[3] = q1[3] + q2[3];
00233       return result;
00234    }
00235 
00240    template <typename DATA_TYPE>
00241    Quat<DATA_TYPE> operator+( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00242    {
00243       Quat<DATA_TYPE> temporary;
00244       return add( temporary, q1, q2 );
00245    }
00246 
00251    template <typename DATA_TYPE>
00252    Quat<DATA_TYPE>& operator+=( Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00253    {
00254       return add( q1, q1, q2 );
00255    }
00256 
00260    template <typename DATA_TYPE>
00261    Quat<DATA_TYPE>& sub( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00262    {
00263       result[0] = q1[0] - q2[0];
00264       result[1] = q1[1] - q2[1];
00265       result[2] = q1[2] - q2[2];
00266       result[3] = q1[3] - q2[3];
00267       return result;
00268    }
00269 
00274    template <typename DATA_TYPE>
00275    Quat<DATA_TYPE> operator-( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00276    {
00277       Quat<DATA_TYPE> temporary;
00278       return sub( temporary, q1, q2 );
00279    }
00280 
00285    template <typename DATA_TYPE>
00286    Quat<DATA_TYPE>& operator-=( Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00287    {
00288       return sub( q1, q1, q2 );
00289    }
00290 
00297    template <typename DATA_TYPE>
00298    DATA_TYPE dot( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00299    {
00300       return DATA_TYPE( (q1[0] * q2[0]) +
00301                         (q1[1] * q2[1]) +
00302                         (q1[2] * q2[2]) +
00303                         (q1[3] * q2[3])  );
00304    }
00305 
00313    template <typename DATA_TYPE>
00314    DATA_TYPE lengthSquared( const Quat<DATA_TYPE>& q )
00315    {
00316       return dot( q, q );
00317    }
00318 
00325    template <typename DATA_TYPE>
00326    DATA_TYPE length( const Quat<DATA_TYPE>& q )
00327    {
00328       return Math::sqrt( lengthSquared( q ) );
00329    }
00330 
00336    template <typename DATA_TYPE>
00337    Quat<DATA_TYPE>& normalize( Quat<DATA_TYPE>& result )
00338    {
00339       DATA_TYPE l = length( result );
00340 
00341       // return if no magnitude (already as normalized as possible)
00342       if (l < (DATA_TYPE)0.0001)
00343          return result;
00344 
00345       DATA_TYPE l_inv = ((DATA_TYPE)1.0) / l;
00346       result[Xelt] *= l_inv;
00347       result[Yelt] *= l_inv;
00348       result[Zelt] *= l_inv;
00349       result[Welt] *= l_inv;
00350 
00351       return result;
00352    }
00353 
00363    template< typename DATA_TYPE >
00364    bool isNormalized( const Quat<DATA_TYPE>& q1, const DATA_TYPE eps = 0.0001f )
00365    {
00366       return Math::isEqual( lengthSquared( q1 ), DATA_TYPE(1), eps );
00367    }
00368 
00375    template <typename DATA_TYPE>
00376    Quat<DATA_TYPE>& conj( Quat<DATA_TYPE>& result )
00377    {
00378       result[Xelt] = -result[Xelt];
00379       result[Yelt] = -result[Yelt];
00380       result[Zelt] = -result[Zelt];
00381       return result;
00382    }
00383 
00389    template <typename DATA_TYPE>
00390    Quat<DATA_TYPE>& invert( Quat<DATA_TYPE>& result )
00391    {
00392       // from game programming gems p198
00393       // do result = conj( q ) / norm( q )
00394       conj( result );
00395 
00396       // return if norm() is near 0 (divide by 0 would result in NaN)
00397       DATA_TYPE l = lengthSquared( result );
00398       if (l < (DATA_TYPE)0.0001)
00399          return result;
00400 
00401       DATA_TYPE l_inv = ((DATA_TYPE)1.0) / l;
00402       result[Xelt] *= l_inv;
00403       result[Yelt] *= l_inv;
00404       result[Zelt] *= l_inv;
00405       result[Welt] *= l_inv;
00406       return result;
00407    }
00408 
00414    template <typename DATA_TYPE>
00415    Quat<DATA_TYPE>& exp( Quat<DATA_TYPE>& result )
00416    {
00417       DATA_TYPE len1, len2;
00418 
00419       len1 = Math::sqrt( result[Xelt] * result[Xelt] +
00420                          result[Yelt] * result[Yelt] +
00421                          result[Zelt] * result[Zelt] );
00422       if (len1 > (DATA_TYPE)0.0)
00423          len2 = Math::sin( len1 ) / len1;
00424       else
00425          len2 = (DATA_TYPE)1.0;
00426 
00427       result[Xelt] = result[Xelt] * len2;
00428       result[Yelt] = result[Yelt] * len2;
00429       result[Zelt] = result[Zelt] * len2;
00430       result[Welt] = Math::cos( len1 );
00431 
00432       return result;
00433    }
00434 
00439    template <typename DATA_TYPE>
00440    Quat<DATA_TYPE>& log( Quat<DATA_TYPE>& result )
00441    {
00442       DATA_TYPE length;
00443 
00444       length = Math::sqrt( result[Xelt] * result[Xelt] +
00445                            result[Yelt] * result[Yelt] +
00446                            result[Zelt] * result[Zelt] );
00447 
00448       // avoid divide by 0
00449       if (Math::isEqual( result[Welt], (DATA_TYPE)0.0, (DATA_TYPE)0.00001 ) == false)
00450          length = Math::aTan( length / result[Welt] );
00451       else
00452          length = Math::PI_OVER_2;
00453 
00454       result[Welt] = (DATA_TYPE)0.0;
00455       result[Xelt] = result[Xelt] * length;
00456       result[Yelt] = result[Yelt] * length;
00457       result[Zelt] = result[Zelt] * length;
00458       return result;
00459    }
00460    
00462    template <typename DATA_TYPE>
00463    void squad( Quat<DATA_TYPE>& result, DATA_TYPE t, const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2, const Quat<DATA_TYPE>& a, const Quat<DATA_TYPE>& b )
00464    {
00465       gmtlASSERT( false );
00466    }
00467 
00469    template <typename DATA_TYPE>
00470    void meanTangent( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2, const Quat<DATA_TYPE>& q3 )
00471    {
00472        gmtlASSERT( false );
00473    }
00474 
00475 
00476    
00496    template <typename DATA_TYPE>
00497    Quat<DATA_TYPE>& slerp( Quat<DATA_TYPE>& result, const DATA_TYPE t, const Quat<DATA_TYPE>& from, const Quat<DATA_TYPE>& to, bool adjustSign=true)
00498    {
00499       const Quat<DATA_TYPE>& p = from; // just an alias to match q
00500 
00501       // calc cosine theta
00502       DATA_TYPE cosom = dot( from, to );
00503 
00504       // adjust signs (if necessary)
00505       Quat<DATA_TYPE> q;
00506       if (adjustSign && (cosom < (DATA_TYPE)0.0))
00507       {
00508          cosom = -cosom;
00509          q[0] = -to[0];   // Reverse all signs
00510          q[1] = -to[1];
00511          q[2] = -to[2];
00512          q[3] = -to[3];
00513       }
00514       else
00515       {
00516          q = to;
00517       }
00518 
00519       // Calculate coefficients
00520       DATA_TYPE sclp, sclq;
00521       if (((DATA_TYPE)1.0 - cosom) > (DATA_TYPE)0.0001) // 0.0001 -> some epsillon
00522       {
00523          // Standard case (slerp)
00524          DATA_TYPE omega, sinom;
00525          omega = gmtl::Math::aCos( cosom ); // extract theta from dot product's cos theta
00526          sinom = gmtl::Math::sin( omega );
00527          sclp  = gmtl::Math::sin( ((DATA_TYPE)1.0 - t) * omega ) / sinom;
00528          sclq  = gmtl::Math::sin( t * omega ) / sinom;
00529       }
00530       else
00531       {
00532          // Very close, do linear interp (because it's faster)
00533          sclp = (DATA_TYPE)1.0 - t;
00534          sclq = t;
00535       }
00536 
00537       result[Xelt] = sclp * p[Xelt] + sclq * q[Xelt];
00538       result[Yelt] = sclp * p[Yelt] + sclq * q[Yelt];
00539       result[Zelt] = sclp * p[Zelt] + sclq * q[Zelt];
00540       result[Welt] = sclp * p[Welt] + sclq * q[Welt];
00541       return result;
00542    }
00543 
00553    template <typename DATA_TYPE>
00554    Quat<DATA_TYPE>& lerp( Quat<DATA_TYPE>& result, const DATA_TYPE t, const Quat<DATA_TYPE>& from, const Quat<DATA_TYPE>& to)
00555    {
00556       // just an alias to match q
00557       const Quat<DATA_TYPE>& p = from;
00558 
00559       // calc cosine theta
00560       DATA_TYPE cosom = dot( from, to );
00561 
00562       // adjust signs (if necessary)
00563       Quat<DATA_TYPE> q;
00564       if (cosom < (DATA_TYPE)0.0)
00565       {
00566          q[0] = -to[0];   // Reverse all signs
00567          q[1] = -to[1];
00568          q[2] = -to[2];
00569          q[3] = -to[3];
00570       }
00571       else
00572       {
00573          q = to;
00574       }
00575 
00576       // do linear interp
00577       DATA_TYPE sclp, sclq;
00578       sclp = (DATA_TYPE)1.0 - t;
00579       sclq = t;
00580 
00581       result[Xelt] = sclp * p[Xelt] + sclq * q[Xelt];
00582       result[Yelt] = sclp * p[Yelt] + sclq * q[Yelt];
00583       result[Zelt] = sclp * p[Zelt] + sclq * q[Zelt];
00584       result[Welt] = sclp * p[Welt] + sclq * q[Welt];
00585       return result;
00586    }
00587 
00598    template <typename DATA_TYPE>
00599    inline bool operator==( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00600    {
00601       return bool( q1[0] == q2[0] &&
00602                    q1[1] == q2[1] &&
00603                    q1[2] == q2[2] &&
00604                    q1[3] == q2[3]  );
00605    }
00606 
00610    template <typename DATA_TYPE>
00611    inline bool operator!=( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00612    {
00613       return !operator==( q1, q2 );
00614    }
00615 
00618    template <typename DATA_TYPE>
00619    bool isEqual( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2, DATA_TYPE tol = 0.0 )
00620    {
00621       return bool( Math::isEqual( q1[0], q2[0], tol ) &&
00622                    Math::isEqual( q1[1], q2[1], tol ) &&
00623                    Math::isEqual( q1[2], q2[2], tol ) &&
00624                    Math::isEqual( q1[3], q2[3], tol )    );
00625    }
00626 
00632    template <typename DATA_TYPE>
00633    bool isEquiv( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2, DATA_TYPE tol = 0.0 )
00634    {
00635       return bool( isEqual( q1, q2, tol ) || isEqual( q1, -q2, tol ) );
00636    }
00637    
00639 }
00640 
00641 #endif