Main MRPT website > C++ reference for MRPT 1.9.9
CPoint.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #ifndef CPOINT_H
10 #define CPOINT_H
11 
14 
15 namespace mrpt
16 {
17 namespace poses
18 {
19 /** A base class for representing a point in 2D or 3D.
20  * For more information refer to the <a
21  * href="http://www.mrpt.org/2D_3D_Geometry">2D/3D Geometry tutorial</a> online.
22  * \note This class is based on the CRTP design pattern
23  * \sa CPoseOrPoint, CPose
24  * \ingroup poses_grp
25  */
26 template <class DERIVEDCLASS>
27 class CPoint : public CPoseOrPoint<DERIVEDCLASS>
28 {
29  DERIVEDCLASS& derived() { return *static_cast<DERIVEDCLASS*>(this); }
30  const DERIVEDCLASS& derived() const
31  {
32  return *static_cast<const DERIVEDCLASS*>(this);
33  }
34 
35  public:
36  /** @name Methods common to all 2D or 3D points
37  @{ */
38 
39  /** Scalar addition of all coordinates.
40  * This is diferent from poses/point composition, which is implemented as
41  * "+" operators in classes derived from "CPose"
42  */
43  template <class OTHERCLASS>
44  inline void AddComponents(const OTHERCLASS& b)
45  {
46  const int dims = std::min(
48  size_t(OTHERCLASS::is3DPoseOrPoint() ? 3 : 2));
49  for (int i = 0; i < dims; i++)
50  derived().m_coords[i] +=
51  static_cast<const OTHERCLASS*>(&b)->m_coords[i];
52  }
53 
54  /** Scalar multiplication. */
55  inline void operator*=(const double s)
56  {
57  for (int i = 0; i < DERIVEDCLASS::static_size; i++)
58  derived().m_coords[i] *= s;
59  }
60 
61  /** Return the pose or point as a 1x2 or 1x3 vector [x y] or [x y z] */
63  {
65  for (int i = 0; i < DERIVEDCLASS::static_size; i++)
66  v[i] = static_cast<const DERIVEDCLASS*>(this)->m_coords[i];
67  }
68  //! \overload
70  {
72  getAsVector(v);
73  return v;
74  }
75 
76  /** Returns the corresponding 4x4 homogeneous transformation matrix for the
77  * point(translation) or pose (translation+orientation).
78  * \sa getInverseHomogeneousMatrix
79  */
80  template <class MATRIX44>
81  void getHomogeneousMatrix(MATRIX44& 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) =
88  static_cast<const DERIVEDCLASS*>(this)->m_coords[2];
89  }
90 
91  /** Returns a human-readable textual representation of the object (eg:
92  * "[0.02 1.04]" )
93  * \sa fromString
94  */
95  void asString(std::string& s) const
96  {
97  s = (!DERIVEDCLASS::is3DPoseOrPoint())
98  ? mrpt::format(
99  "[%f %f]", static_cast<const DERIVEDCLASS*>(this)->x(),
100  static_cast<const DERIVEDCLASS*>(this)->y())
101  : mrpt::format(
102  "[%f %f %f]", static_cast<const DERIVEDCLASS*>(this)->x(),
103  static_cast<const DERIVEDCLASS*>(this)->y(),
104  static_cast<const DERIVEDCLASS*>(this)->m_coords[2]);
105  }
106  inline std::string asString() const
107  {
108  std::string s;
109  asString(s);
110  return s;
111  }
112 
113  /** Set the current object value from a string generated by 'asString' (eg:
114  * "[0.02 1.04]" )
115  * \sa asString
116  * \exception std::exception On invalid format
117  */
118  void fromString(const std::string& s)
119  {
121  if (!m.fromMatlabStringFormat(s))
122  THROW_EXCEPTION("Malformed expression in ::fromString");
123  ASSERT_EQUAL_(m.rows(), 1);
125  for (int i = 0; i < DERIVEDCLASS::static_size; i++)
126  derived().m_coords[i] = m(0, i);
127  }
128 
129  inline const double& operator[](unsigned int i) const
130  {
131  return static_cast<const DERIVEDCLASS*>(this)->m_coords[i];
132  }
133  inline double& operator[](unsigned int i) { return derived().m_coords[i]; }
134  /** @} */
135 
136 }; // End of class def.
137 
138 /** Dumps a point as a string [x,y] or [x,y,z] */
139 template <class DERIVEDCLASS>
140 std::ostream& operator<<(std::ostream& o, const CPoint<DERIVEDCLASS>& p)
141 {
142  o << "(" << p[0] << "," << p[1];
143  if (p.is3DPoseOrPoint()) o << "," << p[2];
144  o << ")";
145  return o;
146 }
147 
148 /** Used by STL algorithms */
149 template <class DERIVEDCLASS>
150 bool operator<(const CPoint<DERIVEDCLASS>& a, const CPoint<DERIVEDCLASS>& b)
151 {
152  if (a.x() < b.x())
153  return true;
154  else
155  {
156  if (!a.is3DPoseOrPoint())
157  return a.y() < b.y();
158  else if (a.y() < b.y())
159  return true;
160  else
161  return a[2] < b[2];
162  }
163 }
164 
165 template <class DERIVEDCLASS>
167 {
168  for (int i = 0; i < DERIVEDCLASS::static_size; i++)
169  if (p1[i] != p2[i]) return false; //-V550
170  return true;
171 }
172 
173 template <class DERIVEDCLASS>
175 {
176  for (int i = 0; i < DERIVEDCLASS::static_size; i++)
177  if (p1[i] != p2[i]) return true; //-V550
178  return false;
179 }
180 
181 } // namespace poses
182 } // namespace mrpt
183 
184 #endif
const DERIVEDCLASS & derived() const
Definition: CPoint.h:30
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[0.02 1.04]" ) ...
Definition: CPoint.h:95
void getHomogeneousMatrix(MATRIX44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoint.h:81
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
double & operator[](unsigned int i)
Definition: CPoint.h:133
#define min(a, b)
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
std::string asString() const
Definition: CPoint.h:106
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
GLdouble s
Definition: glext.h:3676
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[0.02 1.04]" ) ...
Definition: CPoint.h:118
A base class for representing a point in 2D or 3D.
Definition: CPoint.h:27
void AddComponents(const OTHERCLASS &b)
Scalar addition of all coordinates.
Definition: CPoint.h:44
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:153
bool operator!=(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:174
DERIVEDCLASS & derived()
Definition: CPoint.h:29
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
GLubyte GLubyte b
Definition: glext.h:6279
const double & operator[](unsigned int i) const
Definition: CPoint.h:129
The base template class for 2D & 3D points and poses.
Definition: CPoseOrPoint.h:125
GLsizei const GLchar ** string
Definition: glext.h:4101
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:166
void getAsVector(mrpt::math::CVectorDouble &v) const
Return the pose or point as a 1x2 or 1x3 vector [x y] or [x y z].
Definition: CPoint.h:62
mrpt::math::CVectorDouble getAsVector() const
Definition: CPoint.h:69
void operator*=(const double s)
Scalar multiplication.
Definition: CPoint.h:55
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
GLfloat GLfloat p
Definition: glext.h:6305



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019