Main MRPT website > C++ reference
MRPT logo
CPoint.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CPOINT_H
29 #define CPOINT_H
30 
31 #include <mrpt/poses/CPoseOrPoint.h>
32 
33 namespace mrpt
34 {
35  namespace poses
36  {
37  /** A base class for representing a point in 2D or 3D.
38  * For more information refer to the <a href="http://www.mrpt.org/2D_3D_Geometry">2D/3D Geometry tutorial</a> online.
39  * \note This class is based on the CRTP design pattern
40  * \sa CPoseOrPoint, CPose
41  * \ingroup poses_grp
42  */
43  template <class DERIVEDCLASS>
44  class CPoint : public CPoseOrPoint<DERIVEDCLASS>
45  {
46  public:
47  /** @name Methods common to all 2D or 3D points
48  @{ */
49 
50  /** Scalar addition of all coordinates.
51  * This is diferent from poses/point composition, which is implemented as "+" operators in classes derived from "CPose"
52  */
53  template <class OTHERCLASS>
54  inline void AddComponents(const OTHERCLASS &b)
55  {
56  const int dims = std::min( size_t(DERIVEDCLASS::static_size), size_t(OTHERCLASS::is3DPoseOrPoint() ? 3:2));
57  for (int i=0;i<dims;i++)
58  static_cast<DERIVEDCLASS*>(this)->m_coords[i]+= static_cast<const OTHERCLASS*>(&b)->m_coords[i];
59  }
60 
61  /** Scalar multiplication. */
62  inline void operator *=(const double s)
63  {
64  for (int i=0;i<DERIVEDCLASS::static_size;i++)
65  static_cast<DERIVEDCLASS*>(this)->m_coords[i] *= s;
66  }
67 
68  /** Return the pose or point as a 1x2 or 1x3 vector [x y] or [x y z] */
69  inline void getAsVector(vector_double &v) const
70  {
71  v.resize(DERIVEDCLASS::static_size);
72  for (int i=0;i<DERIVEDCLASS::static_size;i++)
73  v[i] = static_cast<const DERIVEDCLASS*>(this)->m_coords[i];
74  }
75  //! \overload
76  inline vector_double getAsVector() const { vector_double v; getAsVector(v); return v; }
77 
78  /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
79  * \sa getInverseHomogeneousMatrix
80  */
81  void getHomogeneousMatrix(CMatrixDouble44 & out_HM ) const
82  {
83  out_HM.unit(4,1.0);
84  out_HM.get_unsafe(0,3)= static_cast<const DERIVEDCLASS*>(this)->x();
85  out_HM.get_unsafe(1,3)= static_cast<const DERIVEDCLASS*>(this)->y();
86  if (DERIVEDCLASS::is3DPoseOrPoint())
87  out_HM.get_unsafe(2,3)= static_cast<const DERIVEDCLASS*>(this)->m_coords[2];
88  }
89 
90  /** Returns a human-readable textual representation of the object (eg: "[0.02 1.04]" )
91  * \sa fromString
92  */
93  void asString(std::string &s) const
94  {
95  s = (DERIVEDCLASS::is3DPoseOrPoint()) ?
96  mrpt::format("[%f %f]", static_cast<const DERIVEDCLASS*>(this)->x(), static_cast<const DERIVEDCLASS*>(this)->y()) :
97  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]);
98  }
99  inline std::string asString() const { std::string s; asString(s); return s; }
100 
101  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04]" )
102  * \sa asString
103  * \exception std::exception On invalid format
104  */
105  void fromString(const std::string &s)
106  {
107  CMatrixDouble m;
108  if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
109  ASSERT_EQUAL_(mrpt::math::size(m,1),1)
110  ASSERT_EQUAL_(mrpt::math::size(m,2),DERIVEDCLASS::static_size)
111  for (int i=0;i<DERIVEDCLASS::static_size;i++)
112  static_cast<DERIVEDCLASS*>(this)->m_coords[i] = m.get_unsafe(0,i);
113  }
114 
115  inline const double &operator[](unsigned int i) const { return static_cast<const DERIVEDCLASS*>(this)->m_coords[i]; }
116  inline double &operator[](unsigned int i) { return static_cast<DERIVEDCLASS*>(this)->m_coords[i]; }
117 
118  /** @} */
119 
120  }; // End of class def.
121 
122  /** Dumps a point as a string [x,y] or [x,y,z] */
123  template <class DERIVEDCLASS>
124  std::ostream &operator << (std::ostream& o, const CPoint<DERIVEDCLASS>& p)
125  {
126  o << "(" << p[0] << "," << p[1];
127  if (p.is3DPoseOrPoint()) o << "," << p[2];
128  o <<")";
129  return o;
130  }
131 
132  /** Used by STL algorithms */
133  template <class DERIVEDCLASS>
134  bool operator < (const CPoint<DERIVEDCLASS> &a, const CPoint<DERIVEDCLASS> &b)
135  {
136  if (a.x()<b.x()) return true;
137  else
138  {
139  if (!a.is3DPoseOrPoint())
140  return a.y()<b.y();
141  else if (a.y()<b.y())
142  return true;
143  else return a[2]<b[2];
144  }
145  }
146 
147  template <class DERIVEDCLASS>
148  bool operator==(const CPoint<DERIVEDCLASS> &p1,const CPoint<DERIVEDCLASS> &p2)
149  {
150  for (int i=0;i<DERIVEDCLASS::static_size;i++)
151  if (p1[i]!=p2[i]) return false;
152  return true;
153  }
154 
155  template <class DERIVEDCLASS>
156  bool operator!=(const CPoint<DERIVEDCLASS> &p1,const CPoint<DERIVEDCLASS> &p2)
157  {
158  for (int i=0;i<DERIVEDCLASS::static_size;i++)
159  if (p1[i]!=p2[i]) return true;
160  return false;
161  }
162 
163 
164 
165  } // End of namespace
166 } // End of namespace
167 
168 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013