Main MRPT website > C++ reference
MRPT logo

CPoint.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef CPOINT_H
00029 #define CPOINT_H
00030 
00031 #include <mrpt/poses/CPoseOrPoint.h>
00032 
00033 namespace mrpt
00034 {
00035         namespace poses
00036         {
00037                 /** A base class for representing a point in 2D or 3D.
00038                  *   For more information refer to the <a href="http://www.mrpt.org/2D_3D_Geometry">2D/3D Geometry tutorial</a> online.
00039                  * \note This class is based on the CRTP design pattern
00040                  * \sa CPoseOrPoint, CPose
00041                  */
00042                 template <class DERIVEDCLASS>
00043                 class CPoint : public CPoseOrPoint<DERIVEDCLASS>
00044                 {
00045                 public:
00046                         /** @name Methods common to all 2D or 3D points
00047                             @{ */
00048 
00049                         /** Scalar addition of all coordinates.
00050                           * This is diferent from poses/point composition, which is implemented as "+" operators in classes derived from "CPose"
00051                           */
00052                         template <class OTHERCLASS>
00053                         inline void AddComponents(const OTHERCLASS &b)
00054                         {
00055                                 const int dims = std::min( size_t(DERIVEDCLASS::static_size), size_t(OTHERCLASS::is3DPoseOrPoint() ? 3:2));
00056                                 for (int i=0;i<dims;i++)
00057                                         static_cast<DERIVEDCLASS*>(this)->m_coords[i]+= static_cast<const OTHERCLASS*>(&b)->m_coords[i];
00058                         }
00059 
00060                         /** Scalar multiplication. */
00061                         inline void operator *=(const double s)
00062                         {
00063                                 for (int i=0;i<DERIVEDCLASS::static_size;i++)
00064                                         static_cast<DERIVEDCLASS*>(this)->m_coords[i] *= s;
00065                         }
00066 
00067                         /** Return the pose or point as a 1x2 or 1x3 vector [x y] or [x y z] */
00068                         inline void getAsVector(vector_double &v) const
00069                         {
00070                                 v.resize(DERIVEDCLASS::static_size);
00071                                 for (int i=0;i<DERIVEDCLASS::static_size;i++)
00072                                         v[i] = static_cast<const DERIVEDCLASS*>(this)->m_coords[i];
00073                         }
00074                         //! \overload
00075                         inline vector_double getAsVector() const { vector_double v; getAsVector(v); return v; }
00076 
00077                         /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
00078                           * \sa getInverseHomogeneousMatrix
00079                           */
00080                         void getHomogeneousMatrix(CMatrixDouble44 & out_HM ) const
00081                         {
00082                                 out_HM.unit(4,1.0);
00083                                 out_HM.get_unsafe(0,3)= static_cast<const DERIVEDCLASS*>(this)->x();
00084                                 out_HM.get_unsafe(1,3)= static_cast<const DERIVEDCLASS*>(this)->y();
00085                                 if (DERIVEDCLASS::is3DPoseOrPoint())
00086                                         out_HM.get_unsafe(2,3)= static_cast<const DERIVEDCLASS*>(this)->m_coords[2];
00087                         }
00088 
00089                         /** Returns a human-readable textual representation of the object (eg: "[0.02 1.04]" )
00090                         * \sa fromString
00091                         */
00092                         void asString(std::string &s) const
00093                         {
00094                                 s = (DERIVEDCLASS::is3DPoseOrPoint()) ?
00095                                         mrpt::format("[%f %f]", static_cast<const DERIVEDCLASS*>(this)->x(), static_cast<const DERIVEDCLASS*>(this)->y()) :
00096                                         mrpt::format("[%f %f %f]",static_cast<const DERIVEDCLASS*>(this)->x(), static_cast<const DERIVEDCLASS*>(this)->y(), static_cast<const DERIVEDCLASS*>(this)->m_coords[2]);
00097                         }
00098                         inline std::string asString() const { std::string s; asString(s); return s; }
00099 
00100                         /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04]" )
00101                         * \sa asString
00102                         * \exception std::exception On invalid format
00103                         */
00104                         void fromString(const std::string &s)
00105                         {
00106                                 CMatrixDouble  m;
00107                                 if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
00108                                 ASSERT_EQUAL_(mrpt::math::size(m,1),1)
00109                                 ASSERT_EQUAL_(mrpt::math::size(m,2),DERIVEDCLASS::static_size)
00110                                 for (int i=0;i<DERIVEDCLASS::static_size;i++)
00111                                         static_cast<DERIVEDCLASS*>(this)->m_coords[i] = m.get_unsafe(0,i);
00112                         }
00113 
00114                         inline const double &operator[](unsigned int i) const { return static_cast<const DERIVEDCLASS*>(this)->m_coords[i]; }
00115                         inline       double &operator[](unsigned int i)       { return static_cast<DERIVEDCLASS*>(this)->m_coords[i]; }
00116 
00117                         /** @} */
00118 
00119                 }; // End of class def.
00120 
00121         /** Dumps a point as a string [x,y] or [x,y,z]  */
00122         template <class DERIVEDCLASS>
00123         std::ostream &operator << (std::ostream& o, const CPoint<DERIVEDCLASS>& p)
00124         {
00125                 o << "(" << p[0] << "," << p[1];
00126                 if (p.is3DPoseOrPoint())        o << "," << p[2];
00127                 o <<")";
00128                 return o;
00129         }
00130 
00131         /** Used by STL algorithms */
00132         template <class DERIVEDCLASS>
00133         bool operator < (const CPoint<DERIVEDCLASS> &a, const CPoint<DERIVEDCLASS> &b)
00134         {
00135                 if (a.x()<b.x()) return true;
00136                 else
00137                 {
00138                         if (!a.is3DPoseOrPoint())
00139                                 return a.y()<b.y();
00140                         else  if (a.y()<b.y())
00141                                 return true;
00142                         else return a[2]<b[2];
00143                 }
00144         }
00145 
00146         template <class DERIVEDCLASS>
00147         bool operator==(const CPoint<DERIVEDCLASS> &p1,const CPoint<DERIVEDCLASS> &p2)
00148         {
00149                 for (int i=0;i<DERIVEDCLASS::static_size;i++)
00150                         if (p1[i]!=p2[i])       return false;
00151                 return true;
00152         }
00153 
00154         template <class DERIVEDCLASS>
00155         bool operator!=(const CPoint<DERIVEDCLASS> &p1,const CPoint<DERIVEDCLASS> &p2)
00156         {
00157                 for (int i=0;i<DERIVEDCLASS::static_size;i++)
00158                         if (p1[i]!=p2[i])       return true;
00159                 return false;
00160         }
00161 
00162 
00163 
00164         } // End of namespace
00165 } // End of namespace
00166 
00167 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011