rtc: rtcQuaternion.h Source File

Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2008
00003  * Robert Bosch LLC
00004  * Research and Technology Center North America
00005  * Palo Alto, California
00006  *
00007  * All rights reserved.
00008  *
00009  *------------------------------------------------------------------------------
00010  * project ....: Autonomous Technologies
00011  * file .......: rtcQuaternion.h
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 08/16/2006
00015  * modified ...: $Date: 2009-01-21 18:19:16 -0800 (Wed, 21 Jan 2009) $
00016  * changed by .: $Author: benjaminpitzer $
00017  * revision ...: $Revision: 14 $
00018  */
00019 #ifndef RTC_QUATERNION_H
00020 #define RTC_QUATERNION_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include "rtc/rtcMath.h"
00024 #include "rtc/rtcEulerAngles.h"
00025 #include "rtc/rtcRotation.h"
00026 #include "rtc/rtcTransform.h"
00027 #include "rtc/rtcSMat3.h"
00028 #include "rtc/rtcSMat4.h"
00029 #include "rtc/rtcVec3.h"
00030 #include "rtc/rtcVec4.h"
00031 
00032 //== NAMESPACES ================================================================
00033 namespace rtc {
00034 
00035 // Forward declarations
00036 template <class T> class EulerAngles;   // Euler angles
00037 template <class T> class Rotation;      // Rotation matrix (3x3)
00038 template <class T> class Transform;     // Rigid tranform matrix (4x4)
00039 template <class T> class Quaternion;    // Quaternion
00040 template <class T> class SMat3;         // 3x3 Matrix
00041 template <class T> class SMat4;         // 4x4 Matrix
00042 template <class T> class Vec3;          // 3d Vector
00043 
00052 template <class T>
00053 class Quaternion: public Vec4<T> {
00054 public:
00055   // Inherited from parent
00056   using Vec4<T>::x;
00057   using Vec4<T>::set;
00058   using Vec4<T>::normalize;
00059   using Vec4<T>::operator *=;
00060   using Vec4<T>::operator *;
00061 
00063   Quaternion();
00064   Quaternion(const T w, const T x, const T y, const T z);
00065   Quaternion(const Vec<T,4>& v);
00066 
00067   Quaternion(const Vec<T,3>& n, const T theta); // axis angle
00068   Quaternion(const EulerAngles<T>& e);
00069   Quaternion(const Rotation<T>& r);
00070 
00072   void set(const Vec<T,3>& n, const T theta); // axis angle
00073   void set(const EulerAngles<T>& e);
00074   void set(const Rotation<T>& r);
00075   
00077   T angle() const;
00078   T w() const;
00079   Vec<T,3> axis() const;
00080 
00082   Quaternion<T> operator * (const Quaternion<T>& q) const;
00083   void operator *= (const Quaternion<T>& q);
00084   Quaternion<T> conjugated() const;
00085   void conjugate();
00086   Quaternion<T> inverted() const;
00087   void invert();
00088   T angle(const Quaternion<T>& q) const;
00089   Quaternion<T> slerp(const Quaternion<T>& q, const T& t) const;
00090 
00092   SMat3<T> drot_dqj(const int j) const;
00093   static SMat3<T> d2rot_dqkdqj(int k, int j);
00094   static SMat4<T> dquatMat_dqj(const int j);
00095   SMat4<T> quatMat();
00096   SMat4<T> quatMatT();
00097   SMat4<T> quatMatBar();
00098   SMat4<T> quatMatBarT();
00099 
00101   void rotate(Vec<T,3>& v) const;
00102   Vec<T,3> rotated(const Vec<T,3>& v) const;
00103 };
00104 
00105 // Declare a few common typdefs
00106 typedef Quaternion<float> Quaternionf;
00107 typedef Quaternion<double> Quaterniond;
00108 
00109 //==============================================================================
00110 // Quaternion<T>
00111 //==============================================================================
00112 
00113 // Constructors
00114 
00117 template <class T>
00118 inline Quaternion<T>::Quaternion() {
00119   set(T(1),T(0),T(0),T(0));
00120 }
00121 
00124 template <class T>
00125 inline Quaternion<T>::Quaternion(const T w, const T _x, const T y, const T z) {
00126   set(w,_x,y,z);
00127 }
00128 
00131 template <class T>
00132 inline Quaternion<T>::Quaternion(const Vec<T,4>& v) : Vec4<T>(v) {}
00133 
00138 template <class T>
00139 inline Quaternion<T>::Quaternion(const Vec<T,3>& n, const T theta) {
00140   set(n,theta);
00141 }
00142 
00146 template <class T>
00147 inline Quaternion<T>::Quaternion(const EulerAngles<T>& e) {
00148   set(e);
00149 }
00150 
00155 template <class T>
00156 inline Quaternion<T>::Quaternion(const Rotation<T>& r) {
00157   set(r);
00158 }
00159 
00160 // Mutators
00161 
00166 template <class T>
00167 inline void Quaternion<T>::set(const Vec<T,3>& n, const T theta) {
00168   T s = sin(theta/T(2));
00169   Vec<T,3> nn = n.normalized();
00170   set(cos(theta/T(2)),s*nn.x[0],s*nn.x[1],s*nn.x[2]);
00171   normalize();
00172 }
00173 
00177 template <class T>
00178 inline void Quaternion<T>::set(const EulerAngles<T>& e) {
00179   using namespace std;
00180   T cr = cos(T(0.5)*e.x[0]); T sr = sin(T(0.5)*e.x[0]);
00181   T cp = cos(T(0.5)*e.x[1]); T sp = sin(T(0.5)*e.x[1]);
00182   T cy = cos(T(0.5)*e.x[2]); T sy = sin(T(0.5)*e.x[2]);
00183   T w = cy*cp*cr + sy*sp*sr;
00184   if (w<0) set(-w,sy*sp*cr-cy*cp*sr,-cy*sp*cr-sy*cp*sr,cy*sp*sr-sy*cp*cr);
00185   else set(w,cy*cp*sr-sy*sp*cr,cy*sp*cr+sy*cp*sr,sy*cp*cr-cy*sp*sr);
00186 }
00187 
00192 template <class T>
00193 inline void Quaternion<T>::set(const Rotation<T>& r) {
00194   T n4;
00195   T tr = r.trace();
00196   if (tr > T(0)) {
00197     set(tr + T(1), -(r(1,2) - r(2,1)),
00198   -(r(2,0) - r(0,2)), -(r(0,1) - r(1,0)));
00199     n4 = x[0];
00200   } else if ((r(0,0) > r(1,1)) && (r(0,0) > r(2,2))) {
00201     set(r(1,2) - r(2,1), -(1.0f + r(0,0) - r(1,1) - r(2,2)),
00202   -(r(1,0) + r(0,1)), -(r(2,0) + r(0,2)));
00203     n4 = -x[1];
00204   } else if (r(1,1) > r(2,2)) {
00205     set(r(2,0) - r(0,2), -(r(1,0) + r(0,1)),
00206   -(1.0f + r(1,1) - r(0,0) - r(2,2)), -(r(2,1) + r(1,2)));
00207     n4 = -x[2];
00208   } else {
00209     set(r(0,1) - r(1,0), -(r(2,0) + r(0,2)), -(r(2,1) + r(1,2)),
00210   -(1.0f + r(2,2) - r(0,0) - r(1,1)));
00211     n4 = -x[3];
00212   }
00213   if (x[0] < T(0)) operator *= (T(-0.5)/T(sqrt(n4)));
00214   else operator *= (T(0.5)/T(sqrt(n4)));
00215 }
00216 
00222 template <class T>
00223 Quaternion<T> Quaternion<T>::slerp(const Quaternion<T>& q, const T& t) const 
00224 { 
00225   T theta = angle(q); 
00226   if (theta != T(0.0)) 
00227   { 
00228           T d = T(1.0) / rtc_sin(theta); 
00229           T s0 = rtc_sin((T(1.0) - t) * theta); 
00230           T s1 = rtc_sin(t * theta);   
00231           if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp 
00232             return Quaternion<T>((x[0] * s0 + -q.x[0] * s1) * d, 
00233                                  (x[1] * s0 + -q.x[1] * s1) * d, 
00234                                  (x[2] * s0 + -q.x[2] * s1) * d, 
00235                                  (x[3] * s0 + -q.x[3] * s1) * d); 
00236           else 
00237             return Quaternion<T>((x[0] * s0 + q.x[0] * s1) * d, 
00238                                  (x[1] * s0 + q.x[1] * s1) * d, 
00239                                  (x[2] * s0 + q.x[2] * s1) * d, 
00240                                  (x[3] * s0 + q.x[3] * s1) * d); 
00241           
00242   } 
00243   else 
00244   { 
00245           return *this; 
00246   } 
00247 } 
00248 
00249 // Accessors
00250 
00253 template <class T>
00254 inline T Quaternion<T>::w() const {
00255   return x[0];
00256 }
00257 
00260 template <class T>
00261 inline T Quaternion<T>::angle() const {
00262   return T(2.0)*acos(x[0]);
00263 }
00264 
00267 template <class T>
00268 inline Vec<T,3> Quaternion<T>::axis() const {
00269   Vec3<T> n(x[1],x[2],x[3]);
00270   n.normalize();
00271   return n;
00272 }
00273 
00274 // Quaternion operations
00275 
00278 template <class T>
00279 inline Quaternion<T> Quaternion<T>::operator*(const Quaternion<T>& q) const {
00280   return Quaternion<T>(x[0]*q.x[0] - x[1]*q.x[1] - x[2]*q.x[2] - x[3]*q.x[3],
00281                        x[0]*q.x[1] + x[1]*q.x[0] + x[2]*q.x[3] - x[3]*q.x[2],
00282                        x[0]*q.x[2] - x[1]*q.x[3] + x[2]*q.x[0] + x[3]*q.x[1],
00283                        x[0]*q.x[3] + x[1]*q.x[2] - x[2]*q.x[1] + x[3]*q.x[0]);
00284 }
00285 
00286 
00289 template <class T>
00290 inline void Quaternion<T>::operator *= (const Quaternion<T>& q) {
00291   set(x[0]*q.x[0] - x[1]*q.x[1] - x[2]*q.x[2] - x[3]*q.x[3],
00292 x[0]*q.x[1] + x[1]*q.x[0] + x[2]*q.x[3] - x[3]*q.x[2],
00293 x[0]*q.x[2] - x[1]*q.x[3] + x[2]*q.x[0] + x[3]*q.x[1],
00294 x[0]*q.x[3] + x[1]*q.x[2] - x[2]*q.x[1] + x[3]*q.x[0]);
00295 }
00296 
00300 template <class T>
00301 inline Quaternion<T> Quaternion<T>::conjugated() const {
00302   return Quaternion<T>(x[0],-x[1],-x[2],-x[3]);
00303 }
00304 
00308 template <class T>
00309 inline void Quaternion<T>::conjugate() {
00310   x[1] = -x[1]; x[2] = -x[2]; x[3] = -x[3];
00311 }
00312 
00316 template <class T>
00317 inline Quaternion<T> Quaternion<T>::inverted() const {
00318   T l2 = x[0]*x[0] + x[1]*x[1] + x[2]*x[2] + x[3]*x[3];
00319   return Quaternion<T>(x[0]/l2,-x[1]/l2,-x[2]/l2,-x[3]/l2);
00320 }
00321 
00325 template <class T>
00326 inline void Quaternion<T>::invert() {
00327   T l2 = x[0]*x[0] + x[1]*x[1] + x[2]*x[2] + x[3]*x[3];
00328   x[0] = x[0]/l2; x[1] = -x[1]/l2; x[2] = -x[2]/l2; x[3] = -x[3]/l2;
00329 }
00330 
00333 template <class T>
00334 inline T Quaternion<T>::angle(const Quaternion<T>& q) const
00335 {
00336   T s = rtc_sqrt(this->normSqr() * q.normSqr());
00337   if(s == T(0)) return 0.0; 
00338   T retval; 
00339   if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp 
00340     retval = rtc_acos(rtc_clamp(dot(-q) / s, (T)-1.0, (T)1.0)); //Clamped to prevent floating point precision crashes
00341 
00342   else 
00343     retval = rtc_acos(rtc_clamp(dot(q) / s, (T)-1.0, (T)1.0)); //Clamped to prevent floating point precision crashes
00344 
00345   return retval; 
00346 } 
00347 
00348 // Quaternion derivatives and matrices
00349 
00353 template <class T>
00354 inline SMat3<T> Quaternion<T>::drot_dqj(const int j) const {
00355   switch(j) {
00356   case 0: return SMat3<T>(2*x[0], -2*x[3],  2*x[2],
00357                           2*x[3],  2*x[0], -2*x[1],
00358                          -2*x[2], 2*x[1], 2*x[0]);
00359   case 1: return SMat3<T>( 2*x[1],  2*x[2],  2*x[3],
00360                            2*x[2], -2*x[1], -2*x[0],
00361                            2*x[3],  2*x[0], -2*x[1]);
00362   case 2: return SMat3<T>(-2*x[2],  2*x[1],  2*x[0],
00363                            2*x[1],   2*x[2],  2*x[3],
00364                           -2*x[0],  2*x[3], -2*x[2]);
00365   case 3: return SMat3<T>(-2*x[3], -2*x[0],  2*x[1],
00366                            2*x[0],  -2*x[3],  2*x[2],
00367                            2*x[1],   2*x[2],  2*x[3]);
00368   }
00369   return SMat3<T>(T(0));
00370 }
00371 
00375 template <class T>
00376 inline SMat3<T> Quaternion<T>::d2rot_dqkdqj(int k, int j) {
00377   if (j>k) { int temp = j; j=k; k=temp; }
00378   if (j==0 && k==0) return SMat3<T>(1,0,0,  0,1,0,  0,0,1);
00379   if (j==0 && k==1) return SMat3<T>(0,0,0,  0,0,-1,  0,1,0);
00380   if (j==0 && k==2) return SMat3<T>(0,0,1,  0,0,0,  -1,0,0);
00381   if (j==0 && k==3) return SMat3<T>(0,-1,0,  1,0,0,  0,0,0);
00382 
00383   if (j==1 && k==1) return SMat3<T>(1,0,0,  0,-1,0,  0,0,-1);
00384   if (j==1 && k==2) return SMat3<T>(0,1,0,  1,0,0,  0,0,0);
00385   if (j==1 && k==3) return SMat3<T>(0,0,1,  0,0,0,  1,0,0);
00386 
00387   if (j==2 && k==2) return SMat3<T>(-1,0,0,  0,1,0,  0,0,-1);
00388   if (j==2 && k==3) return SMat3<T>(0,0,0,  0,0,1,  0,1,0);
00389 
00390   if (j==3 && k==3) return SMat3<T>(-1,0,0,  0,-1,0,  0,0,1);
00391 
00392   return SMat3<T>(T(0));
00393 }
00394 
00398 template <class T>
00399 inline SMat4<T> Quaternion<T>::quatMat() {
00400   return SMat4<T>(x[0], -x[1], -x[2], -x[3],
00401       x[1],  x[0], -x[3],  x[2],
00402       x[2],  x[3],  x[0], -x[1],
00403       x[3], -x[2],  x[1],  x[0]);
00404 }
00405 
00408 template <class T>
00409 inline SMat4<T> Quaternion<T>::quatMatT() {
00410   return  SMat4<T>(x[0],   x[1],  x[2],  x[3],
00411        -x[1],  x[0],  x[3], -x[2],
00412        -x[2], -x[3],  x[0],  x[1],
00413        -x[3],  x[2], -x[1],  x[0]);
00414 }
00415 
00418 template <class T>
00419 inline SMat4<T> Quaternion<T>::quatMatBar() {
00420   return SMat4<T>(x[0], -x[1], -x[2], -x[3],
00421       x[1],  x[0],  x[3], -x[2],
00422       x[2], -x[3],  x[0],  x[1],
00423       x[3],  x[2], -x[1],  x[0]);
00424 }
00425 
00428 template <class T>
00429 inline SMat4<T> Quaternion<T>::quatMatBarT() {
00430   return  SMat4<T>(x[0],   x[1],  x[2],  x[3],
00431        -x[1],  x[0], -x[3],  x[2],
00432        -x[2],  x[3],  x[0], -x[1],
00433        -x[3], -x[2],  x[1],  x[0]);
00434 }
00435 
00438 template <class T>
00439 inline SMat4<T> Quaternion<T>::dquatMat_dqj(const int j) {
00440   switch(j) {
00441   case 0: return SMat4<T>(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1);
00442   case 1: return SMat4<T>(0,-1,0,0,1,0,0,0,0,0,0,-1,0,0,1,0);
00443   case 2: return SMat4<T>(0,0,-1,0,0,0,0,1,1,0,0,0,0,-1,0,0);
00444   case 4: return SMat4<T>(0,0,0,-1,0,0,-1,0,0,1,0,0,1,0,0,0);
00445   }
00446   return SMat4<T>(T(0));
00447 }
00448 
00451 template <class T>
00452 inline void Quaternion<T>::rotate(Vec<T,3>& v) const {
00453   T a = T(2)*x[0]*x[0] - T(1);
00454   T b = T(2)*(x[1]*v.x[0] + x[2]*v.x[1] + x[3]*v.x[2]);
00455   T u0 = a*v.x[0] + b*x[1] + T(2)*x[0]*(x[2]*v.x[2] - x[3]*v.x[1]);
00456   T u1 = a*v.x[1] + b*x[2] + T(2)*x[0]*(x[3]*v.x[0] - x[1]*v.x[2]);
00457   T u2 = a*v.x[2] + b*x[3] + T(2)*x[0]*(x[1]*v.x[1] - x[2]*v.x[0]);
00458   v.x[0] = u0; v.x[1] = u1; v.x[2] = u2;
00459 }
00460 
00463 template <class T>
00464 inline Vec<T,3> Quaternion<T>::rotated(const Vec<T,3>& v) const {
00465   T a = T(2)*x[0]*x[0] - T(1);
00466   T b = T(2)*(x[1]*v.x[0] + x[2]*v.x[1] + x[3]*v.x[2]);
00467   return Vec3<T>(a*v.x[0] + b*x[1] + 2*x[0]*(x[2]*v.x[2] - x[3]*v.x[1]),
00468       a*v.x[1] + b*x[2] + 2*x[0]*(x[3]*v.x[0] - x[1]*v.x[2]),
00469       a*v.x[2] + b*x[3] + 2*x[0]*(x[1]*v.x[1] - x[2]*v.x[0]));
00470 }
00471 
00472 //==============================================================================
00473 } // namespace rtc
00474 //==============================================================================
00475 #endif // RTC_QUATERNION_H defined
00476 //==============================================================================