QuatOps.h Source File

00001 /************************************************************** ggt-head beg
00002  *
00003  * GGT: Generic Graphics Toolkit
00004  *
00005  * Original Authors:
00006  *   Allen Bierbaum
00007  *
00008  * -----------------------------------------------------------------
00009  * File:          $RCSfile: QuatOps.h,v $
00010  * Date modified: $Date: 2003/03/29 23:22:17 $
00011  * Version:       $Revision: 1.24 $
00012  * -----------------------------------------------------------------
00013  *
00014  *********************************************************** ggt-head end */
00015 /*************************************************************** ggt-cpr beg
00016 *
00017 * GGT: The Generic Graphics Toolkit
00018 * Copyright (C) 2001,2002 Allen Bierbaum
00019 *
00020 * This library is free software; you can redistribute it and/or
00021 * modify it under the terms of the GNU Lesser General Public
00022 * License as published by the Free Software Foundation; either
00023 * version 2.1 of the License, or (at your option) any later version.
00024 *
00025 * This library is distributed in the hope that it will be useful,
00026 * but WITHOUT ANY WARRANTY; without even the implied warranty of
00027 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
00028 * Lesser General Public License for more details.
00029 *
00030 * You should have received a copy of the GNU Lesser General Public
00031 * License along with this library; if not, write to the Free Software
00032 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
00033 *
00034  ************************************************************ ggt-cpr end */
00035 #ifndef _GMTL_QUAT_OPS_H_
00036 #define _GMTL_QUAT_OPS_H_
00037 
00038 #include <gmtl/Math.h>
00039 #include <gmtl/Quat.h>
00040 
00041 namespace gmtl
00042 {
00054    template <typename DATA_TYPE>
00055    Quat<DATA_TYPE>& mult( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00056    {
00057       // Here is the easy to understand equation: (grassman product)
00058       // scalar_component = q1.s * q2.s - dot(q1.v, q2.v)
00059       // vector_component = q2.v * q1.s + q1.v * q2.s + cross(q1.v, q2.v)
00060 
00061       // Here is another version (euclidean product, just FYI)...
00062       // scalar_component = q1.s * q2.s + dot(q1.v, q2.v)
00063       // vector_component = q2.v * q1.s - q1.v * q2.s - cross(q1.v, q2.v)
00064 
00065       // Here it is, using vector algebra (grassman product)
00066       /*
00067       const float& w1( q1[Welt] ), w2( q2[Welt] );
00068       Vec3 v1( q1[Xelt], q1[Yelt], q1[Zelt] ), v2( q2[Xelt], q2[Yelt], q2[Zelt] );
00069 
00070       float w = w1 * w2 - v1.dot( v2 );
00071       Vec3 v = (w1 * v2) + (w2 * v1) + v1.cross( v2 );
00072 
00073       vec[Welt] = w;
00074       vec[Xelt] = v[0];
00075       vec[Yelt] = v[1];
00076       vec[Zelt] = v[2];
00077       */
00078 
00079       // Here is the same, only expanded... (grassman product)
00080       Quat<DATA_TYPE> temporary; // avoid aliasing problems...
00081       temporary[Xelt] = q1[Welt]*q2[Xelt] + q1[Xelt]*q2[Welt] + q1[Yelt]*q2[Zelt] - q1[Zelt]*q2[Yelt];
00082       temporary[Yelt] = q1[Welt]*q2[Yelt] + q1[Yelt]*q2[Welt] + q1[Zelt]*q2[Xelt] - q1[Xelt]*q2[Zelt];
00083       temporary[Zelt] = q1[Welt]*q2[Zelt] + q1[Zelt]*q2[Welt] + q1[Xelt]*q2[Yelt] - q1[Yelt]*q2[Xelt];
00084       temporary[Welt] = q1[Welt]*q2[Welt] - q1[Xelt]*q2[Xelt] - q1[Yelt]*q2[Yelt] - q1[Zelt]*q2[Zelt];
00085 
00086       // use a temporary, in case q1 or q2 is the same as self.
00087       result[Xelt] = temporary[Xelt];
00088       result[Yelt] = temporary[Yelt];
00089       result[Zelt] = temporary[Zelt];
00090       result[Welt] = temporary[Welt];
00091 
00092       // don't normalize, because it might not be rotation arithmetic we're doing
00093       // (only rotation quats have unit length)
00094       return result;
00095    }
00096 
00103    template <typename DATA_TYPE>
00104    Quat<DATA_TYPE> operator*( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00105    {
00106       // (grassman product - see mult() for discussion)
00107       // don't normalize, because it might not be rotation arithmetic we're doing
00108       // (only rotation quats have unit length)
00109       return Quat<DATA_TYPE>( q1[Welt]*q2[Xelt] + q1[Xelt]*q2[Welt] + q1[Yelt]*q2[Zelt] - q1[Zelt]*q2[Yelt],
00110                               q1[Welt]*q2[Yelt] + q1[Yelt]*q2[Welt] + q1[Zelt]*q2[Xelt] - q1[Xelt]*q2[Zelt],
00111                               q1[Welt]*q2[Zelt] + q1[Zelt]*q2[Welt] + q1[Xelt]*q2[Yelt] - q1[Yelt]*q2[Xelt],
00112                               q1[Welt]*q2[Welt] - q1[Xelt]*q2[Xelt] - q1[Yelt]*q2[Yelt] - q1[Zelt]*q2[Zelt] );
00113    }
00114 
00119    template <typename DATA_TYPE>
00120    Quat<DATA_TYPE>& operator*=( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q2 )
00121    {
00122       return mult( result, result, q2 );
00123    }
00124 
00130    template <typename DATA_TYPE>
00131    Quat<DATA_TYPE>& negate( Quat<DATA_TYPE>& result )
00132    {
00133       result[0] = -result[0];
00134       result[1] = -result[1];
00135       result[2] = -result[2];
00136       result[3] = -result[3];
00137       return result;
00138    }
00139 
00145    template <typename DATA_TYPE>
00146    Quat<DATA_TYPE> operator-( const Quat<DATA_TYPE>& quat )
00147    {
00148       return Quat<DATA_TYPE>( -quat[0], -quat[1], -quat[2], -quat[3] );
00149    }
00150 
00155    template <typename DATA_TYPE>
00156    Quat<DATA_TYPE>& mult( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q, DATA_TYPE s )
00157    {
00158       result[0] = q[0] * s;
00159       result[1] = q[1] * s;
00160       result[2] = q[2] * s;
00161       result[3] = q[3] * s;
00162       return result;
00163    }
00164 
00169    template <typename DATA_TYPE>
00170    Quat<DATA_TYPE> operator*( const Quat<DATA_TYPE>& q, DATA_TYPE s )
00171    {
00172       Quat<DATA_TYPE> temporary;
00173       return mult( temporary, q, s );
00174    }
00175 
00180    template <typename DATA_TYPE>
00181    Quat<DATA_TYPE>& operator*=( Quat<DATA_TYPE>& q, DATA_TYPE s )
00182    {
00183       return mult( q, q, s );
00184    }
00185 
00190    template <typename DATA_TYPE>
00191    Quat<DATA_TYPE>& div( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q1, Quat<DATA_TYPE> q2 )
00192    {
00193       // multiply q1 by the multiplicative inverse of the quaternion
00194       return mult( result, q1, invert( q2 ) );
00195    }
00196 
00201    template <typename DATA_TYPE>
00202    Quat<DATA_TYPE> operator/( const Quat<DATA_TYPE>& q1, Quat<DATA_TYPE> q2 )
00203    {
00204       return q1 * invert( q2 );
00205    }
00206 
00211    template <typename DATA_TYPE>
00212    Quat<DATA_TYPE>& operator/=( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q2 )
00213    {
00214       return div( result, result, q2 );
00215    }
00216 
00221    template <typename DATA_TYPE>
00222    Quat<DATA_TYPE>& div( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q, DATA_TYPE s )
00223    {
00224       result[0] = q[0] / s;
00225       result[1] = q[1] / s;
00226       result[2] = q[2] / s;
00227       result[3] = q[3] / s;
00228       return result;
00229    }
00230 
00235    template <typename DATA_TYPE>
00236    Quat<DATA_TYPE> operator/( const Quat<DATA_TYPE>& q, DATA_TYPE s )
00237    {
00238       Quat<DATA_TYPE> temporary;
00239       return div( temporary, q, s );
00240    }
00241 
00246    template <typename DATA_TYPE>
00247    Quat<DATA_TYPE>& operator/=( const Quat<DATA_TYPE>& q, DATA_TYPE s )
00248    {
00249       return div( q, q, s );
00250    }
00251 
00255    template <typename DATA_TYPE>
00256    Quat<DATA_TYPE>& add( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00257    {
00258       result[0] = q1[0] + q2[0];
00259       result[1] = q1[1] + q2[1];
00260       result[2] = q1[2] + q2[2];
00261       result[3] = q1[3] + q2[3];
00262       return result;
00263    }
00264 
00269    template <typename DATA_TYPE>
00270    Quat<DATA_TYPE> operator+( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00271    {
00272       Quat<DATA_TYPE> temporary;
00273       return add( temporary, q1, q2 );
00274    }
00275 
00280    template <typename DATA_TYPE>
00281    Quat<DATA_TYPE>& operator+=( Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00282    {
00283       return add( q1, q1, q2 );
00284    }
00285 
00289    template <typename DATA_TYPE>
00290    Quat<DATA_TYPE>& sub( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00291    {
00292       result[0] = q1[0] - q2[0];
00293       result[1] = q1[1] - q2[1];
00294       result[2] = q1[2] - q2[2];
00295       result[3] = q1[3] - q2[3];
00296       return result;
00297    }
00298 
00303    template <typename DATA_TYPE>
00304    Quat<DATA_TYPE> operator-( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00305    {
00306       Quat<DATA_TYPE> temporary;
00307       return sub( temporary, q1, q2 );
00308    }
00309 
00314    template <typename DATA_TYPE>
00315    Quat<DATA_TYPE>& operator-=( Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00316    {
00317       return sub( q1, q1, q2 );
00318    }
00319 
00326    template <typename DATA_TYPE>
00327    DATA_TYPE dot( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00328    {
00329       return DATA_TYPE( (q1[0] * q2[0]) +
00330                         (q1[1] * q2[1]) +
00331                         (q1[2] * q2[2]) +
00332                         (q1[3] * q2[3])  );
00333    }
00334 
00342    template <typename DATA_TYPE>
00343    DATA_TYPE lengthSquared( const Quat<DATA_TYPE>& q )
00344    {
00345       return dot( q, q );
00346    }
00347 
00354    template <typename DATA_TYPE>
00355    DATA_TYPE length( const Quat<DATA_TYPE>& q )
00356    {
00357       return Math::sqrt( lengthSquared( q ) );
00358    }
00359 
00365    template <typename DATA_TYPE>
00366    Quat<DATA_TYPE>& normalize( Quat<DATA_TYPE>& result )
00367    {
00368       DATA_TYPE l = length( result );
00369 
00370       // return if no magnitude (already as normalized as possible)
00371       if (l < (DATA_TYPE)0.0001)
00372          return result;
00373 
00374       DATA_TYPE l_inv = ((DATA_TYPE)1.0) / l;
00375       result[Xelt] *= l_inv;
00376       result[Yelt] *= l_inv;
00377       result[Zelt] *= l_inv;
00378       result[Welt] *= l_inv;
00379 
00380       return result;
00381    }
00382 
00392    template< typename DATA_TYPE >
00393    bool isNormalized( const Quat<DATA_TYPE>& q1, const DATA_TYPE eps = (DATA_TYPE)0.0001f )
00394    {
00395       return Math::isEqual( lengthSquared( q1 ), DATA_TYPE(1), eps );
00396    }
00397 
00404    template <typename DATA_TYPE>
00405    Quat<DATA_TYPE>& conj( Quat<DATA_TYPE>& result )
00406    {
00407       result[Xelt] = -result[Xelt];
00408       result[Yelt] = -result[Yelt];
00409       result[Zelt] = -result[Zelt];
00410       return result;
00411    }
00412 
00418    template <typename DATA_TYPE>
00419    Quat<DATA_TYPE>& invert( Quat<DATA_TYPE>& result )
00420    {
00421       // from game programming gems p198
00422       // do result = conj( q ) / norm( q )
00423       conj( result );
00424 
00425       // return if norm() is near 0 (divide by 0 would result in NaN)
00426       DATA_TYPE l = lengthSquared( result );
00427       if (l < (DATA_TYPE)0.0001)
00428          return result;
00429 
00430       DATA_TYPE l_inv = ((DATA_TYPE)1.0) / l;
00431       result[Xelt] *= l_inv;
00432       result[Yelt] *= l_inv;
00433       result[Zelt] *= l_inv;
00434       result[Welt] *= l_inv;
00435       return result;
00436    }
00437 
00443    template <typename DATA_TYPE>
00444    Quat<DATA_TYPE>& exp( Quat<DATA_TYPE>& result )
00445    {
00446       DATA_TYPE len1, len2;
00447 
00448       len1 = Math::sqrt( result[Xelt] * result[Xelt] +
00449                          result[Yelt] * result[Yelt] +
00450                          result[Zelt] * result[Zelt] );
00451       if (len1 > (DATA_TYPE)0.0)
00452          len2 = Math::sin( len1 ) / len1;
00453       else
00454          len2 = (DATA_TYPE)1.0;
00455 
00456       result[Xelt] = result[Xelt] * len2;
00457       result[Yelt] = result[Yelt] * len2;
00458       result[Zelt] = result[Zelt] * len2;
00459       result[Welt] = Math::cos( len1 );
00460 
00461       return result;
00462    }
00463 
00468    template <typename DATA_TYPE>
00469    Quat<DATA_TYPE>& log( Quat<DATA_TYPE>& result )
00470    {
00471       DATA_TYPE length;
00472 
00473       length = Math::sqrt( result[Xelt] * result[Xelt] +
00474                            result[Yelt] * result[Yelt] +
00475                            result[Zelt] * result[Zelt] );
00476 
00477       // avoid divide by 0
00478       if (Math::isEqual( result[Welt], (DATA_TYPE)0.0, (DATA_TYPE)0.00001 ) == false)
00479          length = Math::aTan( length / result[Welt] );
00480       else
00481          length = Math::PI_OVER_2;
00482 
00483       result[Welt] = (DATA_TYPE)0.0;
00484       result[Xelt] = result[Xelt] * length;
00485       result[Yelt] = result[Yelt] * length;
00486       result[Zelt] = result[Zelt] * length;
00487       return result;
00488    }
00489    
00491    template <typename DATA_TYPE>
00492    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 )
00493    {
00494       gmtlASSERT( false );
00495    }
00496 
00498    template <typename DATA_TYPE>
00499    void meanTangent( Quat<DATA_TYPE>& result, const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2, const Quat<DATA_TYPE>& q3 )
00500    {
00501        gmtlASSERT( false );
00502    }
00503 
00504 
00505    
00525    template <typename DATA_TYPE>
00526    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)
00527    {
00528       const Quat<DATA_TYPE>& p = from; // just an alias to match q
00529 
00530       // calc cosine theta
00531       DATA_TYPE cosom = dot( from, to );
00532 
00533       // adjust signs (if necessary)
00534       Quat<DATA_TYPE> q;
00535       if (adjustSign && (cosom < (DATA_TYPE)0.0))
00536       {
00537          cosom = -cosom;
00538          q[0] = -to[0];   // Reverse all signs
00539          q[1] = -to[1];
00540          q[2] = -to[2];
00541          q[3] = -to[3];
00542       }
00543       else
00544       {
00545          q = to;
00546       }
00547 
00548       // Calculate coefficients
00549       DATA_TYPE sclp, sclq;
00550       if (((DATA_TYPE)1.0 - cosom) > (DATA_TYPE)0.0001) // 0.0001 -> some epsillon
00551       {
00552          // Standard case (slerp)
00553          DATA_TYPE omega, sinom;
00554          omega = gmtl::Math::aCos( cosom ); // extract theta from dot product's cos theta
00555          sinom = gmtl::Math::sin( omega );
00556          sclp  = gmtl::Math::sin( ((DATA_TYPE)1.0 - t) * omega ) / sinom;
00557          sclq  = gmtl::Math::sin( t * omega ) / sinom;
00558       }
00559       else
00560       {
00561          // Very close, do linear interp (because it's faster)
00562          sclp = (DATA_TYPE)1.0 - t;
00563          sclq = t;
00564       }
00565 
00566       result[Xelt] = sclp * p[Xelt] + sclq * q[Xelt];
00567       result[Yelt] = sclp * p[Yelt] + sclq * q[Yelt];
00568       result[Zelt] = sclp * p[Zelt] + sclq * q[Zelt];
00569       result[Welt] = sclp * p[Welt] + sclq * q[Welt];
00570       return result;
00571    }
00572 
00582    template <typename DATA_TYPE>
00583    Quat<DATA_TYPE>& lerp( Quat<DATA_TYPE>& result, const DATA_TYPE t, const Quat<DATA_TYPE>& from, const Quat<DATA_TYPE>& to)
00584    {
00585       // just an alias to match q
00586       const Quat<DATA_TYPE>& p = from;
00587 
00588       // calc cosine theta
00589       DATA_TYPE cosom = dot( from, to );
00590 
00591       // adjust signs (if necessary)
00592       Quat<DATA_TYPE> q;
00593       if (cosom < (DATA_TYPE)0.0)
00594       {
00595          q[0] = -to[0];   // Reverse all signs
00596          q[1] = -to[1];
00597          q[2] = -to[2];
00598          q[3] = -to[3];
00599       }
00600       else
00601       {
00602          q = to;
00603       }
00604 
00605       // do linear interp
00606       DATA_TYPE sclp, sclq;
00607       sclp = (DATA_TYPE)1.0 - t;
00608       sclq = t;
00609 
00610       result[Xelt] = sclp * p[Xelt] + sclq * q[Xelt];
00611       result[Yelt] = sclp * p[Yelt] + sclq * q[Yelt];
00612       result[Zelt] = sclp * p[Zelt] + sclq * q[Zelt];
00613       result[Welt] = sclp * p[Welt] + sclq * q[Welt];
00614       return result;
00615    }
00616 
00627    template <typename DATA_TYPE>
00628    inline bool operator==( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00629    {
00630       return bool( q1[0] == q2[0] &&
00631                    q1[1] == q2[1] &&
00632                    q1[2] == q2[2] &&
00633                    q1[3] == q2[3]  );
00634    }
00635 
00639    template <typename DATA_TYPE>
00640    inline bool operator!=( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2 )
00641    {
00642       return !operator==( q1, q2 );
00643    }
00644 
00647    template <typename DATA_TYPE>
00648    bool isEqual( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2, DATA_TYPE tol = 0.0 )
00649    {
00650       return bool( Math::isEqual( q1[0], q2[0], tol ) &&
00651                    Math::isEqual( q1[1], q2[1], tol ) &&
00652                    Math::isEqual( q1[2], q2[2], tol ) &&
00653                    Math::isEqual( q1[3], q2[3], tol )    );
00654    }
00655 
00661    template <typename DATA_TYPE>
00662    bool isEquiv( const Quat<DATA_TYPE>& q1, const Quat<DATA_TYPE>& q2, DATA_TYPE tol = 0.0 )
00663    {
00664       return bool( isEqual( q1, q2, tol ) || isEqual( q1, -q2, tol ) );
00665    }
00666    
00668 }
00669 
00670 #endif