Transformation2D.cpp
Go to the documentation of this file.
00001 /*******************************************************************************
00002  *  Transformation2D.cpp
00003  *
00004  *  (C) 2008 AG Aktives Sehen <agas@uni-koblenz.de>
00005  *           Universitaet Koblenz-Landau
00006  *
00007  * $Id: Transformation2D.cpp 44313 2011-04-06 22:46:28Z agas $
00008  *******************************************************************************/
00009 
00010 #include "Transformation2D.h"
00011 
00012 #include <cmath>
00013 #include <vector>
00014 #include <iostream>
00015 #include <sstream>
00016 #include "vec2.h" // TODO das sieht nach baselib aus ggf. durch baselib ersetzen
00017 #include "mat2.h" // TODO das sieht nach baselib aus ggf. durch baselib ersetzen
00018 #include "Point2D.h"
00019 #include "Line2D.h"
00020 
00021 #define THIS Transformation2D
00022 #define BASE CVec2
00023 
00024 THIS::Transformation2D() : BASE()
00025 {
00026   m_Theta = 0.0;
00027 }
00028 
00029 THIS::Transformation2D ( double x, double y, double theta ) : BASE ( x,y )
00030 {
00031   m_Theta = theta;
00032 }
00033 
00034 THIS::Transformation2D ( const CVec2& vec, double theta ) : BASE ( vec )
00035 {
00036   m_Theta = theta;
00037 }
00038 
00039 THIS::~Transformation2D()
00040 {
00041 }
00042 
00043 void THIS::set ( double x, double y, double theta )
00044 {
00045   m_X = x;
00046   m_Y = y;
00047   m_Theta = theta;
00048 }
00049 
00050 double THIS::theta() const
00051 {
00052   return m_Theta;
00053 }
00054 
00055 Transformation2D THIS::operator+ ( Transformation2D t ) const
00056 {
00057   double theta = m_Theta + t.theta();
00058   // TODO comment only for scan matching test
00059 //   while ( theta >= M_PI ) theta -= 2*M_PI;
00060 //   while ( theta < -M_PI ) theta += 2*M_PI;
00061   return Transformation2D ( m_X + t.x(), m_Y + t.y(), theta );
00062 }
00063 
00064 Transformation2D& THIS::operator+= ( Transformation2D t )
00065 {
00066   m_X += t.x();
00067   m_Y += t.y();
00068   m_Theta += t.theta();
00069   // TODO comment only for scan matching test
00070 //   while ( m_Theta >= M_PI ) m_Theta -= 2*M_PI;
00071 //   while ( m_Theta < -M_PI ) m_Theta += 2*M_PI;
00072   return ( *this );
00073 }
00074 
00075 Transformation2D THIS::operator- ( Transformation2D t ) const
00076 {
00077   float s1, s2, theta;
00078   if ( m_Theta > t.theta() )
00079   {
00080     s1 = - ( 2 * M_PI - m_Theta + t.theta() );
00081     s2 = m_Theta - t.theta();
00082   }
00083   else
00084   {
00085     s1 = 2 * M_PI - t.theta() + m_Theta;
00086     s2 = - ( t.theta() - m_Theta );
00087   }
00088   if ( fabs ( s1 ) > fabs ( s2 ) )
00089   {
00090     theta = s2;
00091   }
00092   else
00093   {
00094     theta = s1;
00095   }
00096   while ( theta >= M_PI ) theta -= 2*M_PI;
00097   while ( theta < -M_PI ) theta += 2*M_PI;
00098 //   double theta = m_Theta - t.theta();
00099 //   while ( theta >= M_PI ) theta -= 2*M_PI;
00100 //   while ( theta < -M_PI ) theta += 2*M_PI;
00101   return Transformation2D ( m_X - t.x(), m_Y - t.y(), theta );
00102 }
00103 
00104 Transformation2D& THIS::operator-= ( Transformation2D t )
00105 {
00106   m_X -= t.x();
00107   m_Y -= t.y();
00108 
00109   float s1, s2, theta;
00110   if ( m_Theta > t.theta() )
00111   {
00112     s1 = - ( 2 * M_PI - m_Theta + t.theta() );
00113     s2 = m_Theta - t.theta();
00114   }
00115   else
00116   {
00117     s1 = 2 * M_PI - t.theta() + m_Theta;
00118     s2 = - ( t.theta() - m_Theta );
00119   }
00120   if ( fabs ( s1 ) > fabs ( s2 ) )
00121   {
00122     theta = s2;
00123   }
00124   else
00125   {
00126     theta = s1;
00127   }
00128   while ( theta >= M_PI ) theta -= 2*M_PI;
00129   while ( theta < -M_PI ) theta += 2*M_PI;
00130   m_Theta = theta;
00131 
00132   return ( *this );
00133 
00134 //   m_X -= t.x();
00135 //   m_Y -= t.y();
00136 //   m_Theta -= t.theta();
00137 //   while ( m_Theta >= M_PI ) m_Theta -= 2*M_PI;
00138 //   while ( m_Theta < -M_PI ) m_Theta += 2*M_PI;
00139 //   return ( *this );
00140 }
00141 
00142 Transformation2D THIS::operator* ( float factor ) const
00143 {
00144 
00145   double theta = m_Theta * factor;
00146   while ( theta >= M_PI ) theta -= 2*M_PI;
00147   while ( theta < -M_PI ) theta += 2*M_PI;
00148   return Transformation2D ( m_X * factor, m_Y * factor, theta );
00149 }
00150 
00151 Transformation2D& THIS::operator*= ( float factor )
00152 {
00153   m_X *= factor;
00154   m_Y *= factor;
00155   m_Theta *= factor;
00156   while ( m_Theta >= M_PI ) m_Theta -= 2*M_PI;
00157   while ( m_Theta < -M_PI ) m_Theta += 2*M_PI;
00158   return ( *this );
00159 }
00160 
00161 
00162 Transformation2D THIS::operator/ ( float factor ) const
00163 {
00164   double theta = m_Theta / factor;
00165   return Transformation2D ( m_X / factor, m_Y / factor, theta );
00166 }
00167 
00168 Transformation2D& THIS::operator/= ( float factor )
00169 {
00170   m_X /= factor;
00171   m_Y /= factor;
00172   m_Theta /= factor;
00173   return ( *this );
00174 }
00175 
00176 bool THIS::operator== ( Transformation2D t ) const
00177 {
00178   if ( t.x() == m_X && t.y() == m_Y && t.theta() == m_Theta )
00179   {
00180     return true;
00181   }
00182   else
00183   {
00184     return false;
00185   }
00186 }
00187 
00188 bool THIS::operator!= ( Transformation2D t ) const
00189 {
00190   return ! ( ( *this ) ==t );
00191 }
00192 
00193 bool THIS::operator<= ( Transformation2D t ) const
00194 {
00195   return ( this->magnitude() <= t.magnitude() ) && ( m_Theta <= t.theta() );
00196 }
00197 
00198 bool THIS::operator>= ( Transformation2D t ) const
00199 {
00200   return ( this->magnitude() >= t.magnitude() ) && ( m_Theta >= t.theta() );
00201 }
00202 
00203 bool THIS::operator< ( Transformation2D t ) const
00204 {
00205   return ( m_X < t.x() ) || ( m_Y < t.y() ) || ( ( m_Theta < t.theta() ) && ( *this <= t ) );
00206 }
00207 
00208 bool THIS::operator> ( Transformation2D t ) const
00209 {
00210   return ( m_X > t.x() ) || ( m_Y > t.y() ) || ( ( m_Theta > t.theta() ) && ( *this >= t ) );
00211 }
00212 
00213 Transformation2D THIS::abs() const
00214 {
00215   return Transformation2D ( std::abs ( m_X ), std::abs ( m_Y ), std::abs ( m_Theta ) );
00216 }
00217 
00218 Transformation2D THIS::inverse() const
00219 {
00220   return ( *this ) * ( -1.0 );
00221 }
00222 
00223 Point2D THIS::transform ( const Point2D& point ) const
00224 {
00225   CMat2 rotMat = CMat2 ( m_Theta );
00226   CVec2 transVec = CVec2 ( m_X, m_Y );
00227   Point2D transformedPoint = rotMat * ( point );
00228   transformedPoint += transVec;
00229   return transformedPoint;
00230 }
00231 
00232 std::vector<Point2D> THIS::transform ( const std::vector<Point2D>& points ) const
00233 {
00234   CMat2 rotMat = CMat2 ( m_Theta );
00235   CVec2 transVec = CVec2 ( m_X, m_Y );
00236   std::vector<Point2D> transformedPoints;
00237   std::vector<Point2D>::const_iterator iter = points.begin();
00238   while ( iter != points.end() )
00239   {
00240     Point2D currPoint = rotMat * ( *iter );
00241     currPoint += transVec;
00242     transformedPoints.push_back ( currPoint );
00243     iter++;
00244   }
00245   return transformedPoints;
00246 }
00247 
00248 // // Reihenfolge rotation/translation vertauscht !!!
00249 // Point2D THIS::transform ( Point2D point ) const
00250 // {
00251 //   CMat2 rotMat = CMat2 ( m_Theta );
00252 //   CVec2 transVec = CVec2 ( m_X, m_Y );
00253 //   Point2D transformedPoint = point+transVec;
00254 //   transformedPoint = rotMat * point;
00255 //   return transformedPoint;
00256 // }
00257 //
00258 // // Reihenfolge rotation/translation vertauscht !!!
00259 // std::vector<Point2D> THIS::transform ( std::vector<Point2D> points ) const
00260 // {
00261 //   CMat2 rotMat = CMat2 ( m_Theta );
00262 //   CVec2 transVec = CVec2 ( m_X, m_Y );
00263 //   std::vector<Point2D> transformedPoints;
00264 //   std::vector<Point2D>::const_iterator iter = points.begin();
00265 //   while ( iter != points.end() )
00266 //   {
00267 //     Point2D currPoint = ( *iter )+ transVec;
00268 //     currPoint = rotMat * currPoint;
00269 //     transformedPoints.push_back ( currPoint );
00270 //     iter++;
00271 //   }
00272 //   return transformedPoints;
00273 // }
00274 
00275 Line2D THIS::transform ( const Line2D& line ) const
00276 {
00277   CMat2 rotMat = CMat2 ( m_Theta );
00278   CVec2 transVec = CVec2 ( m_X, m_Y );
00279   Line2D transformedLine = Line2D ( rotMat * line.start() + transVec, rotMat * line.end() + transVec );
00280   return transformedLine;
00281 }
00282 
00283 std::vector<Line2D> THIS::transform ( const std::vector<Line2D>& lines ) const
00284 {
00285   //CMat2 rotMat = CMat2 ( m_Theta );
00286   //CVec2 transVec = CVec2 ( m_X, m_Y );
00287   std::vector<Line2D> transformedLines;
00288   std::vector<Line2D>::const_iterator iter = lines.begin();
00289   while ( iter != lines.end() )
00290   {
00291     transformedLines.push_back ( transform(*iter) );
00292     iter++;
00293   }
00294   return transformedLines;
00295 }
00296 
00297 std::string THIS::toString() const
00298 {
00299   std::ostringstream str;
00300   str << "deltaX: " << m_X << ", deltaY: " << m_Y << ", deltaTheta: " << m_Theta;
00301   return str.str();
00302 }
00303 
00304 
00305 
00306 #undef THIS
00307 #undef BASE
00308 


robbie_architecture
Author(s): Viktor Seib
autogenerated on Mon Oct 6 2014 02:53:09