rtcTransform.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 .......: rtcTransform.h
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 08/16/2006
00015  * modified ...: $Date: 2009-03-19 15:02:17 -0700 (Thu, 19 Mar 2009) $
00016  * changed by .: $Author: wg75pal $
00017  * revision ...: $Revision: 103 $
00018  */
00019 #ifndef RTC_TRANSFORM_H
00020 #define RTC_TRANSFORM_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include "rtc/rtcMath.h"
00024 #include "rtc/rtcRotation.h"
00025 #include "rtc/rtcEulerAngles.h"
00026 #include "rtc/rtcQuaternion.h"
00027 #include "rtc/rtcVec3.h"
00028 #include "rtc/rtcSMat4.h"
00029 
00030 //== NAMESPACES ================================================================
00031 namespace rtc {
00032 
00033 // Forward declarations
00034 template <class T> class EulerAngles;   // Euler angles
00035 template <class T> class Rotation;      // Rotation matrix (3x3)
00036 template <class T> class Transform;     // Rigid tranform matrix (4x4)
00037 template <class T> class Quaternion;    // Quaternion
00038 template <class T> class Vec3;          // 3d Vector
00039 
00047 template <class T>
00048 class Transform: public SMat4<T> {
00049 public:
00050   // Constructors
00051   Transform();
00052   Transform(const Mat<T,4,4>& m);
00053   Transform(const Rotation<T>& rot);
00054   Transform(const Vec3<T>& trans);
00055   Transform(const Rotation<T>& rot, const Vec3<T>& trans);
00056 
00057   // Accessors
00058   void get( Rotation<T>& rot, Vec3<T>& trans) const;
00059   Vec3<T> getTranslation() const;
00060   Rotation<T> getRotation() const;
00061 
00062   // Mutators
00063   void setRotation(const Rotation<T>& rot);
00064   void setTranslation(const Vec3<T>& trans);
00065   void set(const Rotation<T>& rot, const Vec3<T>& trans);
00066   void rotateX(T theta);
00067   void rotateY(T theta);
00068   void rotateZ(T theta);
00069   void translate(T x, T y, T z);
00070   void translate(const Vec3<T>& t);
00071 
00072   // Helper for applying tranform to points: Transform * Vec<T,3>
00073   Vec3<T>& apply(Vec3<T>& v) const;
00074   Vec3<T> apply(const Vec3<T>& v) const;
00075 
00076 
00077   //Special inverse that uses the fact this is a rigid transform matrix
00078   Transform<T> inverted() const;
00079 
00080   // calculate a relative transformation inv(referenceFrame)*this
00081   Transform<T> relativeTo(const Transform<T>& referenceFrame) const;
00082 
00083   //TODO: figure out how to make << a friend and make this protected
00084   bool verifyTag(std::string got, std::string expected);
00085 
00086   // Inherited from parent
00087   using SMat<T,4>::set;
00088   using SMat<T,4>::x;
00089 };
00090 
00091 // Declare a few common typdefs
00092 typedef Transform<float> Transformf;
00093 typedef Transform<double> Transformd;
00094 
00095 // ASCII stream IO
00096 template <class T> std::ostream& operator<<(std::ostream& os, const Transform<T>& trans);
00097 template <class T> std::istream& operator>>(std::istream& is, Transform<T>& trans);
00098 
00099 //==============================================================================
00100 // Transform<T>
00101 //==============================================================================
00102 
00105 template <class T>
00106 inline Transform<T>::Transform() {
00107   set(Rotation<T>(),Vec<T,3>(T(0)));
00108 }
00109 
00112 template <class T>
00113 inline Transform<T>::Transform(const Rotation<T>& r){
00114   set(r,Vec3<T>(T(0)));
00115 }
00116 
00119 template <class T>
00120 inline Transform<T>::Transform(const Vec3<T>& t){
00121   set(Rotation<T>(),t);
00122 }
00123 
00126 template <class T>
00127 inline Transform<T>::Transform(const Rotation<T>& rot, const Vec3<T>& trans){
00128   set(rot,trans);
00129 }
00130 
00133 template <class T>
00134 inline Transform<T>::Transform(const Mat<T,4,4>& m) : SMat4<T>(m) {}
00135 
00136 
00137 // Accessors
00138 
00141 template <class T>
00142 inline void Transform<T>::get(Rotation<T>& r, Vec3<T>& t) const{
00143   r=Rotation<T>(x[0],x[1],x[2],
00144                 x[4],x[5],x[6],
00145                 x[8],x[9],x[10]);
00146   t=Vec3<T>(x[3],x[7],x[11]);
00147 }
00148 
00151 template <class T>
00152 inline Vec3<T> Transform<T>::getTranslation() const{
00153 
00154   return Vec3<T>(x[3],x[7],x[11]);
00155 }
00156 
00159 template <class T>
00160 inline  Rotation<T> Transform<T>::getRotation() const{
00161   return Rotation<T>(x[0],x[1],x[2],
00162                 x[4],x[5],x[6],
00163                 x[8],x[9],x[10]);
00164 }
00165 
00166 // Mutators
00167 
00170 template <class T>
00171 inline void Transform<T>::setRotation(const Rotation<T>& r) {
00172   for (int i=0;i<3;i++) for (int j=0;j<3;j++) (*this)(i,j) = r(i,j);
00173   x[12] = 0; x[13] = 0; x[14] = 0; x[15] = 1;
00174 }
00175 
00178 template <class T>
00179 inline void Transform<T>::setTranslation(const Vec3<T>& t) {
00180   x[3] = t(0); x[7] = t(1); x[11] = t(2);
00181   x[12] = 0; x[13] = 0; x[14] = 0; x[15] = 1;
00182 }
00183 
00186 template <class T>
00187 inline void Transform<T>::set(const Rotation<T>& r, const Vec3<T>& t) {
00188   setRotation(r);
00189   setTranslation(t);
00190 }
00191 
00194 template <class T>
00195 inline void Transform<T>::rotateX(T theta)
00196 {
00197   Transform<T> temp;
00198   T ctheta = cos(theta), stheta = sin(theta);
00199   temp(1,1) = ctheta;
00200   temp(1,2) = -stheta;
00201   temp(2,1) = stheta;
00202   temp(2,2) = ctheta;
00203   leftMultiply(temp);
00204 }
00205 
00208 template <class T>
00209 inline void Transform<T>::rotateY(T theta)
00210 {
00211   Transform<T> temp;
00212   T ctheta = cos(theta), stheta = sin(theta);
00213   temp(0,0) = ctheta;
00214   temp(0,2) = stheta;
00215   temp(2,0) = -stheta;
00216   temp(2,2) = ctheta;
00217   leftMultiply(temp);
00218 }
00219 
00222 template <class T>
00223 inline void Transform<T>::rotateZ(T theta)
00224 {
00225   Transform<T> temp;
00226   T ctheta = cos(theta), stheta = sin(theta);
00227   temp(0,0) = ctheta;
00228   temp(0,1) = -stheta;
00229   temp(1,0) = stheta;
00230   temp(1,1) = ctheta;
00231   leftMultiply(temp);
00232 }
00233 
00236 template <class T>
00237 inline void Transform<T>::translate(T _x, T _y, T _z)
00238 {
00239   x[3] += _x;
00240   x[7] += _y;
00241   x[11] += _z;
00242 }
00243 
00246 template <class T>
00247 inline void Transform<T>::translate(const Vec3<T>& t)
00248 {
00249   translate(t[0],t[1],t[2]);
00250 }
00251 
00254 //template <class T>
00255 //inline Vec<T,4> Transform<T>::operator * (const Vec3<T>& v) const{
00256 //  return (*(Mat<T,4,4>*)this)*Vec4<T>(v(0),v(1),v(2),T(1.0));
00257 //}
00258 
00261 template <class T>
00262 inline Vec3<T>& Transform<T>::apply(Vec3<T>& v) const {
00263   Vec4<T> v0(v[0],v[1],v[2],T(1));
00264   Vec<T,4> v1 = (*this)*v0;
00265   v.set(v1[0],v1[1],v1[2]);
00266   return v;
00267 }
00268 
00271 template <class T>
00272 inline Vec3<T> Transform<T>::apply(const Vec3<T>& v) const {
00273   Vec4<T> v0(v[0],v[1],v[2],T(1));
00274   Vec<T,4> v1 = (*this)*v0;
00275   return(Vec3<T>(v1[0],v1[1],v1[2]));
00276 }
00277 
00285 template <class T>
00286 inline Transform<T> Transform<T>::inverted() const{
00287   Rotation<T> r(x[0],x[4],x[8],
00288     x[1],x[5],x[9],
00289     x[2],x[6],x[10]);
00290   Vec3<T> t(-x[3],-x[7],-x[11]);
00291   return Transform<T>(r,r*t);
00292 }
00293 
00297 template <class T>
00298 inline Transform<T> Transform<T>::relativeTo(const Transform<T>& referenceFrame) const {
00299   return SMat4<T>(referenceFrame.inverted()*(*this));
00300 }
00301 
00302 template <class T>
00303 inline bool Transform<T>::verifyTag(std::string got, std::string expected) {
00304     if (got == expected){
00305       return true;
00306     }
00307     else{
00308       std::cerr << "ERROR: input stream format error." << std::endl;
00309       std::cerr << "Expected: " << expected << " Got: " << got << std::endl;
00310       return false;
00311     }
00312  }
00313 
00316 template <class T>
00317 std::ostream& operator<<(std::ostream& os, const Transform<T>& trans){
00318   Rotation<T> rot;
00319   Vec3<T> t;
00320   trans.get( rot,  t);
00321   EulerAngles<T> r;
00322   r.set(rot);
00323 
00324   os << "rot (Euler Angles) " << r << std::endl;
00325   os << "trans " << t << std::endl;
00326 
00327   return os;
00328 }
00329 
00330 
00334 template <class T>
00335 std::istream& operator>>(std::istream& is, Transform<T>& trans){
00336   using namespace std;
00337   Rotation<T> rot;
00338   Vec3<T> t;
00339   string buf;
00340 
00341   is >> buf;
00342 
00343   //old format (rot as quat)
00344   if (trans.verifyTag(buf,"r")){
00345     Quaternion<T> q;
00346     if (trans.verifyTag(buf,"r")){
00347       is >> q;
00348     }
00349     else{
00350       cout << "format error" << endl;
00351       exit(0);
00352     }
00353 
00354     is >> buf;
00355     if (trans.verifyTag(buf,"t")){
00356       is >> t;
00357     }
00358     else{
00359       cout << "format error" << endl;
00360       exit(0);
00361     }
00362 
00363     rot = Rotation<T>(q);
00364 
00365   }
00366   //new format (rot as euler angles)
00367   else{
00368      EulerAngles<T> ea;
00369      if (trans.verifyTag(buf,"rot")){
00370       is >> ea;
00371     }
00372     else{
00373       cout << "format error" << endl;
00374       exit(0);
00375     }
00376 
00377     is >> buf;
00378     if (trans.verifyTag(buf,"trans")){
00379       is >> t;
00380     }
00381     else{
00382       cout << "format error" << endl;
00383       exit(0);
00384     }
00385      rot = Rotation<T>(ea);
00386   }
00387 
00388   trans.set(rot,t);
00389 
00390   return is;
00391 }
00392 
00393 //==============================================================================
00394 } // namespace rtc
00395 //==============================================================================
00396 #endif // RTC_TRANSFORM_H defined
00397 //==============================================================================
00398 


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