Main MRPT website > C++ reference for MRPT 1.5.6
lightweight_geom_data.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-2017, 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 LIGHTWEIGHT_GEOM_DATA_H
10 #define LIGHTWEIGHT_GEOM_DATA_H
11 
12 #include <mrpt/utils/utils_defs.h>
13 #include <mrpt/config.h>
14 #include <mrpt/base/link_pragmas.h>
15 #include <mrpt/utils/TPixelCoord.h>
16 #include <mrpt/utils/TTypeName.h>
17 #include <mrpt/math/math_frwds.h> // forward declarations
18 #include <mrpt/math/eigen_frwds.h> // forward declarations
19 #include <mrpt/math/wrap2pi.h>
20 #include <vector>
21 #include <stdexcept>
22 
23 namespace mrpt {
24 namespace math {
25  /** \addtogroup geometry_grp
26  * @{ */
27 
28  //Set of typedefs for lightweight geometric items.
29  /**
30  * Lightweight 2D point. Allows coordinate access using [] operator.
31  * \sa mrpt::poses::CPoint2D
32  */
34  enum { static_size = 2 };
35  double x, y; //!< X,Y coordinates
36  /** Constructor from TPose2D, discarding phi.
37  * \sa TPose2D
38  */
39  explicit TPoint2D(const TPose2D &p);
40  /**
41  * Constructor from TPoint3D, discarding z.
42  * \sa TPoint3D
43  */
44  explicit TPoint2D(const TPoint3D &p);
45  /**
46  * Constructor from TPose3D, discarding z and the angular coordinates.
47  * \sa TPose3D
48  */
49  explicit TPoint2D(const TPose3D &p);
50  /**
51  * Constructor from CPoseOrPoint, perhaps losing 3D information
52  * \sa CPoseOrPoint mrpt::poses::CPoint3D,CPose2D,CPose3D
53  */
54  template <class DERIVEDCLASS>
55  explicit TPoint2D(const mrpt::poses::CPoseOrPoint<DERIVEDCLASS> &p) :x(p.x()),y(p.y()) {}
56 
57  /** Implicit transformation constructor from TPixelCoordf */
58  inline TPoint2D(const mrpt::utils::TPixelCoordf &p) :x(p.x),y(p.y) {}
59 
60  /** Implicit constructor from mrpt::poses::CPoint2D */
62  /**
63  * Constructor from coordinates.
64  */
65  inline TPoint2D(double xx,double yy):x(xx),y(yy) {}
66  /**
67  * Default fast constructor. Initializes to garbage.
68  */
69  inline TPoint2D() {}
70  /** Coordinate access using operator[]. Order: x,y */
71  inline double &operator[](size_t i) { switch (i) { case 0: return x; case 1: return y; default: throw std::out_of_range("index out of range"); } }
72  /** Coordinate access using operator[]. Order: x,y */
73  inline const double &operator[](size_t i) const { switch (i) { case 0: return x; case 1: return y; default: throw std::out_of_range("index out of range"); } }
74  /**
75  * Transformation into vector.
76  */
77  inline void getAsVector(std::vector<double> &v) const {
78  v.resize(2);
79  v[0]=x; v[1]=y;
80  }
81 
82  bool operator<(const TPoint2D &p) const;
83 
84  inline TPoint2D &operator+=(const TPoint2D &p) {
85  x+=p.x;
86  y+=p.y;
87  return *this;
88  }
89 
90  inline TPoint2D &operator-=(const TPoint2D &p) {
91  x-=p.x;
92  y-=p.y;
93  return *this;
94  }
95 
96  inline TPoint2D &operator*=(double d) {
97  x*=d;
98  y*=d;
99  return *this;
100  }
101 
102  inline TPoint2D &operator/=(double d) {
103  x/=d;
104  y/=d;
105  return *this;
106  }
107 
108  inline TPoint2D operator+(const TPoint2D &p) const {
109  TPoint2D r(*this);
110  return r+=p;
111  }
112 
113  inline TPoint2D operator-(const TPoint2D &p) const {
114  TPoint2D r(*this);
115  return r-=p;
116  }
117 
118  inline TPoint2D operator*(double d) const {
119  TPoint2D r(*this);
120  return r*=d;
121  }
122 
123  inline TPoint2D operator/(double d) const {
124  TPoint2D r(*this);
125  return r/=d;
126  }
127  /** Returns a human-readable textual representation of the object (eg: "[0.02 1.04]" )
128  * \sa fromString
129  */
130  void asString(std::string &s) const { s = mrpt::format("[%f %f]",x,y); }
131  inline std::string asString() const { std::string s; asString(s); return s; }
132 
133  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04]" )
134  * \sa asString
135  * \exception std::exception On invalid format
136  */
137  void fromString(const std::string &s);
138  static size_t size() { return 2; }
139 
140  /** Point norm. */
141  inline double norm() const { return sqrt(square(x)+square(y)); }
142  };
143 
144  /**
145  * Lightweight 2D pose. Allows coordinate access using [] operator.
146  * \sa mrpt::poses::CPose2D
147  */
149  enum { static_size = 3 };
150  double x,y; //!< X,Y coordinates
151  double phi; //!< Orientation (rads)
152  /** Implicit constructor from TPoint2D. Zeroes the phi coordinate.
153  * \sa TPoint2D
154  */
155  TPose2D(const TPoint2D &p);
156  /**
157  * Constructor from TPoint3D, losing information. Zeroes the phi coordinate.
158  * \sa TPoint3D
159  */
160  explicit TPose2D(const TPoint3D &p);
161  /**
162  * Constructor from TPose3D, losing information. The phi corresponds to the original pose's yaw.
163  * \sa TPose3D
164  */
165  explicit TPose2D(const TPose3D &p);
166  /**
167  * Implicit constructor from heavyweight type.
168  * \sa mrpt::poses::CPose2D
169  */
171  /**
172  * Constructor from coordinates.
173  */
174  inline TPose2D(double xx,double yy,double pphi):x(xx),y(yy),phi(pphi) {}
175  /**
176  * Default fast constructor. Initializes to garbage.
177  */
178  inline TPose2D() {}
179  /** Coordinate access using operator[]. Order: x,y,phi */
180  inline double &operator[](size_t i) { switch (i) { case 0: return x; case 1: return y; case 2: return phi; default: throw std::out_of_range("index out of range"); } }
181  /** Coordinate access using operator[]. Order: x,y,phi */
182  inline const double &operator[](size_t i) const { switch (i) { case 0: return x; case 1: return y; case 2: return phi; default: throw std::out_of_range("index out of range"); } }
183  /**
184  * Transformation into vector.
185  */
186  inline void getAsVector(std::vector<double> &v) const {
187  v.resize(3);
188  v[0]=x; v[1]=y; v[2]=phi;
189  }
190  /** Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees)
191  * \sa fromString
192  */
193  void asString(std::string &s) const;
194  inline std::string asString() const { std::string s; asString(s); return s; }
195 
196  mrpt::math::TPose2D operator+(const mrpt::math::TPose2D& b) const; //!< Operator "oplus" pose composition: "ret=this \oplus b" \sa CPose2D
197  mrpt::math::TPose2D operator-(const mrpt::math::TPose2D& b) const; //!< Operator "ominus" pose composition: "ret=this \ominus b" \sa CPose2D
198  inline void composePoint(const TPoint2D l, TPoint2D &g) const {
199  const double ccos = ::cos(phi), csin=::sin(phi);
200  g.x = x+ l.x * ccos - l.y * csin;
201  g.y = y+ l.x * csin + l.y * ccos;
202  }
203  inline void inverseComposePoint(const TPoint2D g, TPoint2D &l) const {
204  const double Ax = g.x - x, Ay = g.y - y, ccos = ::cos(phi), csin=::sin(phi);
205  l.x = Ax * ccos + Ay * csin;
206  l.y =-Ax * csin + Ay * ccos;
207  }
208  /** Returns the norm of the (x,y) vector (phi is not used) */
209  inline double norm() const {
210  return mrpt::math::hypot_fast(x, y);
211  }
212 
213  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -45.0]" )
214  * \sa asString
215  * \exception std::exception On invalid format
216  */
217  void fromString(const std::string &s);
218  static size_t size() { return 3; }
219  };
220 
221  /** Lightweight 3D point (float version).
222  * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3D
223  */
225  {
226  enum { static_size = 3 };
227  float x;
228  float y;
229  float z;
230 
231  inline TPoint3Df() { }
232  inline TPoint3Df(const float xx,const float yy,const float zz) : x(xx), y(yy),z(zz) { }
233  inline TPoint3Df & operator +=(const TPoint3Df &p) { x+=p.x; y+=p.y; z+=p.z; return *this; }
234  inline TPoint3Df operator *(const float s) { return TPoint3Df(x*s,y*s,z*s); }
235  /** Coordinate access using operator[]. Order: x,y,z */
236  inline float &operator[](size_t i) { switch (i) { case 0: return x; case 1: return y; case 2: return z; default: throw std::out_of_range("index out of range"); } }
237 
238  /** Coordinate access using operator[]. Order: x,y,z */
239  inline const float &operator[](size_t i) const { switch (i) { case 0: return x; case 1: return y; case 2: return z; default: throw std::out_of_range("index out of range"); } }
240  };
241 
242  /**
243  * Lightweight 3D point. Allows coordinate access using [] operator.
244  * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3Df
245  */
247  enum { static_size = 3 };
248  double x,y,z; //!< X,Y,Z coordinates
249 
250  /** Constructor from coordinates. */
251  inline TPoint3D(double xx,double yy,double zz):x(xx),y(yy),z(zz) {}
252  /** Default fast constructor. Initializes to garbage. */
253  inline TPoint3D() {}
254  /** Explicit constructor from coordinates. */
255  explicit inline TPoint3D(const TPoint3Df &p):x(p.x),y(p.y),z(p.z) {}
256 
257  /** Implicit constructor from TPoint2D. Zeroes the z.
258  * \sa TPoint2D
259  */
260  TPoint3D(const TPoint2D &p);
261  /**
262  * Constructor from TPose2D, losing information. Zeroes the z.
263  * \sa TPose2D
264  */
265  explicit TPoint3D(const TPose2D &p);
266  /**
267  * Constructor from TPose3D, losing information.
268  * \sa TPose3D
269  */
270  explicit TPoint3D(const TPose3D &p);
271  /**
272  * Implicit constructor from heavyweight type.
273  * \sa mrpt::poses::CPoint3D
274  */
276  /**
277  * Constructor from heavyweight 3D pose.
278  * \sa mrpt::poses::CPose3D.
279  */
280  explicit TPoint3D(const mrpt::poses::CPose3D &p);
281  /** Coordinate access using operator[]. Order: x,y,z */
282  inline double &operator[](size_t i) { switch (i) { case 0: return x; case 1: return y; case 2: return z; default: throw std::out_of_range("index out of range"); } }
283  /** Coordinate access using operator[]. Order: x,y,z */
284  inline const double &operator[](size_t i) const { switch (i) { case 0: return x; case 1: return y; case 2: return z; default: throw std::out_of_range("index out of range"); } }
285  /**
286  * Point-to-point distance.
287  */
288  inline double distanceTo(const TPoint3D &p) const {
289  return sqrt(square(p.x-x)+square(p.y-y)+square(p.z-z));
290  }
291  /**
292  * Point-to-point distance, squared.
293  */
294  inline double sqrDistanceTo(const TPoint3D &p) const {
295  return square(p.x-x)+square(p.y-y)+square(p.z-z);
296  }
297  /**
298  * Point norm.
299  */
300  inline double norm() const {
301  return sqrt(square(x)+square(y)+square(z));
302  }
303  /**
304  * Point scale.
305  */
306  inline TPoint3D &operator*=(const double f) {
307  x*=f;y*=f;z*=f;
308  return *this;
309  }
310  /**
311  * Transformation into vector.
312  */
313  template <class VECTORLIKE>
314  void getAsVector(VECTORLIKE &v) const {
315  v.resize(3);
316  v[0]=x; v[1]=y; v[2]=z;
317  }
318  /**
319  * Translation.
320  */
321  inline TPoint3D &operator+=(const TPoint3D &p) {
322  x+=p.x;
323  y+=p.y;
324  z+=p.z;
325  return *this;
326  }
327  /**
328  * Difference between points.
329  */
330  inline TPoint3D &operator-=(const TPoint3D &p) {
331  x-=p.x;
332  y-=p.y;
333  z-=p.z;
334  return *this;
335  }
336  /**
337  * Points addition.
338  */
339  inline TPoint3D operator+(const TPoint3D &p) const {
340  return TPoint3D(x+p.x,y+p.y,z+p.z);
341  }
342  /**
343  * Points substraction.
344  */
345  inline TPoint3D operator-(const TPoint3D &p) const {
346  return TPoint3D(x-p.x,y-p.y,z-p.z);
347  }
348 
349  inline TPoint3D operator*(double d) const {
350  return TPoint3D(x*d,y*d,z*d);
351  }
352 
353  inline TPoint3D operator/(double d) const {
354  return TPoint3D(x/d,y/d,z/d);
355  }
356 
357  bool operator<(const TPoint3D &p) const;
358 
359  /** Returns a human-readable textual representation of the object (eg: "[0.02 1.04 -0.8]" )
360  * \sa fromString
361  */
362  void asString(std::string &s) const { s = mrpt::format("[%f %f %f]",x,y,z); }
363  inline std::string asString() const { std::string s; asString(s); return s; }
364 
365  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" )
366  * \sa asString
367  * \exception std::exception On invalid format
368  */
369  void fromString(const std::string &s);
370  static size_t size() { return 3; }
371  };
372 
373  /** XYZ point (double) + Intensity(u8) \sa mrpt::math::TPoint3D */
377  inline TPointXYZIu8() : pt(), intensity(0) {}
378  inline TPointXYZIu8(double x,double y,double z, uint8_t intensity_val) : pt(x,y,z),intensity(intensity_val) {}
379  };
380  /** XYZ point (double) + RGB(u8) \sa mrpt::math::TPoint3D */
383  uint8_t R,G,B;
384  inline TPointXYZRGBu8() : pt(), R(0),G(0),B(0) {}
385  inline TPointXYZRGBu8(double x,double y,double z, uint8_t R_val, uint8_t G_val, uint8_t B_val) : pt(x,y,z),R(R_val),G(G_val),B(B_val) {}
386  };
387  /** XYZ point (float) + Intensity(u8) \sa mrpt::math::TPoint3D */
391  inline TPointXYZfIu8() : pt(), intensity(0) {}
392  inline TPointXYZfIu8(float x,float y,float z, uint8_t intensity_val) : pt(x,y,z),intensity(intensity_val) {}
393  };
394  /** XYZ point (float) + RGB(u8) \sa mrpt::math::TPoint3D */
397  uint8_t R,G,B;
398  inline TPointXYZfRGBu8() : pt(), R(0),G(0),B(0) {}
399  inline TPointXYZfRGBu8(float x,float y,float z, uint8_t R_val, uint8_t G_val, uint8_t B_val) : pt(x,y,z),R(R_val),G(G_val),B(B_val) {}
400  };
401 
402  /**
403  * Lightweight 3D pose (three spatial coordinates, plus three angular coordinates). Allows coordinate access using [] operator.
404  * \sa mrpt::poses::CPose3D
405  */
407  enum { static_size = 6 };
408  double x,y,z; //!< X,Y,Z, coords
409  double yaw; //!< Yaw coordinate (rotation angle over Z axis).
410  double pitch; //!< Pitch coordinate (rotation angle over Y axis).
411  double roll; //!< Roll coordinate (rotation angle over X coordinate).
412  /** Implicit constructor from TPoint2D. Zeroes all the unprovided information.
413  * \sa TPoint2D
414  */
415  TPose3D(const TPoint2D &p);
416  /**
417  * Implicit constructor from TPose2D. Gets the yaw from the 2D pose's phi, zeroing all the unprovided information.
418  * \sa TPose2D
419  */
420  TPose3D(const TPose2D &p);
421  /**
422  * Implicit constructor from TPoint3D. Zeroes angular information.
423  * \sa TPoint3D
424  */
425  TPose3D(const TPoint3D &p);
426  /**
427  * Implicit constructor from heavyweight type.
428  * \sa mrpt::poses::CPose3D
429  */
431  /**
432  * Constructor from coordinates.
433  */
434  TPose3D(double _x,double _y,double _z,double _yaw,double _pitch,double _roll):x(_x),y(_y),z(_z),yaw(_yaw),pitch(_pitch),roll(_roll) {}
435  /**
436  * Default fast constructor. Initializes to garbage.
437  */
438  inline TPose3D() {}
439  /** Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll */
440  inline double &operator[](size_t i) { switch (i) { case 0: return x; case 1: return y; case 2: return z; case 3: return yaw; case 4: return pitch; case 5: return roll; default: throw std::out_of_range("index out of range"); } }
441  /** Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll */
442  inline const double &operator[](size_t i) const { switch (i) { case 0: return x; case 1: return y; case 2: return z; case 3: return yaw; case 4: return pitch; case 5: return roll; default: throw std::out_of_range("index out of range"); } }
443  /**
444  * Pose's spatial coordinates norm.
445  */
446  double norm() const {
447  return std::sqrt(square(x)+square(y)+square(z));
448  }
449  /**
450  * Gets the pose as a vector of doubles.
451  */
452  void getAsVector(std::vector<double> &v) const {
453  v.resize(6);
454  v[0]=x; v[1]=y; v[2]=z; v[3]=yaw; v[4]=pitch; v[5]=roll;
455  }
456  /** Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]", angles in degrees.)
457  * \sa fromString
458  */
459  void asString(std::string &s) const;
460  inline std::string asString() const { std::string s; asString(s); return s; }
461 
462  /** Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored)
463  * \f[ \mathbf{q} = \left( \begin{array}{c} \cos (\phi /2) \cos (\theta /2) \cos (\psi /2) + \sin (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \sin (\phi /2) \cos (\theta /2) \cos (\psi /2) - \cos (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \sin (\theta /2) \cos (\psi /2) + \sin (\phi /2) \cos (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \cos (\theta /2) \sin (\psi /2) - \sin (\phi /2) \sin (\theta /2) \cos (\psi /2) \\ \end{array}\right) \f]
464  * With : \f$ \phi = roll \f$, \f$ \theta = pitch \f$ and \f$ \psi = yaw \f$.
465  * \param out_dq_dr If provided, the 4x3 Jacobian of the transformation will be computed and stored here. It's the Jacobian of the transformation from (yaw pitch roll) to (qr qx qy qz).
466  */
467  void getAsQuaternion(
470  ) const;
471 
472  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" )
473  * \sa asString
474  * \exception std::exception On invalid format
475  */
476  void fromString(const std::string &s);
477  static size_t size() { return 6; }
478  };
479 
480  /** Lightweight 3D pose (three spatial coordinates, plus a quaternion ). Allows coordinate access using [] operator.
481  * \sa mrpt::poses::CPose3DQuat
482  */
484  enum { static_size = 7 };
485  double x,y,z; //!< Translation in x,y,z
486  double qr,qx,qy,qz; //!< Unit quaternion part, qr,qx,qy,qz
487 
488  /** Constructor from coordinates. */
489  inline TPose3DQuat(double _x,double _y,double _z,double _qr,double _qx, double _qy, double _qz):x(_x),y(_y),z(_z),qr(_qr),qx(_qx),qy(_qy),qz(_qz) { }
490  /** Default fast constructor. Initializes to garbage. */
491  inline TPose3DQuat() {}
492  /** Constructor from a CPose3DQuat */
494 
495  /** Coordinate access using operator[]. Order: x,y,z,qr,qx,qy,qz */
496  inline double &operator[](size_t i) { switch (i) { case 0: return x; case 1: return y; case 2: return z; case 3: return qr; case 4: return qx; case 5: return qy; case 6: return qz; default: throw std::out_of_range("index out of range"); } }
497  /** Coordinate access using operator[]. Order: x,y,z,qr,qx,qy,qz */
498  inline const double &operator[](size_t i) const { switch (i) { case 0: return x; case 1: return y; case 2: return z; case 3: return qr; case 4: return qx; case 5: return qy; case 6: return qz; default: throw std::out_of_range("index out of range"); } }
499  /** Pose's spatial coordinates norm. */
500  double norm() const {
501  return sqrt(square(x)+square(y)+square(z));
502  }
503  /** Gets the pose as a vector of doubles. */
504  void getAsVector(std::vector<double> &v) const {
505  v.resize(7);
506  for (size_t i=0;i<7;i++) v[i]=(*this)[i];
507  }
508  /** Returns a human-readable textual representation of the object as "[x y z qr qx qy qz]"
509  * \sa fromString
510  */
511  void asString(std::string &s) const { s = mrpt::format("[%f %f %f %f %f %f %f]",x,y,z,qr,qx,qy,qz); }
512  inline std::string asString() const { std::string s; asString(s); return s; }
513 
514  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8 1.0 0.0 0.0 0.0]" )
515  * \sa asString
516  * \exception std::exception On invalid format
517  */
518  void fromString(const std::string &s);
519  static size_t size() { return 7; }
520  };
521 
522  // Text streaming functions:
523  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPoint2D & p);
524  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPoint3D & p);
525  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose2D & p);
526  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose3D & p);
527  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose3DQuat & p);
528 
529 
530  /**
531  * Unary minus operator for 3D points.
532  */
533  inline TPoint3D operator-(const TPoint3D &p1) {
534  return TPoint3D(-p1.x,-p1.y,-p1.z);
535  }
536  /**
537  * Exact comparison between 2D points.
538  */
539  inline bool operator==(const TPoint2D &p1,const TPoint2D &p2) {
540  return (p1.x==p2.x)&&(p1.y==p2.y); //-V550
541  }
542  /**
543  * Exact comparison between 2D points.
544  */
545  inline bool operator!=(const TPoint2D &p1,const TPoint2D &p2) {
546  return (p1.x!=p2.x)||(p1.y!=p2.y); //-V550
547  }
548  /**
549  * Exact comparison between 3D points.
550  */
551  inline bool operator==(const TPoint3D &p1,const TPoint3D &p2) {
552  return (p1.x==p2.x)&&(p1.y==p2.y)&&(p1.z==p2.z); //-V550
553  }
554  /**
555  * Exact comparison between 3D points.
556  */
557  inline bool operator!=(const TPoint3D &p1,const TPoint3D &p2) {
558  return (p1.x!=p2.x)||(p1.y!=p2.y)||(p1.z!=p2.z); //-V550
559  }
560  /**
561  * Exact comparison between 2D poses, taking possible cycles into account.
562  */
563  inline bool operator==(const TPose2D &p1,const TPose2D &p2) {
564  return (p1.x==p2.x)&&(p1.y==p2.y)&&(mrpt::math::wrapTo2Pi(p1.phi)==mrpt::math::wrapTo2Pi(p2.phi)); //-V550
565  }
566  /**
567  * Exact comparison between 2D poses, taking possible cycles into account.
568  */
569  inline bool operator!=(const TPose2D &p1,const TPose2D &p2) {
570  return (p1.x!=p2.x)||(p1.y!=p2.y)||(mrpt::math::wrapTo2Pi(p1.phi)!=mrpt::math::wrapTo2Pi(p2.phi)); //-V550
571  }
572  /**
573  * Exact comparison between 3D poses, taking possible cycles into account.
574  */
575  inline bool operator==(const TPose3D &p1,const TPose3D &p2) {
576  return (p1.x==p2.x)&&(p1.y==p2.y)&&(p1.z==p2.z)&&(mrpt::math::wrapTo2Pi(p1.yaw)==mrpt::math::wrapTo2Pi(p2.yaw))&&(mrpt::math::wrapTo2Pi(p1.pitch)==mrpt::math::wrapTo2Pi(p2.pitch))&&(mrpt::math::wrapTo2Pi(p1.roll)==mrpt::math::wrapTo2Pi(p2.roll)); //-V550
577  }
578  /**
579  * Exact comparison between 3D poses, taking possible cycles into account.
580  */
581  inline bool operator!=(const TPose3D &p1,const TPose3D &p2) {
582  return (p1.x!=p2.x)||(p1.y!=p2.y)||(p1.z!=p2.z)||(mrpt::math::wrapTo2Pi(p1.yaw)!=mrpt::math::wrapTo2Pi(p2.yaw))||(mrpt::math::wrapTo2Pi(p1.pitch)!=mrpt::math::wrapTo2Pi(p2.pitch))||(mrpt::math::wrapTo2Pi(p1.roll)!=mrpt::math::wrapTo2Pi(p2.roll)); //-V550
583  }
584  //Forward declarations
589 
590  //Pragma defined to ensure no structure packing
591  /**
592  * 2D segment, consisting of two points.
593  * \sa TSegment3D,TLine2D,TPolygon2D,TPoint2D
594  */
596  public:
597  /**
598  * Origin point.
599  */
601  /**
602  * Destiny point.
603  */
605  /**
606  * Segment length.
607  */
608  double length() const;
609  /**
610  * Distance to point.
611  */
612  double distance(const TPoint2D &point) const;
613  /**
614  * Distance with sign to point (sign indicates which side the point is).
615  */
616  double signedDistance(const TPoint2D &point) const;
617  /**
618  * Check whether a point is inside a segment.
619  */
620  bool contains(const TPoint2D &point) const;
621  /** Access to points using operator[0-1] */
622  inline TPoint2D &operator[](size_t i) { switch (i) { case 0: return point1; case 1: return point2; default: throw std::out_of_range("index out of range"); } }
623  /** Access to points using operator[0-1] */
624  inline const TPoint2D &operator[](size_t i) const { switch (i) { case 0: return point1; case 1: return point2; default: throw std::out_of_range("index out of range"); } }
625  /**
626  * Project into 3D space, setting the z to 0.
627  */
628  void generate3DObject(TSegment3D &s) const;
629  /**
630  * Segment's central point.
631  */
632  inline void getCenter(TPoint2D &p) const {
633  p.x=(point1.x+point2.x)/2;
634  p.y=(point1.y+point2.y)/2;
635  }
636  /**
637  * Constructor from both points.
638  */
639  TSegment2D(const TPoint2D &p1,const TPoint2D &p2):point1(p1),point2(p2) {}
640  /**
641  * Fast default constructor. Initializes to garbage.
642  */
644  /**
645  * Explicit constructor from 3D object, discarding the z.
646  */
647  explicit TSegment2D(const TSegment3D &s);
648 
649  bool operator<(const TSegment2D &s) const;
650  };
651  /**
652  * 3D segment, consisting of two points.
653  * \sa TSegment2D,TLine3D,TPlane,TPolygon3D,TPoint3D
654  */
656  public:
657  /**
658  * Origin point.
659  */
661  /**
662  * Destiny point.
663  */
665  /**
666  * Segment length.
667  */
668  double length() const;
669  /**
670  * Distance to point.
671  */
672  double distance(const TPoint3D &point) const;
673  /**
674  * Distance to another segment.
675  */
676  double distance(const TSegment3D &segment) const;
677  /**
678  * Check whether a point is inside the segment.
679  */
680  bool contains(const TPoint3D &point) const;
681  /** Access to points using operator[0-1] */
682  inline TPoint3D &operator[](size_t i) { switch (i) { case 0: return point1; case 1: return point2; default: throw std::out_of_range("index out of range"); } }
683  /** Access to points using operator[0-1] */
684  inline const TPoint3D &operator[](size_t i) const { switch (i) { case 0: return point1; case 1: return point2; default: throw std::out_of_range("index out of range"); } }
685  /**
686  * Projection into 2D space, discarding the z.
687  */
688  inline void generate2DObject(TSegment2D &s) const {
689  s=TSegment2D(*this);
690  }
691  /**
692  * Segment's central point.
693  */
694  inline void getCenter(TPoint3D &p) const {
695  p.x=(point1.x+point2.x)/2;
696  p.y=(point1.y+point2.y)/2;
697  p.z=(point1.z+point2.z)/2;
698  }
699  /**
700  * Constructor from both points.
701  */
702  TSegment3D(const TPoint3D &p1,const TPoint3D &p2):point1(p1),point2(p2) {}
703  /**
704  * Fast default constructor. Initializes to garbage.
705  */
707  /**
708  * Constructor from 2D object. Sets the z to zero.
709  */
710  explicit TSegment3D(const TSegment2D &s):point1(s.point1),point2(s.point2) {}
711 
712  bool operator<(const TSegment3D &s) const;
713  };
714 
715  inline bool operator==(const TSegment2D &s1,const TSegment2D &s2) {
716  return (s1.point1==s2.point1)&&(s1.point2==s2.point2);
717  }
718 
719  inline bool operator!=(const TSegment2D &s1,const TSegment2D &s2) {
720  return (s1.point1!=s2.point1)||(s1.point2!=s2.point2);
721  }
722 
723  inline bool operator==(const TSegment3D &s1,const TSegment3D &s2) {
724  return (s1.point1==s2.point1)&&(s1.point2==s2.point2);
725  }
726 
727  inline bool operator!=(const TSegment3D &s1,const TSegment3D &s2) {
728  return (s1.point1!=s2.point1)||(s1.point2!=s2.point2);
729  }
730 
731  /**
732  * 2D line without bounds, represented by its equation \f$Ax+By+C=0\f$.
733  * \sa TLine3D,TSegment2D,TPolygon2D,TPoint2D
734  */
736  public:
737  /**
738  * Line coefficients, stored as an array: \f$\left[A,B,C\right]\f$.
739  */
740  double coefs[3];
741  /**
742  * Evaluate point in the line's equation.
743  */
744  double evaluatePoint(const TPoint2D &point) const;
745  /**
746  * Check whether a point is inside the line.
747  */
748  bool contains(const TPoint2D &point) const;
749  /**
750  * Distance from a given point.
751  */
752  double distance(const TPoint2D &point) const;
753  /**
754  * Distance with sign from a given point (sign indicates side).
755  */
756  double signedDistance(const TPoint2D &point) const;
757  /**
758  * Get line's normal vector.
759  */
760  void getNormalVector(double (&vector)[2]) const;
761  /**
762  * Unitarize line's normal vector.
763  */
764  void unitarize();
765  /**
766  * Get line's normal vector after unitarizing line.
767  */
768  inline void getUnitaryNormalVector(double (&vector)[2]) {
769  unitarize();
770  getNormalVector(vector);
771  }
772  /**
773  * Get line's director vector.
774  */
775  void getDirectorVector(double (&vector)[2]) const;
776  /**
777  * Unitarize line and then get director vector.
778  */
779  inline void getUnitaryDirectorVector(double (&vector)[2]) {
780  unitarize();
781  getDirectorVector(vector);
782  }
783  /**
784  * Project into 3D space, setting the z to 0.
785  */
786  void generate3DObject(TLine3D &l) const;
787  /**
788  * Get a pose2D whose X axis corresponds to the line.
789  * \sa mrpt::poses::CPose2D.
790  */
791  void getAsPose2D(mrpt::poses::CPose2D &outPose) const;
792  /**
793  * Get a pose2D whose X axis corresponds to the line, forcing the base point to one given.
794  * \throw logic_error if the point is not inside the line.
795  * \sa mrpt::poses::CPose2D.
796  */
797  void getAsPose2DForcingOrigin(const TPoint2D &origin,mrpt::poses::CPose2D &outPose) const;
798  /**
799  * Constructor from two points, through which the line will pass.
800  * \throw logic_error if both points are the same
801  */
802  TLine2D(const TPoint2D &p1,const TPoint2D &p2);
803  /**
804  * Constructor from a segment.
805  */
806  explicit TLine2D(const TSegment2D &s);
807  /**
808  * Fast default constructor. Initializes to garbage.
809  */
810  TLine2D() {}
811  /**
812  * Constructor from line's coefficients.
813  */
814  inline TLine2D(double A,double B,double C) {
815  coefs[0]=A;
816  coefs[1]=B;
817  coefs[2]=C;
818  }
819  /**
820  * Construction from 3D object, discarding the Z.
821  * \throw std::logic_error if the line is normal to the XY plane.
822  */
823  explicit TLine2D(const TLine3D &l);
824  };
825 
826  /**
827  * 3D line, represented by a base point and a director vector.
828  * \sa TLine2D,TSegment3D,TPlane,TPolygon3D,TPoint3D
829  */
831  public:
832  /**
833  * Base point.
834  */
836  /**
837  * Director vector.
838  */
839  double director[3];
840  /**
841  * Check whether a point is inside the line.
842  */
843  bool contains(const TPoint3D &point) const;
844  /**
845  * Distance between the line and a point.
846  */
847  double distance(const TPoint3D &point) const;
848  /**
849  * Unitarize director vector.
850  */
851  void unitarize();
852  /**
853  * Get director vector.
854  */
855  inline void getDirectorVector(double (&vector)[3]) const {
856  for (size_t i=0;i<3;i++) vector[i]=director[i];
857  }
858  /**
859  * Unitarize and then get director vector.
860  */
861  inline void getUnitaryDirectorVector(double (&vector)[3]) {
862  unitarize();
863  getDirectorVector(vector);
864  }
865  /**
866  * Project into 2D space, discarding the Z coordinate.
867  * \throw std::logic_error if the line's director vector is orthogonal to the XY plane.
868  */
869  inline void generate2DObject(TLine2D &l) const {
870  l=TLine2D(*this);
871  }
872  /**
873  * Constructor from two points, through which the line will pass.
874  * \throw std::logic_error if both points are the same.
875  */
876  TLine3D(const TPoint3D &p1,const TPoint3D &p2);
877  /**
878  * Constructor from 3D segment.
879  */
880  explicit TLine3D(const TSegment3D &s);
881  /**
882  * Fast default constructor. Initializes to garbage.
883  */
884  TLine3D() {}
885  /** Constructor from 2D object. Zeroes the z. */
886  explicit TLine3D(const TLine2D &l);
887  };
888 
889  /**
890  * 3D Plane, represented by its equation \f$Ax+By+Cz+D=0\f$
891  * \sa TSegment3D,TLine3D,TPolygon3D,TPoint3D
892  */
894  public:
895  /**
896  * Plane coefficients, stored as an array: \f$\left[A,B,C,D\right]\f$
897  */
898  double coefs[4];
899  /**
900  * Evaluate a point in the plane's equation.
901  */
902  double evaluatePoint(const TPoint3D &point) const;
903  /**
904  * Check whether a point is contained into the plane.
905  */
906  bool contains(const TPoint3D &point) const;
907  /**
908  * Check whether a segment is fully contained into the plane.
909  */
910  inline bool contains(const TSegment3D &segment) const {
911  return contains(segment.point1)&&contains(segment.point2);
912  }
913  /**
914  * Check whether a line is fully contained into the plane.
915  */
916  bool contains(const TLine3D &line) const;
917  /**
918  * Distance to 3D point.
919  */
920  double distance(const TPoint3D &point) const;
921  /**
922  * Distance to 3D line. Will be zero if the line is not parallel to the plane.
923  */
924  double distance(const TLine3D &line) const;
925  /**
926  * Get plane's normal vector.
927  */
928  void getNormalVector(double (&vec)[3]) const;
929  /**
930  * Unitarize normal vector.
931  */
932  void unitarize();
933  /**
934  * Unitarize, then get normal vector.
935  */
936  inline void getUnitaryNormalVector(double (&vec)[3]) {
937  unitarize();
938  getNormalVector(vec);
939  }
940  /**
941  * Gets a pose whose XY plane corresponds to this plane.
942  */
943  void getAsPose3D(mrpt::poses::CPose3D &outPose);
944  /**
945  * Gets a pose whose XY plane corresponds to this plane.
946  */
947  inline void getAsPose3D(mrpt::poses::CPose3D &outPose) const {
948  TPlane p=*this;
949  p.getAsPose3D(outPose);
950  }
951  /**
952  * Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates.
953  * \throw std::logic_error if the point is not inside the plane.
954  */
955  void getAsPose3DForcingOrigin(const TPoint3D &newOrigin,mrpt::poses::CPose3D &pose);
956  /**
957  * Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates.
958  * \throw std::logic_error if the point is not inside the plane.
959  */
960  inline void getAsPose3DForcingOrigin(const TPoint3D &newOrigin,mrpt::poses::CPose3D &pose) const {
961  TPlane p=*this;
962  p.getAsPose3DForcingOrigin(newOrigin,pose);
963  }
964  /**
965  * Gets a plane which contains these three points.
966  * \throw std::logic_error if the points are linearly dependants.
967  */
968  TPlane(const TPoint3D &p1,const TPoint3D &p2,const TPoint3D &p3);
969  /**
970  * Gets a plane which contains this point and this line.
971  * \throw std::logic_error if the point is inside the line.
972  */
973  TPlane(const TPoint3D &p1,const TLine3D &r2);
974  /**
975  * Gets a plane which contains the two lines.
976  * \throw std::logic_error if the lines do not cross.
977  */
978  TPlane(const TLine3D &r1,const TLine3D &r2);
979  /**
980  * Fast default constructor. Initializes to garbage.
981  */
982  TPlane() {}
983  /**
984  * Constructor from plane coefficients.
985  */
986  inline TPlane(double A,double B,double C,double D) {
987  coefs[0]=A;
988  coefs[1]=B;
989  coefs[2]=C;
990  coefs[3]=D;
991  }
992  /**
993  * Constructor from an array of coefficients.
994  */
995  inline TPlane(const double (&vec)[4]) {
996  for (size_t i=0;i<4;i++) coefs[i]=vec[i];
997  }
998  };
999 
1000  typedef TPlane TPlane3D;
1001 
1002  /**
1003  * 2D polygon, inheriting from std::vector<TPoint2D>.
1004  * \sa TPolygon3D,TSegment2D,TLine2D,TPoint2D, CPolygon
1005  */
1006  class BASE_IMPEXP TPolygon2D:public std::vector<TPoint2D> {
1007  public:
1008  double distance(const TPoint2D &point) const; //!< Distance to a point (always >=0)
1009  bool contains(const TPoint2D &point) const; //!< Check whether a point is inside (or within geometryEpsilon of a polygon edge). This works for concave or convex polygons.
1010  void getAsSegmentList(std::vector<TSegment2D> &v) const; //!< Gets as set of segments, instead of points.
1011  void generate3DObject(TPolygon3D &p) const; //!< Projects into 3D space, zeroing the z.
1012  void getCenter(TPoint2D &p) const; //!< Polygon's central point.
1013  bool isConvex() const; //!< Checks whether is convex.
1014  void removeRepeatedVertices(); //!< Erase repeated vertices. \sa removeRedundantVertices
1015  void removeRedundantVertices(); //!< Erase every redundant vertex from the polygon, saving space. \sa removeRepeatedVertices
1016  void getPlotData(std::vector<double> &x,std::vector<double> &y) const; //!< Gets plot data, ready to use on a 2D plot. \sa mrpt::gui::CDisplayWindowPlots
1017  void getBoundingBox(TPoint2D &min_coords, TPoint2D&max_coords) const; //!< Get polygon bounding box. \exception On empty polygon
1018  /** Default constructor */
1019  TPolygon2D():std::vector<TPoint2D>() {}
1020  /** Constructor for a given number of vertices, intializing them as garbage. */
1021  explicit TPolygon2D(size_t N):std::vector<TPoint2D>(N) {}
1022  /** Implicit constructor from a vector of 2D points */
1023  TPolygon2D(const std::vector<TPoint2D> &v):std::vector<TPoint2D>(v) {}
1024  /** Constructor from a 3D object. */
1025  explicit TPolygon2D(const TPolygon3D &p);
1026  /** Static method to create a regular polygon, given its size and radius.
1027  * \throw std::logic_error if radius is near zero or the number of edges is less than three.
1028  */
1029  static void createRegularPolygon(size_t numEdges,double radius,TPolygon2D &poly);
1030  /** Static method to create a regular polygon from its size and radius. The center will correspond to the given pose.
1031  * \throw std::logic_error if radius is near zero or the number of edges is less than three.
1032  */
1033  static inline void createRegularPolygon(size_t numEdges,double radius,TPolygon2D &poly,const mrpt::poses::CPose2D &pose);
1034  };
1035 
1036  /**
1037  * 3D polygon, inheriting from std::vector<TPoint3D>
1038  * \sa TPolygon2D,TSegment3D,TLine3D,TPlane,TPoint3D
1039  */
1040  class BASE_IMPEXP TPolygon3D:public std::vector<TPoint3D> {
1041  public:
1042  double distance(const TPoint3D &point) const; //!< Distance to point (always >=0)
1043  bool contains(const TPoint3D &point) const; //!< Check whether a point is inside (or within geometryEpsilon of a polygon edge). This works for concave or convex polygons.
1044  void getAsSegmentList(std::vector<TSegment3D> &v) const; //!< Gets as set of segments, instead of set of points.
1045  bool getPlane(TPlane &p) const; //!< Gets a plane which contains the polygon. Returns false if the polygon is skew and cannot be fit inside a plane.
1046  /** Gets the best fitting plane, disregarding whether the polygon actually fits inside or not. \sa getBestFittingPlane */
1047  void getBestFittingPlane(TPlane &p) const;
1048  /** Projects into a 2D space, discarding the z. \sa getPlane,isSkew */
1049  inline void generate2DObject(TPolygon2D &p) const {
1050  p=TPolygon2D(*this);
1051  }
1052  void getCenter(TPoint3D &p) const; //!< Get polygon's central point.
1053  bool isSkew() const; //!< Check whether the polygon is skew. Returns true if there doesn't exist a plane in which the polygon can fit. \sa getBestFittingPlane
1054  void removeRepeatedVertices(); //!< Remove polygon's repeated vertices.
1055  void removeRedundantVertices(); //!< Erase every redundant vertex, thus saving space.
1056  /** Default constructor. Creates a polygon with no vertices. */
1057  TPolygon3D():std::vector<TPoint3D>() {}
1058  /** Constructor for a given size. Creates a polygon with a fixed number of vertices, which are initialized to garbage. */
1059  explicit TPolygon3D(size_t N):std::vector<TPoint3D>(N) {}
1060  /** Implicit constructor from a 3D points vector. */
1061  TPolygon3D(const std::vector<TPoint3D> &v):std::vector<TPoint3D>(v) {}
1062  /** Constructor from a 2D object. Zeroes the z. */
1063  explicit TPolygon3D(const TPolygon2D &p);
1064  /** Static method to create a regular polygon, given its size and radius.
1065  * \throw std::logic_error if number of edges is less than three, or radius is near zero. */
1066  static void createRegularPolygon(size_t numEdges,double radius,TPolygon3D &poly);
1067  /** Static method to create a regular polygon, given its size and radius. The center will be located on the given pose.
1068  * \throw std::logic_error if number of edges is less than three, or radius is near zero.
1069  */
1070  static inline void createRegularPolygon(size_t numEdges,double radius,TPolygon3D &poly,const mrpt::poses::CPose3D &pose);
1071  };
1072 
1073  /**
1074  * Object type identifier for TPoint2D or TPoint3D.
1075  * \sa TObject2D,TObject3D
1076  */
1077  const unsigned char GEOMETRIC_TYPE_POINT=0;
1078  /**
1079  * Object type identifier for TSegment2D or TSegment3D.
1080  * \sa TObject2D,TObject3D
1081  */
1082  const unsigned char GEOMETRIC_TYPE_SEGMENT=1;
1083  /**
1084  * Object type identifier for TLine2D or TLine3D.
1085  * \sa TObject2D,TObject3D
1086  */
1087  const unsigned char GEOMETRIC_TYPE_LINE=2;
1088  /**
1089  * Object type identifier for TPolygon2D or TPolygon3D.
1090  * \sa TObject2D,TObject3D
1091  */
1092  const unsigned char GEOMETRIC_TYPE_POLYGON=3;
1093  /**
1094  * Object type identifier for TPlane.
1095  * \sa TObject3D
1096  */
1097  const unsigned char GEOMETRIC_TYPE_PLANE=4;
1098  /**
1099  * Object type identifier for empty TObject2D or TObject3D.
1100  * \sa TObject2D,TObject3D
1101  */
1102  const unsigned char GEOMETRIC_TYPE_UNDEFINED=255;
1103 
1104  /**
1105  * Standard type for storing any lightweight 2D type. Do not inherit from this class.
1106  * \sa TPoint2D,TSegment2D,TLine2D,TPolygon2D
1107  */
1109  private:
1110  /**
1111  * Object type identifier.
1112  */
1113  unsigned char type;
1114  /**
1115  * Union type storing pointers to every allowed type.
1116  */
1122 
1123  tobject2d_data_t() : polygon(NULL) { }
1124  } data;
1125  /**
1126  * Destroys the object, releasing the pointer to the content (if any).
1127  */
1128  inline void destroy() {
1129  if (type==GEOMETRIC_TYPE_POLYGON) delete data.polygon;
1131  }
1132  public:
1133  /**
1134  * Implicit constructor from point.
1135  */
1137  data.point=p;
1138  }
1139  /**
1140  * Implicit constructor from segment.
1141  */
1143  data.segment=s;
1144  }
1145  /**
1146  * Implicit constructor from line.
1147  */
1149  data.line=r;
1150  }
1151  /**
1152  * Implicit constructor from polygon.
1153  */
1155  data.polygon=new TPolygon2D(p);
1156  }
1157  /**
1158  * Implicit constructor from polygon.
1159  */
1161  /**
1162  * Object destruction.
1163  */
1165  destroy();
1166  }
1167  /**
1168  * Checks whether content is a point.
1169  */
1170  inline bool isPoint() const {
1171  return type==GEOMETRIC_TYPE_POINT;
1172  }
1173  /**
1174  * Checks whether content is a segment.
1175  */
1176  inline bool isSegment() const {
1177  return type==GEOMETRIC_TYPE_SEGMENT;
1178  }
1179  /**
1180  * Checks whether content is a line.
1181  */
1182  inline bool isLine() const {
1183  return type==GEOMETRIC_TYPE_LINE;
1184  }
1185  /**
1186  * Checks whether content is a polygon.
1187  */
1188  inline bool isPolygon() const {
1189  return type==GEOMETRIC_TYPE_POLYGON;
1190  }
1191  /**
1192  * Gets content type.
1193  */
1194  inline unsigned char getType() const {
1195  return type;
1196  }
1197  /**
1198  * Gets the content as a point, returning false if the type is inadequate.
1199  */
1200  inline bool getPoint(TPoint2D &p) const {
1201  if (isPoint()) {
1202  p=data.point;
1203  return true;
1204  } else return false;
1205  }
1206  /**
1207  * Gets the content as a segment, returning false if the type is inadequate.
1208  */
1209  inline bool getSegment(TSegment2D &s) const {
1210  if (isSegment()) {
1211  s=data.segment;
1212  return true;
1213  } else return false;
1214  }
1215  /**
1216  * Gets the content as a line, returning false if the type is inadequate.
1217  */
1218  inline bool getLine(TLine2D &r) const {
1219  if (isLine()) {
1220  r=data.line;
1221  return true;
1222  } else return false;
1223  }
1224  /**
1225  * Gets the content as a polygon, returning false if the type is inadequate.
1226  */
1227  inline bool getPolygon(TPolygon2D &p) const {
1228  if (isPolygon()) {
1229  p=*(data.polygon);
1230  return true;
1231  } else return false;
1232  }
1233  /**
1234  * Assign another TObject2D. Pointers are not shared.
1235  */
1237  if (this==&obj) return *this;
1238  destroy();
1239  switch (type=obj.type) {
1240  case GEOMETRIC_TYPE_POINT:
1241  data.point=obj.data.point;
1242  break;
1244  data.segment=obj.data.segment;
1245  break;
1246  case GEOMETRIC_TYPE_LINE:
1247  data.line=obj.data.line;
1248  break;
1250  data.polygon=new TPolygon2D(*(obj.data.polygon));
1251  break;
1252  }
1253  return *this;
1254  }
1255  /**
1256  * Assign a point to this object.
1257  */
1258  inline void operator=(const TPoint2D &p) {
1259  destroy();
1261  data.point=p;
1262  }
1263  /**
1264  * Assign a segment to this object.
1265  */
1266  inline void operator=(const TSegment2D &s) {
1267  destroy();
1269  data.segment=s;
1270  }
1271  /**
1272  * Assign a line to this object.
1273  */
1274  inline void operator=(const TLine2D &l) {
1275  destroy();
1277  data.line=l;
1278  }
1279  /**
1280  * Assign a polygon to this object.
1281  */
1282  inline void operator=(const TPolygon2D &p) {
1283  destroy();
1285  data.polygon=new TPolygon2D(p);
1286  }
1287  /**
1288  * Project into 3D space.
1289  */
1290  void generate3DObject(TObject3D &obj) const;
1291  /**
1292  * Constructor from another TObject2D.
1293  */
1295  operator=(obj);
1296  }
1297  /**
1298  * Static method to retrieve all the points in a vector of TObject2D.
1299  */
1300  static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts);
1301  /**
1302  * Static method to retrieve all the segments in a vector of TObject2D.
1303  */
1304  static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms);
1305  /**
1306  * Static method to retrieve all the lines in a vector of TObject2D.
1307  */
1308  static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins);
1309  /**
1310  * Static method to retrieve all the polygons in a vector of TObject2D.
1311  */
1312  static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys);
1313  /**
1314  * Static method to retrieve all the points in a vector of TObject2D, returning the remainder objects in another parameter.
1315  */
1316  static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts,std::vector<TObject2D> &remainder);
1317  /**
1318  * Static method to retrieve all the segments in a vector of TObject2D, returning the remainder objects in another parameter.
1319  */
1320  static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms,std::vector<TObject2D> &remainder);
1321  /**
1322  * Static method to retrieve all the lines in a vector of TObject2D, returning the remainder objects in another parameter.
1323  */
1324  static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins,std::vector<TObject2D> &remainder);
1325  /**
1326  * Static method to retrieve all the polygons in a vector of TObject2D, returning the remainder objects in another parameter.
1327  */
1328  static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys,std::vector<TObject2D> &remainder);
1329  };
1330  /**
1331  * Standard object for storing any 3D lightweight object. Do not inherit from this class.
1332  * \sa TPoint3D,TSegment3D,TLine3D,TPlane,TPolygon3D
1333  */
1335  private:
1336  /**
1337  * Object type identifier.
1338  */
1339  unsigned char type;
1340  /**
1341  * Union containing pointer to actual data.
1342  */
1349 
1350  tobject3d_data_t() : polygon(NULL) { }
1351  } data;
1352  /**
1353  * Destroys the object and releases the pointer, if any.
1354  */
1355  void destroy() {
1356  if (type==GEOMETRIC_TYPE_POLYGON) delete data.polygon;
1358  }
1359  public:
1360  /**
1361  * Constructor from point.
1362  */
1364  data.point=p;
1365  }
1366  /**
1367  * Constructor from segment.
1368  */
1370  data.segment=s;
1371  }
1372  /**
1373  * Constructor from line.
1374  */
1376  data.line=r;
1377  }
1378  /**
1379  * Constructor from polygon.
1380  */
1382  data.polygon=new TPolygon3D(p);
1383  }
1384  /**
1385  * Constructor from plane.
1386  */
1388  data.plane=p;
1389  }
1390  /**
1391  * Empty constructor.
1392  */
1394  /**
1395  * Destructor.
1396  */
1398  destroy();
1399  }
1400  /**
1401  * Checks whether content is a point.
1402  */
1403  inline bool isPoint() const {
1404  return type==GEOMETRIC_TYPE_POINT;
1405  }
1406  /**
1407  * Checks whether content is a segment.
1408  */
1409  inline bool isSegment() const {
1410  return type==GEOMETRIC_TYPE_SEGMENT;
1411  }
1412  /**
1413  * Checks whether content is a line.
1414  */
1415  inline bool isLine() const {
1416  return type==GEOMETRIC_TYPE_LINE;
1417  }
1418  /**
1419  * Checks whether content is a polygon.
1420  */
1421  inline bool isPolygon() const {
1422  return type==GEOMETRIC_TYPE_POLYGON;
1423  }
1424  /**
1425  * Checks whether content is a plane.
1426  */
1427  inline bool isPlane() const {
1428  return type==GEOMETRIC_TYPE_PLANE;
1429  }
1430  /**
1431  * Gets object type.
1432  */
1433  inline unsigned char getType() const {
1434  return type;
1435  }
1436  /**
1437  * Gets the content as a point, returning false if the type is not adequate.
1438  */
1439  inline bool getPoint(TPoint3D &p) const {
1440  if (isPoint()) {
1441  p=data.point;
1442  return true;
1443  } else return false;
1444  }
1445  /**
1446  * Gets the content as a segment, returning false if the type is not adequate.
1447  */
1448  inline bool getSegment(TSegment3D &s) const {
1449  if (isSegment()) {
1450  s=data.segment;
1451  return true;
1452  } else return false;
1453  }
1454  /**
1455  * Gets the content as a line, returning false if the type is not adequate.
1456  */
1457  inline bool getLine(TLine3D &r) const {
1458  if (isLine()) {
1459  r=data.line;
1460  return true;
1461  } else return false;
1462  }
1463  /**
1464  * Gets the content as a polygon, returning false if the type is not adequate.
1465  */
1466  inline bool getPolygon(TPolygon3D &p) const {
1467  if (isPolygon()) {
1468  p=*(data.polygon);
1469  return true;
1470  } else return false;
1471  }
1472  /**
1473  * Gets the content as a plane, returning false if the type is not adequate.
1474  */
1475  inline bool getPlane(TPlane &p) const {
1476  if (isPlane()) {
1477  p=data.plane;
1478  return true;
1479  } else return false;
1480  }
1481  /**
1482  * Assigns another object, creating a new pointer if needed.
1483  */
1485  if (this==&obj) return *this;
1486  destroy();
1487  switch (type=obj.type) {
1488  case GEOMETRIC_TYPE_POINT:
1489  data.point=obj.data.point;
1490  break;
1492  data.segment=obj.data.segment;
1493  break;
1494  case GEOMETRIC_TYPE_LINE:
1495  data.line=obj.data.line;
1496  break;
1498  data.polygon=new TPolygon3D(*(obj.data.polygon));
1499  break;
1500  case GEOMETRIC_TYPE_PLANE:
1501  data.plane=obj.data.plane;
1502  break;
1504  break;
1505  default:
1506  THROW_EXCEPTION("Invalid TObject3D object");
1507  }
1508  return *this;
1509  }
1510  /**
1511  * Assigns a point to this object.
1512  */
1513  inline void operator=(const TPoint3D &p) {
1514  destroy();
1516  data.point=p;
1517  }
1518  /**
1519  * Assigns a segment to this object.
1520  */
1521  inline void operator=(const TSegment3D &s) {
1522  destroy();
1524  data.segment=s;
1525  }
1526  /**
1527  * Assigns a line to this object.
1528  */
1529  inline void operator=(const TLine3D &l) {
1530  destroy();
1532  data.line=l;
1533  }
1534  /**
1535  * Assigns a polygon to this object.
1536  */
1537  inline void operator=(const TPolygon3D &p) {
1538  destroy();
1540  data.polygon=new TPolygon3D(p);
1541  }
1542  /**
1543  * Assigns a plane to this object.
1544  */
1545  inline void operator=(const TPlane &p) {
1546  destroy();
1548  data.plane=p;
1549  }
1550  /**
1551  * Projects into 2D space.
1552  * \throw std::logic_error if the 3D object loses its properties when projecting into 2D space (for example, it's a plane or a vertical line).
1553  */
1554  inline void generate2DObject(TObject2D &obj) const {
1555  switch (type) {
1556  case GEOMETRIC_TYPE_POINT:
1557  obj=TPoint2D(data.point);
1558  break;
1560  obj=TSegment2D(data.segment);
1561  break;
1562  case GEOMETRIC_TYPE_LINE:
1563  obj=TLine2D(data.line);
1564  break;
1566  obj=TPolygon2D(*(data.polygon));
1567  break;
1568  case GEOMETRIC_TYPE_PLANE:
1569  throw std::logic_error("Too many dimensions");
1570  default:
1571  obj=TObject2D();
1572  break;
1573  }
1574  }
1575  /**
1576  * Constructs from another object.
1577  */
1579  operator=(obj);
1580  }
1581  /**
1582  * Static method to retrieve every point included in a vector of objects.
1583  */
1584  static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts);
1585  /**
1586  * Static method to retrieve every segment included in a vector of objects.
1587  */
1588  static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms);
1589  /**
1590  * Static method to retrieve every line included in a vector of objects.
1591  */
1592  static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins);
1593  /**
1594  * Static method to retrieve every plane included in a vector of objects.
1595  */
1596  static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns);
1597  /**
1598  * Static method to retrieve every polygon included in a vector of objects.
1599  */
1600  static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys);
1601  /**
1602  * Static method to retrieve every point included in a vector of objects, returning the remaining objects in another argument.
1603  */
1604  static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts,std::vector<TObject3D> &remainder);
1605  /**
1606  * Static method to retrieve every segment included in a vector of objects, returning the remaining objects in another argument.
1607  */
1608  static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms,std::vector<TObject3D> &remainder);
1609  /**
1610  * Static method to retrieve every line included in a vector of objects, returning the remaining objects in another argument.
1611  */
1612  static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins,std::vector<TObject3D> &remainder);
1613  /**
1614  * Static method to retrieve every plane included in a vector of objects, returning the remaining objects in another argument.
1615  */
1616  static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns,std::vector<TObject3D> &remainder);
1617  /**
1618  * Static method to retrieve every polygon included in a vector of objects, returning the remaining objects in another argument.
1619  */
1620  static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder);
1621  };
1622 
1623  /** 2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
1624  * \sa mrpt::math::TTwist3D, mrpt::math::TPose2D
1625  */
1627  enum { static_size = 3 };
1628  double vx,vy; //!< Velocity components: X,Y (m/s)
1629  double omega; //!< Angular velocity (rad/s)
1630 
1631  /** Constructor from components */
1632  inline TTwist2D(double vx_,double vy_,double omega_):vx(vx_),vy(vy_),omega(omega_) {}
1633  /** Default fast constructor. Initializes to garbage */
1634  inline TTwist2D() {}
1635  /** Coordinate access using operator[]. Order: vx,vy,vphi */
1636  inline double &operator[](size_t i) { switch (i) { case 0: return vx; case 1: return vy; case 2: return omega; default: throw std::out_of_range("index out of range"); } }
1637  /** Coordinate access using operator[]. Order: vx,vy,vphi */
1638  inline const double &operator[](size_t i) const { switch (i) { case 0: return vx; case 1: return vy; case 2: return omega; default: throw std::out_of_range("index out of range"); } }
1639  /** Transformation into vector */
1640  inline void getAsVector(std::vector<double> &v) const {
1641  v.resize(3);
1642  v[0]=vx; v[1]=vy; v[2]=omega;
1643  }
1644  /** Transform the (vx,vy) components for a counterclockwise rotation of `ang` radians. */
1645  void rotate(const double ang);
1646  bool operator ==(const TTwist2D&o) const;
1647  bool operator !=(const TTwist2D&o) const;
1648  mrpt::math::TPose2D operator *(const double dt) const; //!< Returns the pose increment of multiplying each twist component times "dt" seconds.
1649  /** Returns a human-readable textual representation of the object (eg: "[vx vy omega]", omega in deg/s)
1650  * \sa fromString
1651  */
1652  void asString(std::string &s) const;
1653  inline std::string asString() const { std::string s; asString(s); return s; }
1654 
1655  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -45.0]" )
1656  * \sa asString
1657  * \exception std::exception On invalid format
1658  */
1659  void fromString(const std::string &s);
1660  static size_t size() { return 3; }
1661  };
1662 
1663  /** 3D twist: 3D velocity vector (vx,vy,vz) + angular velocity (wx,wy,wz)
1664  * \sa mrpt::math::TTwist2D, mrpt::math::TPose3D
1665  */
1667  enum { static_size = 6 };
1668  double vx,vy,vz; //!< Velocity components: X,Y (m/s)
1669  double wx,wy,wz; //!< Angular velocity (rad/s)
1670 
1671  /** Constructor from components */
1672  inline TTwist3D(double vx_,double vy_,double vz_,double wx_,double wy_,double wz_):vx(vx_),vy(vy_),vz(vz_),wx(wx_),wy(wy_),wz(wz_) {}
1673  /** Default fast constructor. Initializes to garbage */
1674  inline TTwist3D() {}
1675  /** Coordinate access using operator[]. Order: vx,vy,vphi */
1676  inline double &operator[](size_t i) { switch (i) { case 0: return vx; case 1: return vy; case 2: return vz; case 3: return wx; case 4: return wy; case 5: return wz; default: throw std::out_of_range("index out of range"); } }
1677  /** Coordinate access using operator[]. Order: vx,vy,vphi */
1678  inline const double &operator[](size_t i) const { switch (i) { case 0: return vx; case 1: return vy; case 2: return vz; case 3: return wx; case 4: return wy; case 5: return wz; default: throw std::out_of_range("index out of range"); } }
1679  /** Transformation into vector */
1680  inline void getAsVector(std::vector<double> &v) const {
1681  v.resize(6);
1682  for (int i=0;i<6;i++) v[i]=(*this)[i];
1683  }
1684  bool operator ==(const TTwist3D&o) const;
1685  bool operator !=(const TTwist3D&o) const;
1686  /** Returns a human-readable textual representation of the object (eg: "[vx vy vz wx wy wz]", omegas in deg/s)
1687  * \sa fromString
1688  */
1689  void asString(std::string &s) const;
1690  inline std::string asString() const { std::string s; asString(s); return s; }
1691 
1692  /** Transform all 6 components for a change of reference frame from "A" to
1693  * another frame "B" whose rotation with respect to "A" is given by `rot`. The translational part of the pose is ignored */
1694  void rotate(const mrpt::poses::CPose3D & rot);
1695 
1696  /** Set the current object value from a string generated by 'asString' (eg: "[vx vy vz wx wy wz]" )
1697  * \sa asString
1698  * \exception std::exception On invalid format
1699  */
1700  void fromString(const std::string &s);
1701  static size_t size() { return 3; }
1702  };
1703 
1704  // Binary streaming functions
1707 
1710 
1713 
1716 
1719 
1722 
1725 
1728 
1731 
1734 
1737 
1740 
1743 
1744  /** @} */ // end of grouping
1745 
1746  } //end of namespace math
1747 
1748  namespace utils
1749  {
1750  // Specialization must occur in the same namespace
1766 
1767  } // end of namespace utils
1768 
1769 } //end of namespace
1770 #endif
void getCenter(TPoint2D &p) const
Segment&#39;s central point.
TSegment2D(const TPoint2D &p1, const TPoint2D &p2)
Constructor from both points.
TPoint2D & operator+=(const TPoint2D &p)
const double & operator[](size_t i) const
Coordinate access using operator[].
double & operator[](size_t i)
Coordinate access using operator[].
const unsigned char GEOMETRIC_TYPE_PLANE
Object type identifier for TPlane.
bool getPoint(TPoint2D &p) const
Gets the content as a point, returning false if the type is inadequate.
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:21
void getUnitaryNormalVector(double(&vector)[2])
Get line&#39;s normal vector after unitarizing line.
bool operator==(const TPoint2D &p1, const TPoint2D &p2)
Exact comparison between 2D points.
TPolygon3D(const std::vector< TPoint3D > &v)
Implicit constructor from a 3D points vector.
TPoint2D operator/(double d) const
#define MRPT_DECLARE_TTYPENAME_NAMESPACE(_TYPE, __NS)
Definition: TTypeName.h:64
TPointXYZIu8(double x, double y, double z, uint8_t intensity_val)
bool getPolygon(TPolygon2D &p) const
Gets the content as a polygon, returning false if the type is inadequate.
double norm() const
Returns the norm of the (x,y) vector (phi is not used)
TPoint2D & operator*=(double d)
double y
X,Y coordinates.
const unsigned char GEOMETRIC_TYPE_LINE
Object type identifier for TLine2D or TLine3D.
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[0.02 1.04]" ) ...
GLdouble GLdouble z
Definition: glext.h:3734
TPointXYZfIu8(float x, float y, float z, uint8_t intensity_val)
unsigned char getType() const
Gets content type.
bool isPolygon() const
Checks whether content is a polygon.
void destroy()
Destroys the object and releases the pointer, if any.
const unsigned char GEOMETRIC_TYPE_POINT
Object type identifier for TPoint2D or TPoint3D.
void composePoint(const TPoint2D l, TPoint2D &g) const
double wz
Angular velocity (rad/s)
void getPlanes(const std::vector< TPolygon3D > &polys, std::vector< TPlane > &planes)
Definition: geometry.cpp:1351
TObject2D(const TPoint2D &p)
Implicit constructor from point.
TPoint3Df(const float xx, const float yy, const float zz)
TLine2D()
Fast default constructor.
bool getSegment(TSegment3D &s) const
Gets the content as a segment, returning false if the type is not adequate.
bool contains(const TSegment3D &segment) const
Check whether a segment is fully contained into the plane.
double roll
Roll coordinate (rotation angle over X coordinate).
TPoint3D(double xx, double yy, double zz)
Constructor from coordinates.
TPointXYZfRGBu8(float x, float y, float z, uint8_t R_val, uint8_t G_val, uint8_t B_val)
TPolygon3D()
Default constructor.
#define THROW_EXCEPTION(msg)
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
bool operator!=(const TPoint2D &p1, const TPoint2D &p2)
Exact comparison between 2D points.
void operator=(const TPoint2D &p)
Assign a point to this object.
double norm() const
Point norm.
const float & operator[](size_t i) const
Coordinate access using operator[].
void operator=(const TSegment2D &s)
Assign a segment to this object.
std::string asString() const
Union containing pointer to actual data.
TTwist3D()
Default fast constructor.
TPoint3D()
Default fast constructor.
TPoint3D operator+(const TPoint3D &p) const
Points addition.
Standard type for storing any lightweight 2D type.
TLine3D()
Fast default constructor.
bool getLine(TLine2D &r) const
Gets the content as a line, returning false if the type is inadequate.
TObject2D()
Implicit constructor from polygon.
void getAsPose3D(mrpt::poses::CPose3D &outPose)
Gets a pose whose XY plane corresponds to this plane.
bool getPlane(TPlane &p) const
Gets the content as a plane, returning false if the type is not adequate.
TPoint3D pBase
Base point.
Standard object for storing any 3D lightweight object.
STL namespace.
bool getSegment(TSegment2D &s) const
Gets the content as a segment, returning false if the type is inadequate.
TSegment3D()
Fast default constructor.
TObject3D()
Empty constructor.
double z
X,Y,Z coordinates.
TPoint3D & operator+=(const TPoint3D &p)
Translation.
struct BASE_IMPEXP TObject3D
double yaw
Yaw coordinate (rotation angle over Z axis).
void operator=(const TSegment3D &s)
Assigns a segment to this object.
TTwist2D(double vx_, double vy_, double omega_)
Constructor from components.
GLdouble s
Definition: glext.h:3602
double vz
Velocity components: X,Y (m/s)
void operator=(const TPoint3D &p)
Assigns a point to this object.
TPose2D(double xx, double yy, double pphi)
Constructor from coordinates.
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
TSegment3D(const TSegment2D &s)
Constructor from 2D object.
double & operator[](size_t i)
Coordinate access using operator[].
struct BASE_IMPEXP TSegment3D
double norm() const
Pose&#39;s spatial coordinates norm.
void getUnitaryDirectorVector(double(&vector)[2])
Unitarize line and then get director vector.
double & operator[](size_t i)
Coordinate access using operator[].
3D twist: 3D velocity vector (vx,vy,vz) + angular velocity (wx,wy,wz)
TPoint2D(const mrpt::poses::CPoseOrPoint< DERIVEDCLASS > &p)
Constructor from CPoseOrPoint, perhaps losing 3D information.
TObject3D(const TPlane &p)
Constructor from plane.
TPoint3D point1
Origin point.
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
void getUnitaryDirectorVector(double(&vector)[3])
Unitarize and then get director vector.
unsigned char uint8_t
Definition: rptypes.h:43
void generate2DObject(TLine2D &l) const
Project into 2D space, discarding the Z coordinate.
TPose3D()
Default fast constructor.
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
TPointXYZRGBu8(double x, double y, double z, uint8_t R_val, uint8_t G_val, uint8_t B_val)
std::vector< T1 > operator+(const std::vector< T1 > &a, const std::vector< T2 > &b)
a+b (element-wise sum)
Definition: ops_vectors.h:89
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
std::string asString() const
A numeric matrix of compile-time fixed size.
bool getPolygon(TPolygon3D &p) const
Gets the content as a polygon, returning false if the type is not adequate.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
2D segment, consisting of two points.
bool isPoint() const
Checks whether content is a point.
TPlane(const double(&vec)[4])
Constructor from an array of coefficients.
3D segment, consisting of two points.
TObject2D(const TLine2D &r)
Implicit constructor from line.
Lightweight 3D point (float version).
TObject3D(const TPoint3D &p)
Constructor from point.
TPoint2D(double xx, double yy)
Constructor from coordinates.
const double & operator[](size_t i) const
Coordinate access using operator[].
TPoint3D point2
Destiny point.
float & operator[](size_t i)
Coordinate access using operator[].
std::string asString() const
std::ostream BASE_IMPEXP & operator<<(std::ostream &o, const TPoint2D &p)
double distanceTo(const TPoint3D &p) const
Point-to-point distance.
TObject3D & operator=(const TObject3D &obj)
Assigns another object, creating a new pointer if needed.
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
Definition: ops_vectors.h:70
TLine2D(double A, double B, double C)
Constructor from line&#39;s coefficients.
double z
Translation in x,y,z.
const double & operator[](size_t i) const
Coordinate access using operator[].
const unsigned char GEOMETRIC_TYPE_SEGMENT
Object type identifier for TSegment2D or TSegment3D.
const TPoint3D & operator[](size_t i) const
Access to points using operator[0-1].
TPoint3D operator/(double d) const
XYZ point (float) + RGB(u8)
T wrapTo2Pi(T a)
Modifies the given angle to translate it into the [0,2pi[ range.
Definition: wrap2pi.h:40
void generate2DObject(TPolygon2D &p) const
Projects into a 2D space, discarding the z.
GLubyte g
Definition: glext.h:5575
3D Plane, represented by its equation
double vy
Velocity components: X,Y (m/s)
void getDirectorVector(double(&vector)[3]) const
Get director vector.
GLubyte GLubyte b
Definition: glext.h:5575
class BASE_IMPEXP TPolygon3D
TPoint2D operator*(double d) const
TPolygon2D()
Default constructor.
void operator=(const TPolygon3D &p)
Assigns a polygon to this object.
void getCenter(TPoint3D &p) const
Segment&#39;s central point.
TPoint3D operator*(double d) const
unsigned char type
Object type identifier.
TPose3DQuat()
Default fast constructor.
TPoint2D point2
Destiny point.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
bool isSegment() const
Checks whether content is a segment.
XYZ point (float) + Intensity(u8)
TObject2D(const TObject2D &obj)
Constructor from another TObject2D.
The base template class for 2D & 3D points and poses.
Definition: CPoseOrPoint.h:107
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
TPoint2D & operator[](size_t i)
Access to points using operator[0-1].
void getAsVector(std::vector< double > &v) const
Gets the pose as a vector of doubles.
TObject2D & operator=(const TObject2D &obj)
Assign another TObject2D.
double qz
Unit quaternion part, qr,qx,qy,qz.
bool isPlane() const
Checks whether content is a plane.
GLsizei const GLchar ** string
Definition: glext.h:3919
A class used to store a 2D point.
Definition: CPoint2D.h:36
A class used to store a 3D point.
Definition: CPoint3D.h:32
void operator=(const TPlane &p)
Assigns a plane to this object.
TObject2D(const TSegment2D &s)
Implicit constructor from segment.
TPolygon2D(size_t N)
Constructor for a given number of vertices, intializing them as garbage.
double & operator[](size_t i)
Coordinate access using operator[].
bool getPoint(TPoint3D &p) const
Gets the content as a point, returning false if the type is not adequate.
double pitch
Pitch coordinate (rotation angle over Y axis).
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
void getAsPose3DForcingOrigin(const TPoint3D &newOrigin, mrpt::poses::CPose3D &pose) const
Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates...
TPoint2D point1
Origin point.
double norm() const
Point norm.
TPoint2D()
Default fast constructor.
double y
X,Y coordinates.
TPoint3D & operator*=(const double f)
Point scale.
BASE_IMPEXP ::mrpt::utils::CStream & operator>>(mrpt::utils::CStream &in, CMatrixPtr &pObj)
TPoint2D operator+(const TPoint2D &p) const
TObject3D(const TLine3D &r)
Constructor from line.
const GLdouble * v
Definition: glext.h:3603
bool isLine() const
Checks whether content is a line.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const double & operator[](size_t i) const
Coordinate access using operator[].
TPlane()
Fast default constructor.
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[0.02 1.04 -0.8]" )
std::string asString() const
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
TPose3DQuat(double _x, double _y, double _z, double _qr, double _qx, double _qy, double _qz)
Constructor from coordinates.
void operator=(const TPolygon2D &p)
Assign a polygon to this object.
TPoint3D operator-(const TPoint3D &p) const
Points substraction.
XYZ point (double) + RGB(u8)
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
const float R
TPoint2D operator-(const TPoint2D &p) const
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
struct BASE_IMPEXP TLine3D
double sqrDistanceTo(const TPoint3D &p) const
Point-to-point distance, squared.
void getUnitaryNormalVector(double(&vec)[3])
Unitarize, then get normal vector.
double norm() const
Pose&#39;s spatial coordinates norm.
TSegment2D()
Fast default constructor.
unsigned char type
Object type identifier.
~TObject2D()
Object destruction.
GLuint GLsizei GLsizei * length
Definition: glext.h:3900
const TPoint2D & operator[](size_t i) const
Access to points using operator[0-1].
TPoint3D & operator[](size_t i)
Access to points using operator[0-1].
T hypot_fast(const T x, const T y)
Faster version of std::hypot(), to use when overflow is not an issue and we prefer fast code...
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
GLuint in
Definition: glext.h:6301
Lightweight 2D pose.
bool getLine(TLine3D &r) const
Gets the content as a line, returning false if the type is not adequate.
void getAsVector(std::vector< double > &v) const
Transformation into vector.
const double & operator[](size_t i) const
Coordinate access using operator[].
std::string asString() const
void generate2DObject(TSegment2D &s) const
Projection into 2D space, discarding the z.
void generate2DObject(TObject2D &obj) const
Projects into 2D space.
const double & operator[](size_t i) const
Coordinate access using operator[].
bool isLine() const
Checks whether content is a line.
bool isSegment() const
Checks whether content is a segment.
GLenum GLint GLint y
Definition: glext.h:3516
TObject2D(const TPolygon2D &p)
Implicit constructor from polygon.
TTwist3D(double vx_, double vy_, double vz_, double wx_, double wy_, double wz_)
Constructor from components.
bool operator<(const CPoint< DERIVEDCLASS > &a, const CPoint< DERIVEDCLASS > &b)
Used by STL algorithms.
Definition: CPoint.h:116
TSegment3D(const TPoint3D &p1, const TPoint3D &p2)
Constructor from both points.
void getAsPose3D(mrpt::poses::CPose3D &outPose) const
Gets a pose whose XY plane corresponds to this plane.
TObject3D(const TObject3D &obj)
Constructs from another object.
TPose2D()
Default fast constructor.
unsigned char getType() const
Gets object type.
const unsigned char GEOMETRIC_TYPE_UNDEFINED
Object type identifier for empty TObject2D or TObject3D.
TPoint2D & operator-=(const TPoint2D &p)
void operator=(const TLine3D &l)
Assigns a line to this object.
double & operator[](size_t i)
Coordinate access using operator[].
GLenum GLint x
Definition: glext.h:3516
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:43
void inverseComposePoint(const TPoint2D g, TPoint2D &l) const
void getAsVector(std::vector< double > &v) const
Transformation into vector.
Lightweight 3D point.
TPolygon3D(size_t N)
Constructor for a given size.
void asString(std::string &s) const
Returns a human-readable textual representation of the object as "[x y z qr qx qy qz]"...
void operator=(const TLine2D &l)
Assign a line to this object.
bool isPolygon() const
Checks whether content is a polygon.
void destroy()
Destroys the object, releasing the pointer to the content (if any).
TObject3D(const TSegment3D &s)
Constructor from segment.
std::string asString() const
TTwist2D()
Default fast constructor.
Lightweight 2D point.
const unsigned char GEOMETRIC_TYPE_POLYGON
Object type identifier for TPolygon2D or TPolygon3D.
void getAsPose3DForcingOrigin(const TPoint3D &newOrigin, mrpt::poses::CPose3D &pose)
Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates...
TPose3D(double _x, double _y, double _z, double _yaw, double _pitch, double _roll)
Constructor from coordinates.
XYZ point (double) + Intensity(u8)
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3520
std::string asString() const
TPoint3D operator-(const TPoint3D &p1)
Unary minus operator for 3D points.
void getAsVector(std::vector< double > &v) const
Gets the pose as a vector of doubles.
void getAsVector(std::vector< double > &v) const
Transformation into vector.
GLfloat GLfloat p
Definition: glext.h:5587
TPlane(double A, double B, double C, double D)
Constructor from plane coefficients.
double z
X,Y,Z, coords.
TObject3D(const TPolygon3D &p)
Constructor from polygon.
double phi
Orientation (rads)
GLuint GLuint GLsizei GLenum type
Definition: glext.h:3512
double & operator[](size_t i)
Coordinate access using operator[].
void getAsVector(VECTORLIKE &v) const
Transformation into vector.
2D polygon, inheriting from std::vector<TPoint2D>.
TPoint3D & operator-=(const TPoint3D &p)
Difference between points.
3D polygon, inheriting from std::vector<TPoint3D>
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1511
Union type storing pointers to every allowed type.
double omega
Angular velocity (rad/s)
void getAsVector(std::vector< double > &v) const
Transformation into vector.
double & operator[](size_t i)
Coordinate access using operator[].
std::vector< T1 > operator*(const std::vector< T1 > &a, const std::vector< T2 > &b)
a*b (element-wise multiplication)
Definition: ops_vectors.h:59
TPoint3D(const TPoint3Df &p)
Explicit constructor from coordinates.
TPoint2D(const mrpt::utils::TPixelCoordf &p)
Implicit transformation constructor from TPixelCoordf.
bool isPoint() const
Checks whether content is a point.
const double & operator[](size_t i) const
Coordinate access using operator[].
TPoint2D & operator/=(double d)
3D line, represented by a base point and a director vector.
TPolygon2D(const std::vector< TPoint2D > &v)
Implicit constructor from a vector of 2D points.
2D line without bounds, represented by its equation .



Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019