rtc: rtc: rtcEulerAngles.h Source File

00001 /*
00002  * Copyright (C) 2009
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 .......: rtcEulerAngles.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_EULERANGLES_H
00020 #define RTC_EULERANGLES_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include "rtc/rtcMath.h"
00024 #include "rtc/rtcRotation.h"
00025 #include "rtc/rtcTransform.h"
00026 #include "rtc/rtcQuaternion.h"
00027 #include "rtc/rtcVec3.h"
00028 
00029 //== NAMESPACES ================================================================
00030 namespace rtc {
00031 
00032 // Forward declarations
00033 template <class T> class EulerAngles; // Euler angles
00034 template <class T> class Rotation; // Rotation matrix (3x3)
00035 template <class T> class Transform; // Rigid tranform matrix (4x4)
00036 template <class T> class Quaternion; // Quaternion
00037 
00074 template <class T>
00075 class EulerAngles: public Vec3<T> {
00076 public:
00078   EulerAngles();
00079   EulerAngles(const T roll, const T pitch, const T yaw);
00080   EulerAngles(const Vec<T,3>& v);
00081   EulerAngles(const Quaternion<T>& q);
00082   EulerAngles(const Rotation<T>& r);
00083 
00085   void set(const Vec<T,3>& v);
00086   void set(const T roll, const T pitch, const T yaw);
00087   void set(const Quaternion<T>& q);
00088   void set(const Rotation<T>& r);
00089   void bound();
00090 
00092   T yaw() const;
00093   T pitch() const;
00094   T roll() const;
00095   Vec3<T> toDegrees();
00096 
00098   using Vec3<T>::x;
00099 };
00100 
00101 // Declare a few common typdefs
00102 typedef EulerAngles<float> EulerAnglesf;
00103 typedef EulerAngles<double> EulerAnglesd;
00104 
00105 //==============================================================================
00106 // EulerAngles<T>
00107 //==============================================================================
00108 
00111 template <class T>
00112 inline EulerAngles<T>::EulerAngles() {
00113   set(T(0),T(0),T(0));
00114 }
00115 
00119 template <class T>
00120 inline EulerAngles<T>::EulerAngles(const T roll,
00121            const T pitch,
00122            const T yaw) {
00123   set(roll,pitch,yaw);
00124 }
00125 
00128 template <class T>
00129 inline EulerAngles<T>::EulerAngles(const Vec<T,3>& v) : Vec3<T>(v) {
00130   bound();
00131 }
00132 
00135 template <class T>
00136 inline EulerAngles<T>::EulerAngles(const Quaternion<T>& q) {
00137   set(q);
00138 }
00139 
00142 template <class T>
00143 inline EulerAngles<T>::EulerAngles(const Rotation<T>& r) {
00144   set(r);
00145 }
00146 
00147 // Mutators
00148 
00151 template <class T>
00152 inline void EulerAngles<T>::set(const Vec<T,3>& v) {
00153   Vec<T,3>::set(v);
00154   bound();
00155 }
00156 
00159 template <class T>
00160 inline void EulerAngles<T>::set(const T roll, const T pitch, const T yaw) {
00161   x[0] = roll; x[1] = pitch; x[2] = yaw;
00162   bound();
00163 }
00164 
00167 template <class T>
00168 inline void EulerAngles<T>::set(const Quaternion<T>& q) {
00169   set(atan2(T(2)*(q.x[2]*q.x[3] + q.x[0]*q.x[1]),q.x[0]*q.x[0] - q.x[1]*q.x[1] - q.x[2]*q.x[2] + q.x[3]*q.x[3]),
00170       asin(T(-2)*(q.x[1]*q.x[3] - q.x[0]*q.x[2])),
00171       atan2(T(2)*(q.x[1]*q.x[2] + q.x[0]*q.x[3]),q.x[0]*q.x[0] + q.x[1]*q.x[1] - q.x[2]*q.x[2] - q.x[3]*q.x[3]));
00172 }
00173 
00176 template <class T>
00177 inline void EulerAngles<T>::set(const Rotation<T>& r) {
00178   set((T)atan2(double(r.x[7]),double(r.x[8])),
00179       (T)asin(-double(r.x[6])),
00180       (T)atan2(double(r.x[3]),double(r.x[0])));
00181 }
00182 
00186 template <class T>
00187 inline void EulerAngles<T>::bound() {
00188   for (int i=0;i<3;i++) {
00189     x[i] = fmod(x[i],T(TWOPI));
00190     if (x[i] > T(PI)) x[i] -= T(TWOPI);
00191     if (x[i] < -T(PI)) x[i] += T(TWOPI);
00192   }
00193   if (rtc_abs(x[1]) > T(PI_2)) {
00194     x[0] -= rtc_sign(x[0])*T(PI);
00195     x[1] = rtc_sign(x[1])*(T(PI) - rtc_abs(x[1]));
00196     x[2] -= rtc_sign(x[2])*T(PI);
00197   }
00198 }
00199 
00202 template <class T>
00203 inline T EulerAngles<T>::roll() const { return x[0]; }
00204 
00207 template <class T>
00208 inline T EulerAngles<T>::pitch() const { return x[1]; }
00209 
00212 template <class T>
00213 inline T EulerAngles<T>::yaw() const { return x[2]; }
00214 
00217 template <class T>
00218 inline Vec3<T> EulerAngles<T>::toDegrees() {
00219   return Vec3<T>((T)R2D*x[0],(T)R2D*x[1],(T)R2D*x[2]);
00220 }
00221 
00222 //==============================================================================
00223 } // namespace rtc
00224 //==============================================================================
00225 #endif // RTC_EULERANGLES_H defined
00226 //==============================================================================