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::poses
16 {
17 /** A base class for representing a point in 2D or 3D.
18  * For more information refer to the <a
19  * href="http://www.mrpt.org/2D_3D_Geometry">2D/3D Geometry tutorial</a> online.
20  * \note This class is based on the CRTP design pattern
21  * \sa CPoseOrPoint, CPose
22  * \ingroup poses_grp
23  */
24 template <class DERIVEDCLASS>
25 class CPoint : public CPoseOrPoint<DERIVEDCLASS>
26 {
27  DERIVEDCLASS& derived() { return *static_cast<DERIVEDCLASS*>(this); }
28  const DERIVEDCLASS& derived() const
29  {
30  return *static_cast<const DERIVEDCLASS*>(this);
31  }
32 
33  public:
34  /** @name Methods common to all 2D or 3D points
35  @{ */
36 
37  /** Scalar addition of all coordinates.
38  * This is diferent from poses/point composition, which is implemented as
39  * "+" operators in classes derived from "CPose"
40  */
41  template <class OTHERCLASS>
42  inline void AddComponents(const OTHERCLASS& b)
43  {
44  const int dims = std::min(
46  size_t(OTHERCLASS::is3DPoseOrPoint() ? 3 : 2));
47  for (int i = 0; i < dims; i++)
48  derived().m_coords[i] +=
49  static_cast<const OTHERCLASS*>(&b)->m_coords[i];
50  }
51 
52  /** Scalar multiplication. */
53  inline void operator*=(const double s)
54  {
55  for (int i = 0; i < DERIVEDCLASS::static_size; i++)
56  derived().m_coords[i] *= s;
57  }
58 
59  /** Return the pose or point as a 1x2 or 1x3 vector [x y] or [x y z] */
61  {
63  for (int i = 0; i < DERIVEDCLASS::static_size; i++)
64  v[i] = static_cast<const DERIVEDCLASS*>(this)->m_coords[i];
65  }
66  //! \overload
68  {
70  getAsVector(v);
71  return v;
72  }
73 
74  /** Returns the corresponding 4x4 homogeneous transformation matrix for the
75  * point(translation) or pose (translation+orientation).
76  * \sa getInverseHomogeneousMatrix
77  */
78  template <class MATRIX44>
79  void getHomogeneousMatrix(MATRIX44& out_HM) const
80  {
81  out_HM.unit(4, 1.0);
82  out_HM.get_unsafe(0, 3) = static_cast<const DERIVEDCLASS*>(this)->x();
83  out_HM.get_unsafe(1, 3) = static_cast<const DERIVEDCLASS*>(this)->y();
84  if (DERIVEDCLASS::is3DPoseOrPoint())
85  out_HM.get_unsafe(2, 3) =
86  static_cast<const DERIVEDCLASS*>(this)->m_coords[2];
87  }
88 
89  /** Returns a human-readable textual representation of the object (eg:
90  * "[0.02 1.04]" )
91  * \sa fromString
92  */
93  void asString(std::string& s) const
94  {
95  s = (!DERIVEDCLASS::is3DPoseOrPoint())
96  ? mrpt::format(
97  "[%f %f]", static_cast<const DERIVEDCLASS*>(this)->x(),
98  static_cast<const DERIVEDCLASS*>(this)->y())
99  : mrpt::format(
100  "[%f %f %f]", static_cast<const DERIVEDCLASS*>(this)->x(),
101  static_cast<const DERIVEDCLASS*>(this)->y(),
102  static_cast<const DERIVEDCLASS*>(this)->m_coords[2]);
103  }
104  inline std::string asString() const
105  {
106  std::string s;
107  asString(s);
108  return s;
109  }
110 
111  /** Set the current object value from a string generated by 'asString' (eg:
112  * "[0.02 1.04]" )
113  * \sa asString
114  * \exception std::exception On invalid format
115  */
116  void fromString(const std::string& s)
117  {
119  if (!m.fromMatlabStringFormat(s))
120  THROW_EXCEPTION("Malformed expression in ::fromString");
121  ASSERT_EQUAL_(m.rows(), 1);
123  for (int i = 0; i < DERIVEDCLASS::static_size; i++)
124  derived().m_coords[i] = m(0, i);
125  }
126 
127  inline const double& operator[](unsigned int i) const
128  {
129  return static_cast<const DERIVEDCLASS*>(this)->m_coords[i];
130  }
131  inline double& operator[](unsigned int i) { return derived().m_coords[i]; }
132  /** @} */
133 
134 }; // End of class def.
135 
136 /** Dumps a point as a string [x,y] or [x,y,z] */
137 template <class DERIVEDCLASS>
138 std::ostream& operator<<(std::ostream& o, const CPoint<DERIVEDCLASS>& p)
139 {
140  o << "(" << p[0] << "," << p[1];
141  if (p.is3DPoseOrPoint()) o << "," << p[2];
142  o << ")";
143  return o;
144 }
145 
146 /** Used by STL algorithms */
147 template <class DERIVEDCLASS>
148 bool operator<(const CPoint<DERIVEDCLASS>& a, const CPoint<DERIVEDCLASS>& b)
149 {
150  if (a.x() < b.x())
151  return true;
152  else
153  {
154  if (!a.is3DPoseOrPoint())
155  return a.y() < b.y();
156  else if (a.y() < b.y())
157  return true;
158  else
159  return a[2] < b[2];
160  }
161 }
162 
163 template <class DERIVEDCLASS>
165 {
166  for (int i = 0; i < DERIVEDCLASS::static_size; i++)
167  if (p1[i] != p2[i]) return false; //-V550
168  return true;
169 }
170 
171 template <class DERIVEDCLASS>
173 {
174  for (int i = 0; i < DERIVEDCLASS::static_size; i++)
175  if (p1[i] != p2[i]) return true; //-V550
176  return false;
177 }
178 } // namespace mrpt
179 
180 #endif
181 
182 
const DERIVEDCLASS & derived() const
Definition: CPoint.h:28
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[0.02 1.04]" ) ...
Definition: CPoint.h:93
void getHomogeneousMatrix(MATRIX44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoint.h:79
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
double & operator[](unsigned int i)
Definition: CPoint.h:131
#define min(a, b)
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
std::string asString() const
Definition: CPoint.h:104
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:116
A base class for representing a point in 2D or 3D.
Definition: CPoint.h:25
void AddComponents(const OTHERCLASS &b)
Scalar addition of all coordinates.
Definition: CPoint.h:42
#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:172
DERIVEDCLASS & derived()
Definition: CPoint.h:27
GLubyte GLubyte b
Definition: glext.h:6279
const double & operator[](unsigned int i) const
Definition: CPoint.h:127
The base template class for 2D & 3D points and poses.
Definition: CPoseOrPoint.h:125
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
const GLdouble * v
Definition: glext.h:3678
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
Definition: CPoint.h:164
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:60
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::math::CVectorDouble getAsVector() const
Definition: CPoint.h:67
void operator*=(const double s)
Scalar multiplication.
Definition: CPoint.h:53
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020