rtc: rtc: rtcTransform2D.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 .......: rtcTransform2D.h
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 02/29/2008
00015  * modified ...: $Date: 2009-03-23 17:48:44 -0700 (Mon, 23 Mar 2009) $
00016  * changed by .: $Author: wg75pal $
00017  * revision ...: $Revision: 158 $
00018  */
00019 #ifndef RTC_TRANSFORM2D_H
00020 #define RTC_TRANSFORM2D_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include "rtc/rtcMath.h"
00024 #include "rtc/rtcTransform2D.h"
00025 #include "rtc/rtcRotation2D.h"
00026 #include "rtc/rtcVec.h"
00027 #include "rtc/rtcVec2.h"
00028 #include "rtc/rtcVec3.h"
00029 #include "rtc/rtcSMat3.h"
00030 
00031 //== NAMESPACES ================================================================
00032 namespace rtc {
00033 
00034 //== FORWARD DECLARATIONS ======================================================
00035 template <class T> class Transform2D;   // Rigid tranform matrix (3x3)
00036 template <class T> class Rotation2D;    // Rotation2D matrix (2x2)
00037 template <class T, int M> class Vec;    // M-d vector
00038 template <class T> class Vec2;          // 2-d vector
00039 
00046 template <class T>
00047 class Transform2D: public SMat3<T> {
00048 public:
00049   // Constructors
00050   Transform2D();
00051   Transform2D(const Mat<T,3,3>& m);
00052   Transform2D(const SMat3<T>& m);
00053   Transform2D(const Rotation2D<T>& rot);
00054   Transform2D(const Vec2<T>& trans);
00055   Transform2D(const Rotation2D<T>& rot, const Vec2<T>& trans);
00056   Transform2D(const T& dtheta, const T& dx, const T& dy);
00057 
00058   // Accessors
00059   void get( Rotation2D<T>& rot, Vec2<T>& trans) const;
00060   void get(T& theta, T& dx, T& dy) const;
00061   Vec2<T> getTrans( ) const;
00062   void getTrans(T& dx, T& dy) const;
00063   Rotation2D<T> getRot( ) const;
00064   void getRot(T& dtheta) const;
00065 
00066   // Mutators
00067   void set(const Rotation2D<T>& rot);
00068   void set(const Vec2<T>& trans);
00069   void set(const Rotation2D<T>& rot, const Vec2<T>& trans);
00070   void set(const T& dtheta, const T& dx, const T& dy);
00071   void rotate(T theta);
00072   void translate(T x, T y);
00073   void translate(const Vec2<T>& t);
00074 
00075   // Helper for applying tranform to points: Transform2D * Vec<T,2>
00076   Vec<T,3> operator * (const Vec2<T>& v) const;
00077   void apply(Vec2<T>& v) const;
00078   Vec2<T> apply(const Vec2<T>& v) const;
00079 
00080   //Special inverse that uses the fact this is a rigid transform matrix
00081   Transform2D<T> inverted() const;
00082   Transform2D<T> relativeTo(const Transform2D<T>& referenceFrame) const;
00083 
00084   // Inherited from parent
00085   using SMat<T,3>::set;
00086   using SMat<T,3>::x;
00087 #ifndef WIN32
00088   using SMat<T,3>::operator*;
00089 #endif
00090 };
00091 
00092 // Declare a few common typdefs
00093 typedef Transform2D<float> Transform2Df;
00094 typedef Transform2D<double> Transform2Dd;
00095 
00096 //==============================================================================
00097 // Transform2D<T>
00098 //==============================================================================
00099 
00102 template <class T>
00103 inline Transform2D<T>::Transform2D() {
00104   set(Rotation2D<T>(),Vec<T,2>(T(0)));
00105 }
00106 
00109 template <class T>
00110 inline Transform2D<T>::Transform2D(const Rotation2D<T>& r){
00111   set(r,Vec2<T>(T(0)));
00112 }
00113 
00116 template <class T>
00117 inline Transform2D<T>::Transform2D(const Vec2<T>& t){
00118   set(Rotation2D<T>(),t);
00119 }
00120 
00123 template <class T>
00124 inline Transform2D<T>::Transform2D(const Rotation2D<T>& rot, const Vec2<T>& trans){
00125   set(rot,trans);
00126 }
00127 
00130 template <class T>
00131 inline Transform2D<T>::Transform2D(const SMat3<T>& m) : SMat3<T>(m) {}
00132 
00135 template <class T>
00136 inline Transform2D<T>::Transform2D(const T& dtheta, const T& dx, const T& dy){
00137   set(dtheta,dx,dy);
00138 }
00139 
00142 template <class T>
00143 inline Transform2D<T>::Transform2D(const Mat<T,3,3>& m) : SMat3<T>(m) {}
00144 
00145 // Accessors
00146 
00149 template <class T>
00150 inline void Transform2D<T>::get(Rotation2D<T>& r, Vec2<T>& t) const
00151 {
00152   r=Rotation2D<T>(x[0],x[1],
00153                   x[3],x[4]);
00154   t=Vec2<T>(x[2],x[5]);
00155 }
00156 
00159 template <class T>
00160 inline void Transform2D<T>::get(T& theta, T& dx, T& dy) const
00161 {
00162   getTrans(dx,dy);
00163   getRot(theta);
00164 }
00165 
00168 template <class T>
00169 inline Vec2<T> Transform2D<T>::getTrans() const
00170 {
00171   return Vec2<T>(x[2],x[5]);
00172 }
00173 
00176 template <class T>
00177 inline void Transform2D<T>::getTrans(T& dx, T& dy) const
00178 {
00179   dx=x[2];
00180   dy=x[5];
00181 }
00182 
00185 template <class T>
00186 inline Rotation2D<T> Transform2D<T>::getRot() const
00187 {
00188   return Rotation2D<T>(x[0],x[1],
00189                        x[3],x[4]);
00190 }
00191 
00194 template <class T>
00195 inline void Transform2D<T>::getRot(T& dtheta) const
00196 {
00197   dtheta = atan2(x[3],x[0]);
00198 }
00199 
00200 // Mutators
00201 
00204 template <class T>
00205 inline void Transform2D<T>::set(const Rotation2D<T>& r) {
00206   for (int i=0;i<2;i++)
00207     for (int j=0;j<2;j++)
00208       (*this)(i,j) = r(i,j);
00209   x[6] = 0; x[7] = 0; x[8] = 1;
00210 }
00211 
00214 template <class T>
00215 inline void Transform2D<T>::set(const Vec2<T>& t) {
00216   x[2] = t(0);
00217   x[5] = t(1);
00218   x[6] = 0; x[7] = 0; x[8] = 1;
00219 }
00220 
00223 template <class T>
00224 inline void Transform2D<T>::set(const Rotation2D<T>& r, const Vec2<T>& t) {
00225   set(r);
00226   set(t);
00227 }
00228 
00231 template <class T>
00232 inline void Transform2D<T>::set(const T& dtheta, const T& dx, const T& dy) {
00233   set(Rotation2D<T>(dtheta),Vec2<T>(dx,dy));
00234 }
00235 
00238 template <class T>
00239 inline void Transform2D<T>::rotate(T theta)
00240 {
00241   Transform2D<T> temp;
00242   T ctheta = cos(theta), stheta = sin(theta);
00243   temp(0,0) = ctheta;
00244   temp(0,1) = -stheta;
00245   temp(1,0) = stheta;
00246   temp(1,1) = ctheta;
00247   leftMultiply(temp);
00248 }
00249 
00252 template <class T>
00253 inline void Transform2D<T>::translate(T _x, T _y)
00254 {
00255   x[2] += _x;
00256   x[5] += _y;
00257 }
00258 
00261 template <class T>
00262 inline void Transform2D<T>::translate(const Vec2<T>& t)
00263 {
00264   translate(t[0],t[1]);
00265 }
00266 
00269 template <class T>
00270 inline Vec<T,3> Transform2D<T>::operator * (const Vec2<T>& v) const {
00271   return (*(Mat<T,3,3>*)this)*Vec3<T>(v(0),v(1),T(1.0));
00272 }
00273 
00276 template <class T>
00277 inline void Transform2D<T>::apply(Vec2<T>& v) const {
00278   Vec<T,3> v1 = (*this)*v;
00279   v.set(v1[0],v1[1]);
00280 }
00281 
00284 template <class T>
00285 inline Vec2<T> Transform2D<T>::apply(const Vec2<T>& v) const {
00286   Vec<T,3> v1 = (*this)*v;
00287   return(Vec2<T>(v1[0],v1[1]));
00288 }
00289 
00297 template <class T>
00298 inline Transform2D<T> Transform2D<T>::inverted() const{
00299   Rotation2D<T> r(x[0],x[3],
00300                   x[1],x[4]);
00301   Vec2<T> t(-x[2],-x[5]);
00302   return Transform2D<T>(r,r*t);
00303 }
00304 
00308 template <class T>
00309 inline Transform2D<T> Transform2D<T>::relativeTo(const Transform2D<T>& referenceFrame) const {
00310   return SMat3<T>(referenceFrame.inverted()*(*this));
00311 }
00312 
00313 //==============================================================================
00314 } // NAMESPACE rtc
00315 //==============================================================================
00316 #endif // RTC_TRANSFORM2D_H
00317 //==============================================================================
00318