Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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 <vector>
19 #include <stdexcept>
20 
21 namespace mrpt {
22 namespace math {
23  using mrpt::utils::square; //!< Allow square() to be also available under mrpt::math, which makes sense
24 
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  */
170  TPose2D(const mrpt::poses::CPose2D &p);
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  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -45.0]" )
197  * \sa asString
198  * \exception std::exception On invalid format
199  */
200  void fromString(const std::string &s);
201  static size_t size() { return 3; }
202  };
203 
204  /** Lightweight 3D point (float version).
205  * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3D
206  */
208  {
209  enum { static_size = 3 };
210  float x;
211  float y;
212  float z;
213 
214  inline TPoint3Df() { }
215  inline TPoint3Df(const float xx,const float yy,const float zz) : x(xx), y(yy),z(zz) { }
216  inline TPoint3Df & operator +=(const TPoint3Df &p) { x+=p.x; y+=p.y; z+=p.z; return *this; }
217  inline TPoint3Df operator *(const float s) { return TPoint3Df(x*s,y*s,z*s); }
218  /** Coordinate access using operator[]. Order: x,y,z */
219  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"); } }
220 
221  /** Coordinate access using operator[]. Order: x,y,z */
222  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"); } }
223  };
224 
225  /**
226  * Lightweight 3D point. Allows coordinate access using [] operator.
227  * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3Df
228  */
230  enum { static_size = 3 };
231  double x,y,z; //!< X,Y,Z coordinates
232 
233  /** Constructor from coordinates. */
234  inline TPoint3D(double xx,double yy,double zz):x(xx),y(yy),z(zz) {}
235  /** Default fast constructor. Initializes to garbage. */
236  inline TPoint3D() {}
237  /** Explicit constructor from coordinates. */
238  explicit inline TPoint3D(const TPoint3Df &p):x(p.x),y(p.y),z(p.z) {}
239 
240  /** Implicit constructor from TPoint2D. Zeroes the z.
241  * \sa TPoint2D
242  */
243  TPoint3D(const TPoint2D &p);
244  /**
245  * Constructor from TPose2D, losing information. Zeroes the z.
246  * \sa TPose2D
247  */
248  explicit TPoint3D(const TPose2D &p);
249  /**
250  * Constructor from TPose3D, losing information.
251  * \sa TPose3D
252  */
253  explicit TPoint3D(const TPose3D &p);
254  /**
255  * Implicit constructor from heavyweight type.
256  * \sa mrpt::poses::CPoint3D
257  */
259  /**
260  * Constructor from heavyweight 3D pose.
261  * \sa mrpt::poses::CPose3D.
262  */
263  explicit TPoint3D(const mrpt::poses::CPose3D &p);
264  /** Coordinate access using operator[]. Order: x,y,z */
265  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"); } }
266  /** Coordinate access using operator[]. Order: x,y,z */
267  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"); } }
268  /**
269  * Point-to-point distance.
270  */
271  inline double distanceTo(const TPoint3D &p) const {
272  return sqrt(square(p.x-x)+square(p.y-y)+square(p.z-z));
273  }
274  /**
275  * Point-to-point distance, squared.
276  */
277  inline double sqrDistanceTo(const TPoint3D &p) const {
278  return square(p.x-x)+square(p.y-y)+square(p.z-z);
279  }
280  /**
281  * Point norm.
282  */
283  inline double norm() const {
284  return sqrt(square(x)+square(y)+square(z));
285  }
286  /**
287  * Point scale.
288  */
289  inline TPoint3D &operator*=(const double f) {
290  x*=f;y*=f;z*=f;
291  return *this;
292  }
293  /**
294  * Transformation into vector.
295  */
296  template <class VECTORLIKE>
297  void getAsVector(VECTORLIKE &v) const {
298  v.resize(3);
299  v[0]=x; v[1]=y; v[2]=z;
300  }
301  /**
302  * Translation.
303  */
304  inline TPoint3D &operator+=(const TPoint3D &p) {
305  x+=p.x;
306  y+=p.y;
307  z+=p.z;
308  return *this;
309  }
310  /**
311  * Difference between points.
312  */
313  inline TPoint3D &operator-=(const TPoint3D &p) {
314  x-=p.x;
315  y-=p.y;
316  z-=p.z;
317  return *this;
318  }
319  /**
320  * Points addition.
321  */
322  inline TPoint3D operator+(const TPoint3D &p) const {
323  return TPoint3D(x+p.x,y+p.y,z+p.z);
324  }
325  /**
326  * Points substraction.
327  */
328  inline TPoint3D operator-(const TPoint3D &p) const {
329  return TPoint3D(x-p.x,y-p.y,z-p.z);
330  }
331 
332  inline TPoint3D operator*(double d) const {
333  return TPoint3D(x*d,y*d,z*d);
334  }
335 
336  inline TPoint3D operator/(double d) const {
337  return TPoint3D(x/d,y/d,z/d);
338  }
339 
340  bool operator<(const TPoint3D &p) const;
341 
342  /** Returns a human-readable textual representation of the object (eg: "[0.02 1.04 -0.8]" )
343  * \sa fromString
344  */
345  void asString(std::string &s) const { s = mrpt::format("[%f %f %f]",x,y,z); }
346  inline std::string asString() const { std::string s; asString(s); return s; }
347 
348  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" )
349  * \sa asString
350  * \exception std::exception On invalid format
351  */
352  void fromString(const std::string &s);
353  static size_t size() { return 3; }
354  };
355 
356  /** XYZ point (double) + Intensity(u8) \sa mrpt::math::TPoint3D */
359  uint8_t intensity;
360  inline TPointXYZIu8() : pt(), intensity(0) {}
361  inline TPointXYZIu8(double x,double y,double z, uint8_t intensity_val) : pt(x,y,z),intensity(intensity_val) {}
362  };
363  /** XYZ point (double) + RGB(u8) \sa mrpt::math::TPoint3D */
366  uint8_t R,G,B;
367  inline TPointXYZRGBu8() : pt(), R(0),G(0),B(0) {}
368  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) {}
369  };
370  /** XYZ point (float) + Intensity(u8) \sa mrpt::math::TPoint3D */
373  uint8_t intensity;
374  inline TPointXYZfIu8() : pt(), intensity(0) {}
375  inline TPointXYZfIu8(float x,float y,float z, uint8_t intensity_val) : pt(x,y,z),intensity(intensity_val) {}
376  };
377  /** XYZ point (float) + RGB(u8) \sa mrpt::math::TPoint3D */
380  uint8_t R,G,B;
381  inline TPointXYZfRGBu8() : pt(), R(0),G(0),B(0) {}
382  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) {}
383  };
384 
385  /**
386  * Lightweight 3D pose (three spatial coordinates, plus three angular coordinates). Allows coordinate access using [] operator.
387  * \sa mrpt::poses::CPose3D
388  */
390  enum { static_size = 6 };
391  double x,y,z; //!< X,Y,Z, coords
392  double yaw; //!< Yaw coordinate (rotation angle over Z axis).
393  double pitch; //!< Pitch coordinate (rotation angle over Y axis).
394  double roll; //!< Roll coordinate (rotation angle over X coordinate).
395  /** Implicit constructor from TPoint2D. Zeroes all the unprovided information.
396  * \sa TPoint2D
397  */
398  TPose3D(const TPoint2D &p);
399  /**
400  * Implicit constructor from TPose2D. Gets the yaw from the 2D pose's phi, zeroing all the unprovided information.
401  * \sa TPose2D
402  */
403  TPose3D(const TPose2D &p);
404  /**
405  * Implicit constructor from TPoint3D. Zeroes angular information.
406  * \sa TPoint3D
407  */
408  TPose3D(const TPoint3D &p);
409  /**
410  * Implicit constructor from heavyweight type.
411  * \sa mrpt::poses::CPose3D
412  */
413  TPose3D(const mrpt::poses::CPose3D &p);
414  /**
415  * Constructor from coordinates.
416  */
417  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) {}
418  /**
419  * Default fast constructor. Initializes to garbage.
420  */
421  inline TPose3D() {}
422  /** Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll */
423  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"); } }
424  /** Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll */
425  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"); } }
426  /**
427  * Pose's spatial coordinates norm.
428  */
429  double norm() const {
430  return sqrt(square(x)+square(y)+square(z));
431  }
432  /**
433  * Gets the pose as a vector of doubles.
434  */
435  void getAsVector(std::vector<double> &v) const {
436  v.resize(6);
437  v[0]=x; v[1]=y; v[2]=z; v[3]=yaw; v[4]=pitch; v[5]=roll;
438  }
439  /** Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]", angles in degrees.)
440  * \sa fromString
441  */
442  void asString(std::string &s) const;
443  inline std::string asString() const { std::string s; asString(s); return s; }
444 
445  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" )
446  * \sa asString
447  * \exception std::exception On invalid format
448  */
449  void fromString(const std::string &s);
450  static size_t size() { return 6; }
451  };
452 
453  /** Lightweight 3D pose (three spatial coordinates, plus a quaternion ). Allows coordinate access using [] operator.
454  * \sa mrpt::poses::CPose3DQuat
455  */
457  enum { static_size = 7 };
458  double x,y,z; //!< Translation in x,y,z
459  double qr,qx,qy,qz; //!< Unit quaternion part, qr,qx,qy,qz
460 
461  /** Constructor from coordinates. */
462  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) { }
463  /** Default fast constructor. Initializes to garbage. */
464  inline TPose3DQuat() {}
465  /** Constructor from a CPose3DQuat */
467 
468  /** Coordinate access using operator[]. Order: x,y,z,qr,qx,qy,qz */
469  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"); } }
470  /** Coordinate access using operator[]. Order: x,y,z,qr,qx,qy,qz */
471  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"); } }
472  /** Pose's spatial coordinates norm. */
473  double norm() const {
474  return sqrt(square(x)+square(y)+square(z));
475  }
476  /** Gets the pose as a vector of doubles. */
477  void getAsVector(std::vector<double> &v) const {
478  v.resize(7);
479  for (size_t i=0;i<7;i++) v[i]=(*this)[i];
480  }
481  /** Returns a human-readable textual representation of the object as "[x y z qr qx qy qz]"
482  * \sa fromString
483  */
484  void asString(std::string &s) const { s = mrpt::format("[%f %f %f %f %f %f %f]",x,y,z,qr,qx,qy,qz); }
485  inline std::string asString() const { std::string s; asString(s); return s; }
486 
487  /** 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]" )
488  * \sa asString
489  * \exception std::exception On invalid format
490  */
491  void fromString(const std::string &s);
492  static size_t size() { return 7; }
493  };
494 
495  // Text streaming functions:
496  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPoint2D & p);
497  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPoint3D & p);
498  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose2D & p);
499  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose3D & p);
500  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose3DQuat & p);
501 
502 
503  /**
504  * Unary minus operator for 3D points.
505  */
506  inline TPoint3D operator-(const TPoint3D &p1) {
507  return TPoint3D(-p1.x,-p1.y,-p1.z);
508  }
509  /**
510  * Exact comparison between 2D points.
511  */
512  inline bool operator==(const TPoint2D &p1,const TPoint2D &p2) {
513  return (p1.x==p2.x)&&(p1.y==p2.y); //-V550
514  }
515  /**
516  * Exact comparison between 2D points.
517  */
518  inline bool operator!=(const TPoint2D &p1,const TPoint2D &p2) {
519  return (p1.x!=p2.x)||(p1.y!=p2.y); //-V550
520  }
521  /**
522  * Exact comparison between 3D points.
523  */
524  inline bool operator==(const TPoint3D &p1,const TPoint3D &p2) {
525  return (p1.x==p2.x)&&(p1.y==p2.y)&&(p1.z==p2.z); //-V550
526  }
527  /**
528  * Exact comparison between 3D points.
529  */
530  inline bool operator!=(const TPoint3D &p1,const TPoint3D &p2) {
531  return (p1.x!=p2.x)||(p1.y!=p2.y)||(p1.z!=p2.z); //-V550
532  }
533  /**
534  * Exact comparison between 2D poses, taking possible cycles into account.
535  */
536  inline bool operator==(const TPose2D &p1,const TPose2D &p2) {
537  return (p1.x==p2.x)&&(p1.y==p2.y)&&(mrpt::math::wrapTo2Pi(p1.phi)==mrpt::math::wrapTo2Pi(p2.phi)); //-V550
538  }
539  /**
540  * Exact comparison between 2D poses, taking possible cycles into account.
541  */
542  inline bool operator!=(const TPose2D &p1,const TPose2D &p2) {
543  return (p1.x!=p2.x)||(p1.y!=p2.y)||(mrpt::math::wrapTo2Pi(p1.phi)!=mrpt::math::wrapTo2Pi(p2.phi)); //-V550
544  }
545  /**
546  * Exact comparison between 3D poses, taking possible cycles into account.
547  */
548  inline bool operator==(const TPose3D &p1,const TPose3D &p2) {
549  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
550  }
551  /**
552  * Exact comparison between 3D poses, taking possible cycles into account.
553  */
554  inline bool operator!=(const TPose3D &p1,const TPose3D &p2) {
555  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
556  }
557  //Forward declarations
562 
563  //Pragma defined to ensure no structure packing
564  /**
565  * 2D segment, consisting of two points.
566  * \sa TSegment3D,TLine2D,TPolygon2D,TPoint2D
567  */
569  public:
570  /**
571  * Origin point.
572  */
574  /**
575  * Destiny point.
576  */
578  /**
579  * Segment length.
580  */
581  double length() const;
582  /**
583  * Distance to point.
584  */
585  double distance(const TPoint2D &point) const;
586  /**
587  * Distance with sign to point (sign indicates which side the point is).
588  */
589  double signedDistance(const TPoint2D &point) const;
590  /**
591  * Check whether a point is inside a segment.
592  */
593  bool contains(const TPoint2D &point) const;
594  /** Access to points using operator[0-1] */
595  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"); } }
596  /** Access to points using operator[0-1] */
597  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"); } }
598  /**
599  * Project into 3D space, setting the z to 0.
600  */
601  void generate3DObject(TSegment3D &s) const;
602  /**
603  * Segment's central point.
604  */
605  inline void getCenter(TPoint2D &p) const {
606  p.x=(point1.x+point2.x)/2;
607  p.y=(point1.y+point2.y)/2;
608  }
609  /**
610  * Constructor from both points.
611  */
612  TSegment2D(const TPoint2D &p1,const TPoint2D &p2):point1(p1),point2(p2) {}
613  /**
614  * Fast default constructor. Initializes to garbage.
615  */
617  /**
618  * Explicit constructor from 3D object, discarding the z.
619  */
620  explicit TSegment2D(const TSegment3D &s);
621 
622  bool operator<(const TSegment2D &s) const;
623  };
624  /**
625  * 3D segment, consisting of two points.
626  * \sa TSegment2D,TLine3D,TPlane,TPolygon3D,TPoint3D
627  */
629  public:
630  /**
631  * Origin point.
632  */
634  /**
635  * Destiny point.
636  */
638  /**
639  * Segment length.
640  */
641  double length() const;
642  /**
643  * Distance to point.
644  */
645  double distance(const TPoint3D &point) const;
646  /**
647  * Distance to another segment.
648  */
649  double distance(const TSegment3D &segment) const;
650  /**
651  * Check whether a point is inside the segment.
652  */
653  bool contains(const TPoint3D &point) const;
654  /** Access to points using operator[0-1] */
655  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"); } }
656  /** Access to points using operator[0-1] */
657  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"); } }
658  /**
659  * Projection into 2D space, discarding the z.
660  */
661  inline void generate2DObject(TSegment2D &s) const {
662  s=TSegment2D(*this);
663  }
664  /**
665  * Segment's central point.
666  */
667  inline void getCenter(TPoint3D &p) const {
668  p.x=(point1.x+point2.x)/2;
669  p.y=(point1.y+point2.y)/2;
670  p.z=(point1.z+point2.z)/2;
671  }
672  /**
673  * Constructor from both points.
674  */
675  TSegment3D(const TPoint3D &p1,const TPoint3D &p2):point1(p1),point2(p2) {}
676  /**
677  * Fast default constructor. Initializes to garbage.
678  */
680  /**
681  * Constructor from 2D object. Sets the z to zero.
682  */
683  TSegment3D(const TSegment2D &s):point1(s.point1),point2(s.point2) {}
684 
685  bool operator<(const TSegment3D &s) const;
686  };
687 
688  inline bool operator==(const TSegment2D &s1,const TSegment2D &s2) {
689  return (s1.point1==s2.point1)&&(s1.point2==s2.point2);
690  }
691 
692  inline bool operator!=(const TSegment2D &s1,const TSegment2D &s2) {
693  return (s1.point1!=s2.point1)||(s1.point2!=s2.point2);
694  }
695 
696  inline bool operator==(const TSegment3D &s1,const TSegment3D &s2) {
697  return (s1.point1==s2.point1)&&(s1.point2==s2.point2);
698  }
699 
700  inline bool operator!=(const TSegment3D &s1,const TSegment3D &s2) {
701  return (s1.point1!=s2.point1)||(s1.point2!=s2.point2);
702  }
703 
704  /**
705  * 2D line without bounds, represented by its equation \f$Ax+By+C=0\f$.
706  * \sa TLine3D,TSegment2D,TPolygon2D,TPoint2D
707  */
709  public:
710  /**
711  * Line coefficients, stored as an array: \f$\left[A,B,C\right]\f$.
712  */
713  double coefs[3];
714  /**
715  * Evaluate point in the line's equation.
716  */
717  double evaluatePoint(const TPoint2D &point) const;
718  /**
719  * Check whether a point is inside the line.
720  */
721  bool contains(const TPoint2D &point) const;
722  /**
723  * Distance from a given point.
724  */
725  double distance(const TPoint2D &point) const;
726  /**
727  * Distance with sign from a given point (sign indicates side).
728  */
729  double signedDistance(const TPoint2D &point) const;
730  /**
731  * Get line's normal vector.
732  */
733  void getNormalVector(double (&vector)[2]) const;
734  /**
735  * Unitarize line's normal vector.
736  */
737  void unitarize();
738  /**
739  * Get line's normal vector after unitarizing line.
740  */
741  inline void getUnitaryNormalVector(double (&vector)[2]) {
742  unitarize();
743  getNormalVector(vector);
744  }
745  /**
746  * Get line's director vector.
747  */
748  void getDirectorVector(double (&vector)[2]) const;
749  /**
750  * Unitarize line and then get director vector.
751  */
752  inline void getUnitaryDirectorVector(double (&vector)[2]) {
753  unitarize();
754  getDirectorVector(vector);
755  }
756  /**
757  * Project into 3D space, setting the z to 0.
758  */
759  void generate3DObject(TLine3D &l) const;
760  /**
761  * Get a pose2D whose X axis corresponds to the line.
762  * \sa mrpt::poses::CPose2D.
763  */
764  void getAsPose2D(mrpt::poses::CPose2D &outPose) const;
765  /**
766  * Get a pose2D whose X axis corresponds to the line, forcing the base point to one given.
767  * \throw logic_error if the point is not inside the line.
768  * \sa mrpt::poses::CPose2D.
769  */
770  void getAsPose2DForcingOrigin(const TPoint2D &origin,mrpt::poses::CPose2D &outPose) const;
771  /**
772  * Constructor from two points, through which the line will pass.
773  * \throw logic_error if both points are the same
774  */
775  TLine2D(const TPoint2D &p1,const TPoint2D &p2) throw(std::logic_error);
776  /**
777  * Constructor from a segment.
778  */
779  explicit TLine2D(const TSegment2D &s);
780  /**
781  * Fast default constructor. Initializes to garbage.
782  */
783  TLine2D() {}
784  /**
785  * Constructor from line's coefficients.
786  */
787  inline TLine2D(double A,double B,double C) {
788  coefs[0]=A;
789  coefs[1]=B;
790  coefs[2]=C;
791  }
792  /**
793  * Construction from 3D object, discarding the Z.
794  * \throw std::logic_error if the line is normal to the XY plane.
795  */
796  explicit TLine2D(const TLine3D &l);
797  };
798 
799  /**
800  * 3D line, represented by a base point and a director vector.
801  * \sa TLine2D,TSegment3D,TPlane,TPolygon3D,TPoint3D
802  */
804  public:
805  /**
806  * Base point.
807  */
809  /**
810  * Director vector.
811  */
812  double director[3];
813  /**
814  * Check whether a point is inside the line.
815  */
816  bool contains(const TPoint3D &point) const;
817  /**
818  * Distance between the line and a point.
819  */
820  double distance(const TPoint3D &point) const;
821  /**
822  * Unitarize director vector.
823  */
824  void unitarize();
825  /**
826  * Get director vector.
827  */
828  inline void getDirectorVector(double (&vector)[3]) const {
829  for (size_t i=0;i<3;i++) vector[i]=director[i];
830  }
831  /**
832  * Unitarize and then get director vector.
833  */
834  inline void getUnitaryDirectorVector(double (&vector)[3]) {
835  unitarize();
836  getDirectorVector(vector);
837  }
838  /**
839  * Project into 2D space, discarding the Z coordinate.
840  * \throw std::logic_error if the line's director vector is orthogonal to the XY plane.
841  */
842  inline void generate2DObject(TLine2D &l) const {
843  l=TLine2D(*this);
844  }
845  /**
846  * Constructor from two points, through which the line will pass.
847  * \throw std::logic_error if both points are the same.
848  */
849  TLine3D(const TPoint3D &p1,const TPoint3D &p2) throw(std::logic_error);
850  /**
851  * Constructor from 3D segment.
852  */
853  explicit TLine3D(const TSegment3D &s);
854  /**
855  * Fast default constructor. Initializes to garbage.
856  */
857  TLine3D() {}
858  /**
859  * Implicit constructor from 2D object. Zeroes the z.
860  */
861  TLine3D(const TLine2D &l);
862  };
863 
864  /**
865  * 3D Plane, represented by its equation \f$Ax+By+Cz+D=0\f$
866  * \sa TSegment3D,TLine3D,TPolygon3D,TPoint3D
867  */
869  public:
870  /**
871  * Plane coefficients, stored as an array: \f$\left[A,B,C,D\right]\f$
872  */
873  double coefs[4];
874  /**
875  * Evaluate a point in the plane's equation.
876  */
877  double evaluatePoint(const TPoint3D &point) const;
878  /**
879  * Check whether a point is contained into the plane.
880  */
881  bool contains(const TPoint3D &point) const;
882  /**
883  * Check whether a segment is fully contained into the plane.
884  */
885  inline bool contains(const TSegment3D &segment) const {
886  return contains(segment.point1)&&contains(segment.point2);
887  }
888  /**
889  * Check whether a line is fully contained into the plane.
890  */
891  bool contains(const TLine3D &line) const;
892  /**
893  * Distance to 3D point.
894  */
895  double distance(const TPoint3D &point) const;
896  /**
897  * Distance to 3D line. Will be zero if the line is not parallel to the plane.
898  */
899  double distance(const TLine3D &line) const;
900  /**
901  * Get plane's normal vector.
902  */
903  void getNormalVector(double (&vec)[3]) const;
904  /**
905  * Unitarize normal vector.
906  */
907  void unitarize();
908  /**
909  * Unitarize, then get normal vector.
910  */
911  inline void getUnitaryNormalVector(double (&vec)[3]) {
912  unitarize();
913  getNormalVector(vec);
914  }
915  /**
916  * Gets a pose whose XY plane corresponds to this plane.
917  */
918  void getAsPose3D(mrpt::poses::CPose3D &outPose);
919  /**
920  * Gets a pose whose XY plane corresponds to this plane.
921  */
922  inline void getAsPose3D(mrpt::poses::CPose3D &outPose) const {
923  TPlane p=*this;
924  p.getAsPose3D(outPose);
925  }
926  /**
927  * Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates.
928  * \throw std::logic_error if the point is not inside the plane.
929  */
930  void getAsPose3DForcingOrigin(const TPoint3D &newOrigin,mrpt::poses::CPose3D &pose);
931  /**
932  * Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates.
933  * \throw std::logic_error if the point is not inside the plane.
934  */
935  inline void getAsPose3DForcingOrigin(const TPoint3D &newOrigin,mrpt::poses::CPose3D &pose) const {
936  TPlane p=*this;
937  p.getAsPose3DForcingOrigin(newOrigin,pose);
938  }
939  /**
940  * Gets a plane which contains these three points.
941  * \throw std::logic_error if the points are linearly dependants.
942  */
943  TPlane(const TPoint3D &p1,const TPoint3D &p2,const TPoint3D &p3) throw(std::logic_error);
944  /**
945  * Gets a plane which contains this point and this line.
946  * \throw std::logic_error if the point is inside the line.
947  */
948  TPlane(const TPoint3D &p1,const TLine3D &r2) throw(std::logic_error);
949  /**
950  * Gets a plane which contains the two lines.
951  * \throw std::logic_error if the lines do not cross.
952  */
953  TPlane(const TLine3D &r1,const TLine3D &r2) throw(std::logic_error);
954  /**
955  * Fast default constructor. Initializes to garbage.
956  */
957  TPlane() {}
958  /**
959  * Constructor from plane coefficients.
960  */
961  inline TPlane(double A,double B,double C,double D) {
962  coefs[0]=A;
963  coefs[1]=B;
964  coefs[2]=C;
965  coefs[3]=D;
966  }
967  /**
968  * Constructor from an array of coefficients.
969  */
970  inline TPlane(const double (&vec)[4]) {
971  for (size_t i=0;i<4;i++) coefs[i]=vec[i];
972  }
973  };
974 
975  typedef TPlane TPlane3D;
976 
977  /**
978  * 2D polygon, inheriting from std::vector<TPoint2D>.
979  * \sa TPolygon3D,TSegment2D,TLine2D,TPoint2D, CPolygon
980  */
981  class BASE_IMPEXP TPolygon2D:public std::vector<TPoint2D> {
982  public:
983  /**
984  * Distance to a point.
985  */
986  double distance(const TPoint2D &point) const;
987  /**
988  * Check whether a point is inside the polygon.
989  */
990  bool contains(const TPoint2D &point) const;
991  /**
992  * Gets as set of segments, instead of points.
993  */
994  void getAsSegmentList(std::vector<TSegment2D> &v) const;
995  /**
996  * Projects into 3D space, zeroing the z.
997  */
998  void generate3DObject(TPolygon3D &p) const;
999  /**
1000  * Polygon's central point.
1001  */
1002  void getCenter(TPoint2D &p) const;
1003  /**
1004  * Checks whether is convex.
1005  */
1006  bool isConvex() const;
1007  /**
1008  * Erase repeated vertices.
1009  * \sa removeRedundantVertices
1010  */
1011  void removeRepeatedVertices();
1012  /**
1013  * Erase every redundant vertex from the polygon, saving space.
1014  * \sa removeRepeatedVertices
1015  */
1016  void removeRedundantVertices();
1017  /**
1018  * Gets plot data, ready to use on a 2D plot.
1019  * \sa mrpt::gui::CDisplayWindowPlots
1020  */
1021  void getPlotData(std::vector<double> &x,std::vector<double> &y) const;
1022  /**
1023  * Default constructor.
1024  */
1025  TPolygon2D():std::vector<TPoint2D>() {}
1026  /**
1027  * Constructor for a given number of vertices, intializing them as garbage.
1028  */
1029  explicit TPolygon2D(size_t N):std::vector<TPoint2D>(N) {}
1030  /**
1031  * Implicit constructor from a vector of 2D points.
1032  */
1033  TPolygon2D(const std::vector<TPoint2D> &v):std::vector<TPoint2D>(v) {}
1034  /**
1035  * Constructor from a 3D object.
1036  */
1037  explicit TPolygon2D(const TPolygon3D &p);
1038  /**
1039  * Static method to create a regular polygon, given its size and radius.
1040  * \throw std::logic_error if radius is near zero or the number of edges is less than three.
1041  */
1042  static void createRegularPolygon(size_t numEdges,double radius,TPolygon2D &poly);
1043  /**
1044  * Static method to create a regular polygon from its size and radius. The center will correspond to the given pose.
1045  * \throw std::logic_error if radius is near zero or the number of edges is less than three.
1046  */
1047  static inline void createRegularPolygon(size_t numEdges,double radius,TPolygon2D &poly,const mrpt::poses::CPose2D &pose);
1048  };
1049 
1050  /**
1051  * 3D polygon, inheriting from std::vector<TPoint3D>
1052  * \sa TPolygon2D,TSegment3D,TLine3D,TPlane,TPoint3D
1053  */
1054  class BASE_IMPEXP TPolygon3D:public std::vector<TPoint3D> {
1055  public:
1056  /**
1057  * Distance to point.
1058  */
1059  double distance(const TPoint3D &point) const;
1060  /**
1061  * Check whether a point is inside the polygon.
1062  */
1063  bool contains(const TPoint3D &point) const;
1064  /**
1065  * Gets as set of segments, instead of set of points.
1066  */
1067  void getAsSegmentList(std::vector<TSegment3D> &v) const;
1068  /**
1069  * Gets a plane which contains the polygon. Returns false if the polygon is skew and cannot be fit inside a plane.
1070  */
1071  bool getPlane(TPlane &p) const;
1072  /**
1073  * Gets the best fitting plane, disregarding whether the polygon actually fits inside or not.
1074  * \sa getBestFittingPlane
1075  */
1076  void getBestFittingPlane(TPlane &p) const;
1077  /**
1078  * Projects into a 2D space, discarding the z.
1079  * \get getPlane,isSkew
1080  */
1081  inline void generate2DObject(TPolygon2D &p) const {
1082  p=TPolygon2D(*this);
1083  }
1084  /**
1085  * Get polygon's central point.
1086  */
1087  void getCenter(TPoint3D &p) const;
1088  /**
1089  * Check whether the polygon is skew. Returns true if there doesn't exist a plane in which the polygon can fit.
1090  * \sa getBestFittingPlane
1091  */
1092  bool isSkew() const;
1093  /**
1094  * Remove polygon's repeated vertices.
1095  */
1096  void removeRepeatedVertices();
1097  /**
1098  * Erase every redundant vertex, thus saving space.
1099  */
1100  void removeRedundantVertices();
1101  /**
1102  * Default constructor. Creates a polygon with no vertices.
1103  */
1104  TPolygon3D():std::vector<TPoint3D>() {}
1105  /**
1106  * Constructor for a given size. Creates a polygon with a fixed number of vertices, which are initialized to garbage.
1107  */
1108  explicit TPolygon3D(size_t N):std::vector<TPoint3D>(N) {}
1109  /**
1110  * Implicit constructor from a 3D points vector.
1111  */
1112  TPolygon3D(const std::vector<TPoint3D> &v):std::vector<TPoint3D>(v) {}
1113  /**
1114  * Constructor from a 2D object. Zeroes the z.
1115  */
1116  TPolygon3D(const TPolygon2D &p);
1117  /**
1118  * Static method to create a regular polygon, given its size and radius.
1119  * \throw std::logic_error if number of edges is less than three, or radius is near zero.
1120  */
1121  static void createRegularPolygon(size_t numEdges,double radius,TPolygon3D &poly);
1122  /**
1123  * Static method to create a regular polygon, given its size and radius. The center will be located on the given pose.
1124  * \throw std::logic_error if number of edges is less than three, or radius is near zero.
1125  */
1126  static inline void createRegularPolygon(size_t numEdges,double radius,TPolygon3D &poly,const mrpt::poses::CPose3D &pose);
1127  };
1128 
1129  /**
1130  * Object type identifier for TPoint2D or TPoint3D.
1131  * \sa TObject2D,TObject3D
1132  */
1133  const unsigned char GEOMETRIC_TYPE_POINT=0;
1134  /**
1135  * Object type identifier for TSegment2D or TSegment3D.
1136  * \sa TObject2D,TObject3D
1137  */
1138  const unsigned char GEOMETRIC_TYPE_SEGMENT=1;
1139  /**
1140  * Object type identifier for TLine2D or TLine3D.
1141  * \sa TObject2D,TObject3D
1142  */
1143  const unsigned char GEOMETRIC_TYPE_LINE=2;
1144  /**
1145  * Object type identifier for TPolygon2D or TPolygon3D.
1146  * \sa TObject2D,TObject3D
1147  */
1148  const unsigned char GEOMETRIC_TYPE_POLYGON=3;
1149  /**
1150  * Object type identifier for TPlane.
1151  * \sa TObject3D
1152  */
1153  const unsigned char GEOMETRIC_TYPE_PLANE=4;
1154  /**
1155  * Object type identifier for empty TObject2D or TObject3D.
1156  * \sa TObject2D,TObject3D
1157  */
1158  const unsigned char GEOMETRIC_TYPE_UNDEFINED=255;
1159 
1160  /**
1161  * Standard type for storing any lightweight 2D type. Do not inherit from this class.
1162  * \sa TPoint2D,TSegment2D,TLine2D,TPolygon2D
1163  */
1165  private:
1166  /**
1167  * Object type identifier.
1168  */
1169  unsigned char type;
1170  /**
1171  * Union type storing pointers to every allowed type.
1172  */
1178 
1179  tobject2d_data_t() : polygon(NULL) { }
1180  } data;
1181  /**
1182  * Destroys the object, releasing the pointer to the content (if any).
1183  */
1184  inline void destroy() {
1185  if (type==GEOMETRIC_TYPE_POLYGON) delete data.polygon;
1187  }
1188  public:
1189  /**
1190  * Implicit constructor from point.
1191  */
1192  inline TObject2D(const TPoint2D &p):type(GEOMETRIC_TYPE_POINT) {
1193  data.point=p;
1194  }
1195  /**
1196  * Implicit constructor from segment.
1197  */
1198  inline TObject2D(const TSegment2D &s):type(GEOMETRIC_TYPE_SEGMENT) {
1199  data.segment=s;
1200  }
1201  /**
1202  * Implicit constructor from line.
1203  */
1204  inline TObject2D(const TLine2D &r):type(GEOMETRIC_TYPE_LINE) {
1205  data.line=r;
1206  }
1207  /**
1208  * Implicit constructor from polygon.
1209  */
1210  inline TObject2D(const TPolygon2D &p):type(GEOMETRIC_TYPE_POLYGON) {
1211  data.polygon=new TPolygon2D(p);
1212  }
1213  /**
1214  * Implicit constructor from polygon.
1215  */
1216  TObject2D():type(GEOMETRIC_TYPE_UNDEFINED) {}
1217  /**
1218  * Object destruction.
1219  */
1221  destroy();
1222  }
1223  /**
1224  * Checks whether content is a point.
1225  */
1226  inline bool isPoint() const {
1227  return type==GEOMETRIC_TYPE_POINT;
1228  }
1229  /**
1230  * Checks whether content is a segment.
1231  */
1232  inline bool isSegment() const {
1233  return type==GEOMETRIC_TYPE_SEGMENT;
1234  }
1235  /**
1236  * Checks whether content is a line.
1237  */
1238  inline bool isLine() const {
1239  return type==GEOMETRIC_TYPE_LINE;
1240  }
1241  /**
1242  * Checks whether content is a polygon.
1243  */
1244  inline bool isPolygon() const {
1245  return type==GEOMETRIC_TYPE_POLYGON;
1246  }
1247  /**
1248  * Gets content type.
1249  */
1250  inline unsigned char getType() const {
1251  return type;
1252  }
1253  /**
1254  * Gets the content as a point, returning false if the type is inadequate.
1255  */
1256  inline bool getPoint(TPoint2D &p) const {
1257  if (isPoint()) {
1258  p=data.point;
1259  return true;
1260  } else return false;
1261  }
1262  /**
1263  * Gets the content as a segment, returning false if the type is inadequate.
1264  */
1265  inline bool getSegment(TSegment2D &s) const {
1266  if (isSegment()) {
1267  s=data.segment;
1268  return true;
1269  } else return false;
1270  }
1271  /**
1272  * Gets the content as a line, returning false if the type is inadequate.
1273  */
1274  inline bool getLine(TLine2D &r) const {
1275  if (isLine()) {
1276  r=data.line;
1277  return true;
1278  } else return false;
1279  }
1280  /**
1281  * Gets the content as a polygon, returning false if the type is inadequate.
1282  */
1283  inline bool getPolygon(TPolygon2D &p) const {
1284  if (isPolygon()) {
1285  p=*(data.polygon);
1286  return true;
1287  } else return false;
1288  }
1289  /**
1290  * Assign another TObject2D. Pointers are not shared.
1291  */
1293  if (this==&obj) return *this;
1294  destroy();
1295  switch (type=obj.type) {
1296  case GEOMETRIC_TYPE_POINT:
1297  data.point=obj.data.point;
1298  break;
1299  case GEOMETRIC_TYPE_SEGMENT:
1300  data.segment=obj.data.segment;
1301  break;
1302  case GEOMETRIC_TYPE_LINE:
1303  data.line=obj.data.line;
1304  break;
1305  case GEOMETRIC_TYPE_POLYGON:
1306  data.polygon=new TPolygon2D(*(obj.data.polygon));
1307  break;
1308  }
1309  return *this;
1310  }
1311  /**
1312  * Assign a point to this object.
1313  */
1314  inline void operator=(const TPoint2D &p) {
1315  destroy();
1316  type=GEOMETRIC_TYPE_POINT;
1317  data.point=p;
1318  }
1319  /**
1320  * Assign a segment to this object.
1321  */
1322  inline void operator=(const TSegment2D &s) {
1323  destroy();
1325  data.segment=s;
1326  }
1327  /**
1328  * Assign a line to this object.
1329  */
1330  inline void operator=(const TLine2D &l) {
1331  destroy();
1332  type=GEOMETRIC_TYPE_LINE;
1333  data.line=l;
1334  }
1335  /**
1336  * Assign a polygon to this object.
1337  */
1338  inline void operator=(const TPolygon2D &p) {
1339  destroy();
1341  data.polygon=new TPolygon2D(p);
1342  }
1343  /**
1344  * Project into 3D space.
1345  */
1346  void generate3DObject(TObject3D &obj) const;
1347  /**
1348  * Constructor from another TObject2D.
1349  */
1350  TObject2D(const TObject2D &obj):type(GEOMETRIC_TYPE_UNDEFINED) {
1351  operator=(obj);
1352  }
1353  /**
1354  * Static method to retrieve all the points in a vector of TObject2D.
1355  */
1356  static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts);
1357  /**
1358  * Static method to retrieve all the segments in a vector of TObject2D.
1359  */
1360  static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms);
1361  /**
1362  * Static method to retrieve all the lines in a vector of TObject2D.
1363  */
1364  static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins);
1365  /**
1366  * Static method to retrieve all the polygons in a vector of TObject2D.
1367  */
1368  static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys);
1369  /**
1370  * Static method to retrieve all the points in a vector of TObject2D, returning the remainder objects in another parameter.
1371  */
1372  static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts,std::vector<TObject2D> &remainder);
1373  /**
1374  * Static method to retrieve all the segments in a vector of TObject2D, returning the remainder objects in another parameter.
1375  */
1376  static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms,std::vector<TObject2D> &remainder);
1377  /**
1378  * Static method to retrieve all the lines in a vector of TObject2D, returning the remainder objects in another parameter.
1379  */
1380  static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins,std::vector<TObject2D> &remainder);
1381  /**
1382  * Static method to retrieve all the polygons in a vector of TObject2D, returning the remainder objects in another parameter.
1383  */
1384  static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys,std::vector<TObject2D> &remainder);
1385  };
1386  /**
1387  * Standard object for storing any 3D lightweight object. Do not inherit from this class.
1388  * \sa TPoint3D,TSegment3D,TLine3D,TPlane,TPolygon3D
1389  */
1391  private:
1392  /**
1393  * Object type identifier.
1394  */
1395  unsigned char type;
1396  /**
1397  * Union containing pointer to actual data.
1398  */
1405 
1406  tobject3d_data_t() : polygon(NULL) { }
1407  } data;
1408  /**
1409  * Destroys the object and releases the pointer, if any.
1410  */
1411  void destroy() {
1412  if (type==GEOMETRIC_TYPE_POLYGON) delete data.polygon;
1414  }
1415  public:
1416  /**
1417  * Constructor from point.
1418  */
1419  TObject3D(const TPoint3D &p):type(GEOMETRIC_TYPE_POINT) {
1420  data.point=p;
1421  }
1422  /**
1423  * Constructor from segment.
1424  */
1425  TObject3D(const TSegment3D &s):type(GEOMETRIC_TYPE_SEGMENT) {
1426  data.segment=s;
1427  }
1428  /**
1429  * Constructor from line.
1430  */
1431  TObject3D(const TLine3D &r):type(GEOMETRIC_TYPE_LINE) {
1432  data.line=r;
1433  }
1434  /**
1435  * Constructor from polygon.
1436  */
1437  TObject3D(const TPolygon3D &p):type(GEOMETRIC_TYPE_POLYGON) {
1438  data.polygon=new TPolygon3D(p);
1439  }
1440  /**
1441  * Constructor from plane.
1442  */
1443  TObject3D(const TPlane &p):type(GEOMETRIC_TYPE_PLANE) {
1444  data.plane=p;
1445  }
1446  /**
1447  * Empty constructor.
1448  */
1449  TObject3D():type(GEOMETRIC_TYPE_UNDEFINED) {}
1450  /**
1451  * Destructor.
1452  */
1454  destroy();
1455  }
1456  /**
1457  * Checks whether content is a point.
1458  */
1459  inline bool isPoint() const {
1460  return type==GEOMETRIC_TYPE_POINT;
1461  }
1462  /**
1463  * Checks whether content is a segment.
1464  */
1465  inline bool isSegment() const {
1466  return type==GEOMETRIC_TYPE_SEGMENT;
1467  }
1468  /**
1469  * Checks whether content is a line.
1470  */
1471  inline bool isLine() const {
1472  return type==GEOMETRIC_TYPE_LINE;
1473  }
1474  /**
1475  * Checks whether content is a polygon.
1476  */
1477  inline bool isPolygon() const {
1478  return type==GEOMETRIC_TYPE_POLYGON;
1479  }
1480  /**
1481  * Checks whether content is a plane.
1482  */
1483  inline bool isPlane() const {
1484  return type==GEOMETRIC_TYPE_PLANE;
1485  }
1486  /**
1487  * Gets object type.
1488  */
1489  inline unsigned char getType() const {
1490  return type;
1491  }
1492  /**
1493  * Gets the content as a point, returning false if the type is not adequate.
1494  */
1495  inline bool getPoint(TPoint3D &p) const {
1496  if (isPoint()) {
1497  p=data.point;
1498  return true;
1499  } else return false;
1500  }
1501  /**
1502  * Gets the content as a segment, returning false if the type is not adequate.
1503  */
1504  inline bool getSegment(TSegment3D &s) const {
1505  if (isSegment()) {
1506  s=data.segment;
1507  return true;
1508  } else return false;
1509  }
1510  /**
1511  * Gets the content as a line, returning false if the type is not adequate.
1512  */
1513  inline bool getLine(TLine3D &r) const {
1514  if (isLine()) {
1515  r=data.line;
1516  return true;
1517  } else return false;
1518  }
1519  /**
1520  * Gets the content as a polygon, returning false if the type is not adequate.
1521  */
1522  inline bool getPolygon(TPolygon3D &p) const {
1523  if (isPolygon()) {
1524  p=*(data.polygon);
1525  return true;
1526  } else return false;
1527  }
1528  /**
1529  * Gets the content as a plane, returning false if the type is not adequate.
1530  */
1531  inline bool getPlane(TPlane &p) const {
1532  if (isPlane()) {
1533  p=data.plane;
1534  return true;
1535  } else return false;
1536  }
1537  /**
1538  * Assigns another object, creating a new pointer if needed.
1539  */
1541  if (this==&obj) return *this;
1542  destroy();
1543  switch (type=obj.type) {
1544  case GEOMETRIC_TYPE_POINT:
1545  data.point=obj.data.point;
1546  break;
1547  case GEOMETRIC_TYPE_SEGMENT:
1548  data.segment=obj.data.segment;
1549  break;
1550  case GEOMETRIC_TYPE_LINE:
1551  data.line=obj.data.line;
1552  break;
1553  case GEOMETRIC_TYPE_POLYGON:
1554  data.polygon=new TPolygon3D(*(obj.data.polygon));
1555  break;
1556  case GEOMETRIC_TYPE_PLANE:
1557  data.plane=obj.data.plane;
1558  break;
1559  case GEOMETRIC_TYPE_UNDEFINED:
1560  break;
1561  default:
1562  THROW_EXCEPTION("Invalid TObject3D object");
1563  }
1564  return *this;
1565  }
1566  /**
1567  * Assigns a point to this object.
1568  */
1569  inline void operator=(const TPoint3D &p) {
1570  destroy();
1571  type=GEOMETRIC_TYPE_POINT;
1572  data.point=p;
1573  }
1574  /**
1575  * Assigns a segment to this object.
1576  */
1577  inline void operator=(const TSegment3D &s) {
1578  destroy();
1580  data.segment=s;
1581  }
1582  /**
1583  * Assigns a line to this object.
1584  */
1585  inline void operator=(const TLine3D &l) {
1586  destroy();
1587  type=GEOMETRIC_TYPE_LINE;
1588  data.line=l;
1589  }
1590  /**
1591  * Assigns a polygon to this object.
1592  */
1593  inline void operator=(const TPolygon3D &p) {
1594  destroy();
1596  data.polygon=new TPolygon3D(p);
1597  }
1598  /**
1599  * Assigns a plane to this object.
1600  */
1601  inline void operator=(const TPlane &p) {
1602  destroy();
1603  type=GEOMETRIC_TYPE_PLANE;
1604  data.plane=p;
1605  }
1606  /**
1607  * Projects into 2D space.
1608  * \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).
1609  */
1610  inline void generate2DObject(TObject2D &obj) const {
1611  switch (type) {
1612  case GEOMETRIC_TYPE_POINT:
1613  obj=TPoint2D(data.point);
1614  break;
1615  case GEOMETRIC_TYPE_SEGMENT:
1616  obj=TSegment2D(data.segment);
1617  break;
1618  case GEOMETRIC_TYPE_LINE:
1619  obj=TLine2D(data.line);
1620  break;
1621  case GEOMETRIC_TYPE_POLYGON:
1622  obj=TPolygon2D(*(data.polygon));
1623  break;
1624  case GEOMETRIC_TYPE_PLANE:
1625  throw std::logic_error("Too many dimensions");
1626  default:
1627  obj=TObject2D();
1628  break;
1629  }
1630  }
1631  /**
1632  * Constructs from another object.
1633  */
1634  TObject3D(const TObject3D &obj):type(GEOMETRIC_TYPE_UNDEFINED) {
1635  operator=(obj);
1636  }
1637  /**
1638  * Static method to retrieve every point included in a vector of objects.
1639  */
1640  static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts);
1641  /**
1642  * Static method to retrieve every segment included in a vector of objects.
1643  */
1644  static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms);
1645  /**
1646  * Static method to retrieve every line included in a vector of objects.
1647  */
1648  static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins);
1649  /**
1650  * Static method to retrieve every plane included in a vector of objects.
1651  */
1652  static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns);
1653  /**
1654  * Static method to retrieve every polygon included in a vector of objects.
1655  */
1656  static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys);
1657  /**
1658  * Static method to retrieve every point included in a vector of objects, returning the remaining objects in another argument.
1659  */
1660  static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts,std::vector<TObject3D> &remainder);
1661  /**
1662  * Static method to retrieve every segment included in a vector of objects, returning the remaining objects in another argument.
1663  */
1664  static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms,std::vector<TObject3D> &remainder);
1665  /**
1666  * Static method to retrieve every line included in a vector of objects, returning the remaining objects in another argument.
1667  */
1668  static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins,std::vector<TObject3D> &remainder);
1669  /**
1670  * Static method to retrieve every plane included in a vector of objects, returning the remaining objects in another argument.
1671  */
1672  static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns,std::vector<TObject3D> &remainder);
1673  /**
1674  * Static method to retrieve every polygon included in a vector of objects, returning the remaining objects in another argument.
1675  */
1676  static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder);
1677  };
1678 
1679 
1680 
1681  //Streaming functions
1682  /** TPoint2D binary input. */
1684  /** TPoint2D binary output. */
1686  /** TPoint3D binary input. */
1688  /** TPoint3D binary output. */
1690  /** TPose2D binary input. */
1692  /**TPose2D binary output. */
1694  /** TPose3D binary input. */
1696  /** TPose3D binary output. */
1698  /**TSegment2D binary input. */
1700  /** TSegment2D binary output. */
1702  /**TLine2D binary input. */
1704  /** TLine2D binary output. */
1706  /** TObject2D binary input. */
1708  /** TObject2D binary input. */
1710  /** TSegment3D binary input. */
1712  /**TSegment3D binary output. */
1714  /** TLine3D binary input. */
1716  /** TLine3D binary output. */
1718  /** TPlane binary input. */
1720  /** TPlane binary output. */
1722  /** TObject3D binary input. */
1724  /** TObject3D binary output. */
1726 
1727  /** @} */ // end of grouping
1728 
1729  } //end of namespace math
1730 
1731  namespace utils
1732  {
1733  // Specialization must occur in the same namespace
1747 
1748  } // end of namespace utils
1749 
1750 } //end of namespace
1751 #endif
bool operator!=(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:287
double distanceTo(const TPoint3D &p) const
Point-to-point distance.
TSegment2D(const TPoint2D &p1, const TPoint2D &p2)
Constructor from both points.
TPoint2D & operator+=(const TPoint2D &p)
double & operator[](size_t i)
Coordinate access using operator[].
const unsigned char GEOMETRIC_TYPE_PLANE
Object type identifier for TPlane.
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:21
bool getPlane(TPlane &p) const
Gets the content as a plane, returning false if the type is not adequate.
void getUnitaryNormalVector(double(&vector)[2])
Get line&#39;s normal vector after unitarizing line.
TPolygon3D(const std::vector< TPoint3D > &v)
Implicit constructor from a 3D points vector.
#define MRPT_DECLARE_TTYPENAME_NAMESPACE(_TYPE, __NS)
Definition: TTypeName.h:64
void generate2DObject(TObject2D &obj) const
Projects into 2D space.
TPointXYZIu8(double x, double y, double z, uint8_t intensity_val)
TPoint2D & operator*=(double d)
double y
X,Y coordinates.
const unsigned char GEOMETRIC_TYPE_LINE
Object type identifier for TLine2D or TLine3D.
const TPoint2D & operator[](size_t i) const
Access to points using operator[0-1].
TPointXYZfIu8(float x, float y, float z, uint8_t intensity_val)
double norm() const
Point norm.
std::string asString() const
void destroy()
Destroys the object and releases the pointer, if any.
bool contains(const TSegment3D &segment) const
Check whether a segment is fully contained into the plane.
const unsigned char GEOMETRIC_TYPE_POINT
Object type identifier for TPoint2D or TPoint3D.
TPoint2D operator/(double d) const
TPoint2D operator+(const TPoint2D &p) const
T square(const T x)
Inline function for the square of a number.
bool getLine(TLine3D &r) const
Gets the content as a line, returning false if the type is not adequate.
TObject2D(const TPoint2D &p)
Implicit constructor from point.
TPoint3Df(const float xx, const float yy, const float zz)
TLine2D()
Fast default constructor.
double sqrDistanceTo(const TPoint3D &p) const
Point-to-point distance, squared.
void asString(std::string &s) const
Returns a human-readable textual representation of the object as "[x y z qr qx qy qz]"...
double roll
Roll coordinate (rotation angle over X coordinate).
TPoint3D operator-(const TPoint3D &p) const
Points substraction.
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)
std::ostream BASE_IMPEXP & operator<<(std::ostream &o, const TPoint2D &p)
TPolygon3D()
Default constructor.
#define THROW_EXCEPTION(msg)
std::string asString() const
bool isPoint() const
Checks whether content is a point.
void operator=(const TPoint2D &p)
Assign a point to this object.
std::string asString() const
void operator=(const TSegment2D &s)
Assign a segment to this object.
Union containing pointer to actual data.
bool getPoint(TPoint2D &p) const
Gets the content as a point, returning false if the type is inadequate.
void getAsVector(std::vector< double > &v) const
Gets the pose as a vector of doubles.
TPoint3D()
Default fast constructor.
Standard type for storing any lightweight 2D type.
TLine3D()
Fast default constructor.
TObject2D()
Implicit constructor from polygon.
void getAsPose3D(mrpt::poses::CPose3D &outPose)
Gets a pose whose XY plane corresponds to this plane.
TPoint3D pBase
Base point.
Standard object for storing any 3D lightweight object.
STL namespace.
TSegment3D()
Fast default constructor.
TObject3D()
Empty constructor.
void generate2DObject(TPolygon2D &p) const
Projects into a 2D space, discarding the z.
const double & operator[](size_t i) const
Coordinate access using operator[].
double z
X,Y,Z coordinates.
TPoint3D & operator+=(const TPoint3D &p)
Translation.
void getCenter(TPoint2D &p) const
Segment&#39;s central point.
double yaw
Yaw coordinate (rotation angle over Z axis).
void operator=(const TSegment3D &s)
Assigns a segment to this object.
void operator=(const TPoint3D &p)
Assigns a point to this object.
double norm() const
Point norm.
const double & operator[](size_t i) const
Coordinate access using operator[].
TPose2D(double xx, double yy, double pphi)
Constructor from coordinates.
TSegment3D(const TSegment2D &s)
Constructor from 2D object.
TPoint3D operator*(double d) const
void getUnitaryDirectorVector(double(&vector)[2])
Unitarize line and then get director vector.
double & operator[](size_t i)
Coordinate access using operator[].
TPoint2D(const mrpt::poses::CPoseOrPoint< DERIVEDCLASS > &p)
Constructor from CPoseOrPoint, perhaps losing 3D information.
void getCenter(TPoint3D &p) const
Segment&#39;s central point.
TObject3D(const TPlane &p)
Constructor from plane.
TPoint3D point1
Origin point.
void getUnitaryDirectorVector(double(&vector)[3])
Unitarize and then get director vector.
TPose3D()
Default fast constructor.
TPointXYZRGBu8(double x, double y, double z, uint8_t R_val, uint8_t G_val, uint8_t B_val)
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This base provides a set of functions for maths stuff.
Definition: CArray.h:18
const TPoint3D & operator[](size_t i) const
Access to points using operator[0-1].
2D segment, consisting of two points.
void getAsVector(VECTORLIKE &v) const
Transformation into vector.
TPlane(const double(&vec)[4])
Constructor from an array of coefficients.
3D segment, consisting of two points.
struct mrpt::math::TObject2D::tobject2d_data_t data
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.
TPoint3D point2
Destiny point.
float & operator[](size_t i)
Coordinate access using operator[].
struct mrpt::math::TObject3D::tobject3d_data_t data
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.
bool getSegment(TSegment3D &s) const
Gets the content as a segment, returning false if the type is not adequate.
bool getPoint(TPoint3D &p) const
Gets the content as a point, returning false if the type is not adequate.
const unsigned char GEOMETRIC_TYPE_SEGMENT
Object type identifier for TSegment2D or TSegment3D.
bool operator==(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:279
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
3D Plane, represented by its equation
bool isSegment() const
Checks whether content is a segment.
void generate2DObject(TLine2D &l) const
Project into 2D space, discarding the Z coordinate.
class BASE_IMPEXP TPolygon3D
TPoint2D operator*(double d) const
TPolygon2D()
Default constructor.
void operator=(const TPolygon3D &p)
Assigns a polygon to this object.
std::string asString() const
unsigned char type
Object type identifier.
TPose3DQuat()
Default fast constructor.
const double & operator[](size_t i) const
Coordinate access using operator[].
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
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 asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[0.02 1.04]" ) ...
TObject2D & operator=(const TObject2D &obj)
Assign another TObject2D.
double qz
Unit quaternion part, qr,qx,qy,qz.
A class used to store a 2D point.
Definition: CPoint2D.h:36
A class used to store a 3D point.
Definition: CPoint3D.h:32
bool isLine() const
Checks whether content is a line.
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.
std::string asString() const
double pitch
Pitch coordinate (rotation angle over Y axis).
bool isPolygon() const
Checks whether content is a polygon.
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
TPoint2D point1
Origin point.
TPoint3D operator+(const TPoint3D &p) const
Points addition.
unsigned char getType() const
Gets content type.
TPoint2D()
Default fast constructor.
bool operator<(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:283
const double & operator[](size_t i) const
Coordinate access using operator[].
double y
X,Y coordinates.
double norm() const
Pose&#39;s spatial coordinates norm.
TPoint3D & operator*=(const double f)
Point scale.
BASE_IMPEXP::mrpt::utils::CStream & operator>>(mrpt::utils::CStream &in, CMatrixPtr &pObj)
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:113
TObject3D(const TLine3D &r)
Constructor from line.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
TPlane()
Fast default constructor.
bool isPoint() const
Checks whether content is a point.
bool isPolygon() const
Checks whether content is a polygon.
TPose3DQuat(double _x, double _y, double _z, double _qr, double _qx, double _qy, double _qz)
Constructor from coordinates.
void getAsPose3D(mrpt::poses::CPose3D &outPose) const
Gets a pose whose XY plane corresponds to this plane.
const double & operator[](size_t i) const
Coordinate access using operator[].
void operator=(const TPolygon2D &p)
Assign a polygon to this object.
XYZ point (double) + RGB(u8)
A class used to store a 2D pose.
Definition: CPose2D.h:36
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[0.02 1.04 -0.8]" )
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
struct BASE_IMPEXP TLine3D
void getUnitaryNormalVector(double(&vec)[3])
Unitarize, then get normal vector.
TPoint3D operator/(double d) const
bool getSegment(TSegment2D &s) const
Gets the content as a segment, returning false if the type is inadequate.
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...
void getAsVector(std::vector< double > &v) const
Transformation into vector.
TSegment2D()
Fast default constructor.
unsigned char type
Object type identifier.
~TObject2D()
Object destruction.
TPoint3D & operator[](size_t i)
Access to points using operator[0-1].
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Lightweight 2D pose.
double norm() const
Pose&#39;s spatial coordinates norm.
bool getPolygon(TPolygon2D &p) const
Gets the content as a polygon, returning false if the type is inadequate.
TObject2D(const TPolygon2D &p)
Implicit constructor from polygon.
TSegment3D(const TPoint3D &p1, const TPoint3D &p2)
Constructor from both points.
TPoint2D operator-(const TPoint2D &p) const
void getDirectorVector(double(&vector)[3]) const
Get director vector.
bool getPolygon(TPolygon3D &p) const
Gets the content as a polygon, returning false if the type is not adequate.
TObject3D(const TObject3D &obj)
Constructs from another object.
TPose2D()
Default fast constructor.
const unsigned char GEOMETRIC_TYPE_UNDEFINED
Object type identifier for empty TObject2D or TObject3D.
TPoint2D & operator-=(const TPoint2D &p)
void generate2DObject(TSegment2D &s) const
Projection into 2D space, discarding the z.
void operator=(const TLine3D &l)
Assigns a line to this object.
double & operator[](size_t i)
Coordinate access using operator[].
Lightweight 3D point.
TPolygon3D(size_t N)
Constructor for a given size.
void operator=(const TLine2D &l)
Assign a line to this object.
const float & operator[](size_t i) const
Coordinate access using operator[].
void destroy()
Destroys the object, releasing the pointer to the content (if any).
bool isPlane() const
Checks whether content is a plane.
TObject3D(const TSegment3D &s)
Constructor from segment.
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)
bool isSegment() const
Checks whether content is a segment.
TPoint3D operator-(const TPoint3D &p1)
Unary minus operator for 3D points.
unsigned char getType() const
Gets object type.
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)
bool getLine(TLine2D &r) const
Gets the content as a line, returning false if the type is inadequate.
double & operator[](size_t i)
Coordinate access using operator[].
2D polygon, inheriting from std::vector<TPoint2D>.
TPoint3D & operator-=(const TPoint3D &p)
Difference between points.
3D polygon, inheriting from std::vector<TPoint3D>
Union type storing pointers to every allowed type.
bool isLine() const
Checks whether content is a line.
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.
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
TPoint2D & operator/=(double d)
void getAsVector(std::vector< double > &v) const
Transformation into vector.
void getAsVector(std::vector< double > &v) const
Gets the pose as a vector of doubles.
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.11 for MRPT 1.4.0 SVN:Unversioned directory at Tue Jun 28 11:46:25 UTC 2016