$search
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