rtcRotation.h
Go to the documentation of this 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 .......: rtcRotation.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_ROTATION_H
00020 #define RTC_ROTATION_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include "rtc/rtcMath.h"
00024 #include "rtc/rtcEulerAngles.h"
00025 #include "rtc/rtcQuaternion.h"
00026 #include "rtc/rtcSMat3.h"
00027 
00028 //== NAMESPACES ================================================================
00029 namespace rtc {
00030 
00032 template <class T> class EulerAngles; // Euler angles
00033 template <class T> class Rotation; // Rotation matrix (3x3)
00034 template <class T> class Transform; // Rigid tranform matrix (4x4)
00035 template <class T> class Quaternion; // Quaternion
00036 
00074 template <class T>
00075 class Rotation: public SMat3<T> {
00076 public:
00078   Rotation();
00079   Rotation(const T x11, const T x12, const T x13,
00080            const T x21, const T x22, const T x23,
00081            const T x31, const T x32, const T x33);
00082   Rotation(const Mat<T,3,3>& m);
00083   Rotation(const Quaternion<T>& q);
00084   Rotation(const EulerAngles<T>& e);
00085 
00087   template <class U> Rotation(const Mat<U,3,3>& m);
00088 
00090   void set(const Quaternion<T>& q);
00091   void set(const EulerAngles<T>& e);
00092   void set(const T theta, const T phi);
00093 
00095   void apply(Vec3<T>& v) const;
00096   Vec3<T> apply(const Vec3<T>& v) const;
00097 
00099   using SMat3<T>::x;
00100   using SMat3<T>::set;
00101 };
00102 
00103 // Declare a few common typdefs
00104 typedef Rotation<float> Rotationf;
00105 typedef Rotation<double> Rotationd;
00106 
00107 //==============================================================================
00108 // Rotation<T>
00109 //==============================================================================
00110 
00111 // Constructors
00112 
00115 template <class T>
00116 inline Rotation<T>::Rotation() {
00117   x[0] = x[4] = x[8] = T(1);
00118   x[1] = x[2] = x[3] = x[5] = x[6] = x[7] = 0;
00119 }
00120 
00123 template <class T>
00124 inline Rotation<T>::Rotation(const T x11, const T x12, const T x13,
00125                              const T x21, const T x22, const T x23,
00126                              const T x31, const T x32, const T x33) {
00127   x[0] = x11; x[1] = x12; x[2] = x13;
00128   x[3] = x21; x[4] = x22; x[5] = x23;
00129   x[6] = x31; x[7] = x32; x[8] = x33;
00130 }
00131 
00134 template <class T>
00135 inline Rotation<T>::Rotation(const Mat<T,3,3>& m) : SMat3<T>(m) {}
00136 
00140 template <class T>
00141 inline Rotation<T>::Rotation(const Quaternion<T>& q) {
00142   set(q);
00143 }
00144 
00147 template <class T>
00148 inline Rotation<T>::Rotation(const EulerAngles<T>& e) {
00149   set(e);
00150 }
00151 
00154 template <class T> template <class U>
00155 inline Rotation<T>::Rotation(const Mat<U,3,3>& m) : SMat3<T>(m) {}
00156 
00157 // Mutators
00158 
00162 template <class T>
00163 inline void Rotation<T>::set(const Quaternion<T>& q) {
00164   x[0] = q.x[0]*q.x[0] + q.x[1]*q.x[1] - q.x[2]*q.x[2] - q.x[3]*q.x[3];
00165   x[1] = T(2)*(q.x[1]*q.x[2] - q.x[0]*q.x[3]);
00166   x[2] = T(2)*(q.x[1]*q.x[3] + q.x[0]*q.x[2]);
00167   x[3] = T(2)*(q.x[1]*q.x[2] + q.x[0]*q.x[3]);
00168   x[4] = q.x[0]*q.x[0] - q.x[1]*q.x[1] + q.x[2]*q.x[2] - q.x[3]*q.x[3];
00169   x[5] = T(2)*(q.x[2]*q.x[3] - q.x[0]*q.x[1]);
00170   x[6] = T(2)*(q.x[1]*q.x[3] - q.x[0]*q.x[2]);
00171   x[7] = T(2)*(q.x[2]*q.x[3] + q.x[0]*q.x[1]);
00172   x[8] = q.x[0]*q.x[0] - q.x[1]*q.x[1] - q.x[2]*q.x[2] + q.x[3]*q.x[3];
00173 }
00174 
00177 template <class T>
00178 inline void Rotation<T>::set(const EulerAngles<T>& e) {
00179   T roll = e.x[0]; T pitch = e.x[1]; T yaw = e.x[2];
00180   x[0] = cos(yaw)*cos(pitch);
00181   x[1] = cos(yaw)*sin(pitch)*sin(roll) - sin(yaw)*cos(roll);
00182   x[2] = cos(yaw)*sin(pitch)*cos(roll) + sin(yaw)*sin(roll);
00183 
00184   x[3] = sin(yaw)*cos(pitch);
00185   x[4] = sin(yaw)*sin(pitch)*sin(roll) + cos(yaw)*cos(roll);
00186   x[5] = sin(yaw)*sin(pitch)*cos(roll) - cos(yaw)*sin(roll);
00187 
00188   x[6] = -sin(pitch);
00189   x[7] = cos(pitch)*sin(roll);
00190   x[8] = cos(pitch)*cos(roll);
00191 }
00192 
00195 template <class T>
00196 inline void Rotation<T>::set(const T theta, const T phi) {
00197   Quaternion<T> q(Vec3f(cos(theta)*sin(phi),sin(theta)*sin(phi),cos(phi)).cross(Vec3f(1,0,0)),acos(cos(theta)*sin(phi)));
00198   set(q);
00199 }
00200 
00203 template <class T>
00204 inline void Rotation<T>::apply(Vec3<T>& v) const {
00205   v.set((*this)*v);
00206 }
00207 
00210 template <class T>
00211 inline Vec3<T> Rotation<T>::apply(const Vec3<T>& v) const {
00212   return((*this)*v);
00213 }
00214 
00215 //==============================================================================
00216 } // namespace rtc
00217 //==============================================================================
00218 #endif // RTC_ROTATION_H defined
00219 //==============================================================================
00220 


rtc
Author(s): Benjamin Pitzer
autogenerated on Thu Jan 2 2014 11:04:53