Main MRPT website > C++ reference for MRPT 1.3.2
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-2015, 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  /**
357  * Lightweight 3D pose (three spatial coordinates, plus three angular coordinates). Allows coordinate access using [] operator.
358  * \sa mrpt::poses::CPose3D
359  */
361  enum { static_size = 6 };
362  double x,y,z; //!< X,Y,Z, coords
363  double yaw; //!< Yaw coordinate (rotation angle over Z axis).
364  double pitch; //!< Pitch coordinate (rotation angle over Y axis).
365  double roll; //!< Roll coordinate (rotation angle over X coordinate).
366  /** Implicit constructor from TPoint2D. Zeroes all the unprovided information.
367  * \sa TPoint2D
368  */
369  TPose3D(const TPoint2D &p);
370  /**
371  * Implicit constructor from TPose2D. Gets the yaw from the 2D pose's phi, zeroing all the unprovided information.
372  * \sa TPose2D
373  */
374  TPose3D(const TPose2D &p);
375  /**
376  * Implicit constructor from TPoint3D. Zeroes angular information.
377  * \sa TPoint3D
378  */
379  TPose3D(const TPoint3D &p);
380  /**
381  * Implicit constructor from heavyweight type.
382  * \sa mrpt::poses::CPose3D
383  */
384  TPose3D(const mrpt::poses::CPose3D &p);
385  /**
386  * Constructor from coordinates.
387  */
388  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) {}
389  /**
390  * Default fast constructor. Initializes to garbage.
391  */
392  inline TPose3D() {}
393  /** Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll */
394  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"); } }
395  /** Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll */
396  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"); } }
397  /**
398  * Pose's spatial coordinates norm.
399  */
400  double norm() const {
401  return sqrt(square(x)+square(y)+square(z));
402  }
403  /**
404  * Gets the pose as a vector of doubles.
405  */
406  void getAsVector(std::vector<double> &v) const {
407  v.resize(6);
408  v[0]=x; v[1]=y; v[2]=z; v[3]=yaw; v[4]=pitch; v[5]=roll;
409  }
410  /** Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]", angles in degrees.)
411  * \sa fromString
412  */
413  void asString(std::string &s) const;
414  inline std::string asString() const { std::string s; asString(s); return s; }
415 
416  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" )
417  * \sa asString
418  * \exception std::exception On invalid format
419  */
420  void fromString(const std::string &s);
421  static size_t size() { return 6; }
422  };
423 
424  /** Lightweight 3D pose (three spatial coordinates, plus a quaternion ). Allows coordinate access using [] operator.
425  * \sa mrpt::poses::CPose3DQuat
426  */
428  enum { static_size = 7 };
429  double x,y,z; //!< Translation in x,y,z
430  double qr,qx,qy,qz; //!< Unit quaternion part, qr,qx,qy,qz
431 
432  /** Constructor from coordinates. */
433  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) { }
434  /** Default fast constructor. Initializes to garbage. */
435  inline TPose3DQuat() {}
436  /** Constructor from a CPose3DQuat */
438 
439  /** Coordinate access using operator[]. Order: x,y,z,qr,qx,qy,qz */
440  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"); } }
441  /** Coordinate access using operator[]. Order: x,y,z,qr,qx,qy,qz */
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 qr; case 4: return qx; case 5: return qy; case 6: return qz; default: throw std::out_of_range("index out of range"); } }
443  /** Pose's spatial coordinates norm. */
444  double norm() const {
445  return sqrt(square(x)+square(y)+square(z));
446  }
447  /** Gets the pose as a vector of doubles. */
448  void getAsVector(std::vector<double> &v) const {
449  v.resize(7);
450  for (size_t i=0;i<7;i++) v[i]=(*this)[i];
451  }
452  /** Returns a human-readable textual representation of the object as "[x y z qr qx qy qz]"
453  * \sa fromString
454  */
455  void asString(std::string &s) const { s = mrpt::format("[%f %f %f %f %f %f %f]",x,y,z,qr,qx,qy,qz); }
456  inline std::string asString() const { std::string s; asString(s); return s; }
457 
458  /** 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]" )
459  * \sa asString
460  * \exception std::exception On invalid format
461  */
462  void fromString(const std::string &s);
463  static size_t size() { return 7; }
464  };
465 
466  // Text streaming functions:
467  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPoint2D & p);
468  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPoint3D & p);
469  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose2D & p);
470  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose3D & p);
471  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose3DQuat & p);
472 
473 
474  /**
475  * Unary minus operator for 3D points.
476  */
477  inline TPoint3D operator-(const TPoint3D &p1) {
478  return TPoint3D(-p1.x,-p1.y,-p1.z);
479  }
480  /**
481  * Exact comparison between 2D points.
482  */
483  inline bool operator==(const TPoint2D &p1,const TPoint2D &p2) {
484  return (p1.x==p2.x)&&(p1.y==p2.y); //-V550
485  }
486  /**
487  * Exact comparison between 2D points.
488  */
489  inline bool operator!=(const TPoint2D &p1,const TPoint2D &p2) {
490  return (p1.x!=p2.x)||(p1.y!=p2.y); //-V550
491  }
492  /**
493  * Exact comparison between 3D points.
494  */
495  inline bool operator==(const TPoint3D &p1,const TPoint3D &p2) {
496  return (p1.x==p2.x)&&(p1.y==p2.y)&&(p1.z==p2.z); //-V550
497  }
498  /**
499  * Exact comparison between 3D points.
500  */
501  inline bool operator!=(const TPoint3D &p1,const TPoint3D &p2) {
502  return (p1.x!=p2.x)||(p1.y!=p2.y)||(p1.z!=p2.z); //-V550
503  }
504  /**
505  * Exact comparison between 2D poses, taking possible cycles into account.
506  */
507  inline bool operator==(const TPose2D &p1,const TPose2D &p2) {
508  return (p1.x==p2.x)&&(p1.y==p2.y)&&(mrpt::math::wrapTo2Pi(p1.phi)==mrpt::math::wrapTo2Pi(p2.phi)); //-V550
509  }
510  /**
511  * Exact comparison between 2D poses, taking possible cycles into account.
512  */
513  inline bool operator!=(const TPose2D &p1,const TPose2D &p2) {
514  return (p1.x!=p2.x)||(p1.y!=p2.y)||(mrpt::math::wrapTo2Pi(p1.phi)!=mrpt::math::wrapTo2Pi(p2.phi)); //-V550
515  }
516  /**
517  * Exact comparison between 3D poses, taking possible cycles into account.
518  */
519  inline bool operator==(const TPose3D &p1,const TPose3D &p2) {
520  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
521  }
522  /**
523  * Exact comparison between 3D poses, taking possible cycles into account.
524  */
525  inline bool operator!=(const TPose3D &p1,const TPose3D &p2) {
526  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
527  }
528  //Forward declarations
533 
534  //Pragma defined to ensure no structure packing
535  /**
536  * 2D segment, consisting of two points.
537  * \sa TSegment3D,TLine2D,TPolygon2D,TPoint2D
538  */
540  public:
541  /**
542  * Origin point.
543  */
545  /**
546  * Destiny point.
547  */
549  /**
550  * Segment length.
551  */
552  double length() const;
553  /**
554  * Distance to point.
555  */
556  double distance(const TPoint2D &point) const;
557  /**
558  * Distance with sign to point (sign indicates which side the point is).
559  */
560  double signedDistance(const TPoint2D &point) const;
561  /**
562  * Check whether a point is inside a segment.
563  */
564  bool contains(const TPoint2D &point) const;
565  /** Access to points using operator[0-1] */
566  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"); } }
567  /** Access to points using operator[0-1] */
568  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"); } }
569  /**
570  * Project into 3D space, setting the z to 0.
571  */
572  void generate3DObject(TSegment3D &s) const;
573  /**
574  * Segment's central point.
575  */
576  inline void getCenter(TPoint2D &p) const {
577  p.x=(point1.x+point2.x)/2;
578  p.y=(point1.y+point2.y)/2;
579  }
580  /**
581  * Constructor from both points.
582  */
583  TSegment2D(const TPoint2D &p1,const TPoint2D &p2):point1(p1),point2(p2) {}
584  /**
585  * Fast default constructor. Initializes to garbage.
586  */
588  /**
589  * Explicit constructor from 3D object, discarding the z.
590  */
591  explicit TSegment2D(const TSegment3D &s);
592 
593  bool operator<(const TSegment2D &s) const;
594  };
595  /**
596  * 3D segment, consisting of two points.
597  * \sa TSegment2D,TLine3D,TPlane,TPolygon3D,TPoint3D
598  */
600  public:
601  /**
602  * Origin point.
603  */
605  /**
606  * Destiny point.
607  */
609  /**
610  * Segment length.
611  */
612  double length() const;
613  /**
614  * Distance to point.
615  */
616  double distance(const TPoint3D &point) const;
617  /**
618  * Distance to another segment.
619  */
620  double distance(const TSegment3D &segment) const;
621  /**
622  * Check whether a point is inside the segment.
623  */
624  bool contains(const TPoint3D &point) const;
625  /** Access to points using operator[0-1] */
626  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"); } }
627  /** Access to points using operator[0-1] */
628  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"); } }
629  /**
630  * Projection into 2D space, discarding the z.
631  */
632  inline void generate2DObject(TSegment2D &s) const {
633  s=TSegment2D(*this);
634  }
635  /**
636  * Segment's central point.
637  */
638  inline void getCenter(TPoint3D &p) const {
639  p.x=(point1.x+point2.x)/2;
640  p.y=(point1.y+point2.y)/2;
641  p.z=(point1.z+point2.z)/2;
642  }
643  /**
644  * Constructor from both points.
645  */
646  TSegment3D(const TPoint3D &p1,const TPoint3D &p2):point1(p1),point2(p2) {}
647  /**
648  * Fast default constructor. Initializes to garbage.
649  */
651  /**
652  * Constructor from 2D object. Sets the z to zero.
653  */
654  TSegment3D(const TSegment2D &s):point1(s.point1),point2(s.point2) {}
655 
656  bool operator<(const TSegment3D &s) const;
657  };
658 
659  inline bool operator==(const TSegment2D &s1,const TSegment2D &s2) {
660  return (s1.point1==s2.point1)&&(s1.point2==s2.point2);
661  }
662 
663  inline bool operator!=(const TSegment2D &s1,const TSegment2D &s2) {
664  return (s1.point1!=s2.point1)||(s1.point2!=s2.point2);
665  }
666 
667  inline bool operator==(const TSegment3D &s1,const TSegment3D &s2) {
668  return (s1.point1==s2.point1)&&(s1.point2==s2.point2);
669  }
670 
671  inline bool operator!=(const TSegment3D &s1,const TSegment3D &s2) {
672  return (s1.point1!=s2.point1)||(s1.point2!=s2.point2);
673  }
674 
675  /**
676  * 2D line without bounds, represented by its equation \f$Ax+By+C=0\f$.
677  * \sa TLine3D,TSegment2D,TPolygon2D,TPoint2D
678  */
680  public:
681  /**
682  * Line coefficients, stored as an array: \f$\left[A,B,C\right]\f$.
683  */
684  double coefs[3];
685  /**
686  * Evaluate point in the line's equation.
687  */
688  double evaluatePoint(const TPoint2D &point) const;
689  /**
690  * Check whether a point is inside the line.
691  */
692  bool contains(const TPoint2D &point) const;
693  /**
694  * Distance from a given point.
695  */
696  double distance(const TPoint2D &point) const;
697  /**
698  * Distance with sign from a given point (sign indicates side).
699  */
700  double signedDistance(const TPoint2D &point) const;
701  /**
702  * Get line's normal vector.
703  */
704  void getNormalVector(double (&vector)[2]) const;
705  /**
706  * Unitarize line's normal vector.
707  */
708  void unitarize();
709  /**
710  * Get line's normal vector after unitarizing line.
711  */
712  inline void getUnitaryNormalVector(double (&vector)[2]) {
713  unitarize();
714  getNormalVector(vector);
715  }
716  /**
717  * Get line's director vector.
718  */
719  void getDirectorVector(double (&vector)[2]) const;
720  /**
721  * Unitarize line and then get director vector.
722  */
723  inline void getUnitaryDirectorVector(double (&vector)[2]) {
724  unitarize();
725  getDirectorVector(vector);
726  }
727  /**
728  * Project into 3D space, setting the z to 0.
729  */
730  void generate3DObject(TLine3D &l) const;
731  /**
732  * Get a pose2D whose X axis corresponds to the line.
733  * \sa mrpt::poses::CPose2D.
734  */
735  void getAsPose2D(mrpt::poses::CPose2D &outPose) const;
736  /**
737  * Get a pose2D whose X axis corresponds to the line, forcing the base point to one given.
738  * \throw logic_error if the point is not inside the line.
739  * \sa mrpt::poses::CPose2D.
740  */
741  void getAsPose2DForcingOrigin(const TPoint2D &origin,mrpt::poses::CPose2D &outPose) const;
742  /**
743  * Constructor from two points, through which the line will pass.
744  * \throw logic_error if both points are the same
745  */
746  TLine2D(const TPoint2D &p1,const TPoint2D &p2) throw(std::logic_error);
747  /**
748  * Constructor from a segment.
749  */
750  explicit TLine2D(const TSegment2D &s);
751  /**
752  * Fast default constructor. Initializes to garbage.
753  */
754  TLine2D() {}
755  /**
756  * Constructor from line's coefficients.
757  */
758  inline TLine2D(double A,double B,double C) {
759  coefs[0]=A;
760  coefs[1]=B;
761  coefs[2]=C;
762  }
763  /**
764  * Construction from 3D object, discarding the Z.
765  * \throw std::logic_error if the line is normal to the XY plane.
766  */
767  explicit TLine2D(const TLine3D &l);
768  };
769 
770  /**
771  * 3D line, represented by a base point and a director vector.
772  * \sa TLine2D,TSegment3D,TPlane,TPolygon3D,TPoint3D
773  */
775  public:
776  /**
777  * Base point.
778  */
780  /**
781  * Director vector.
782  */
783  double director[3];
784  /**
785  * Check whether a point is inside the line.
786  */
787  bool contains(const TPoint3D &point) const;
788  /**
789  * Distance between the line and a point.
790  */
791  double distance(const TPoint3D &point) const;
792  /**
793  * Unitarize director vector.
794  */
795  void unitarize();
796  /**
797  * Get director vector.
798  */
799  inline void getDirectorVector(double (&vector)[3]) const {
800  for (size_t i=0;i<3;i++) vector[i]=director[i];
801  }
802  /**
803  * Unitarize and then get director vector.
804  */
805  inline void getUnitaryDirectorVector(double (&vector)[3]) {
806  unitarize();
807  getDirectorVector(vector);
808  }
809  /**
810  * Project into 2D space, discarding the Z coordinate.
811  * \throw std::logic_error if the line's director vector is orthogonal to the XY plane.
812  */
813  inline void generate2DObject(TLine2D &l) const {
814  l=TLine2D(*this);
815  }
816  /**
817  * Constructor from two points, through which the line will pass.
818  * \throw std::logic_error if both points are the same.
819  */
820  TLine3D(const TPoint3D &p1,const TPoint3D &p2) throw(std::logic_error);
821  /**
822  * Constructor from 3D segment.
823  */
824  explicit TLine3D(const TSegment3D &s);
825  /**
826  * Fast default constructor. Initializes to garbage.
827  */
828  TLine3D() {}
829  /**
830  * Implicit constructor from 2D object. Zeroes the z.
831  */
832  TLine3D(const TLine2D &l);
833  };
834 
835  /**
836  * 3D Plane, represented by its equation \f$Ax+By+Cz+D=0\f$
837  * \sa TSegment3D,TLine3D,TPolygon3D,TPoint3D
838  */
840  public:
841  /**
842  * Plane coefficients, stored as an array: \f$\left[A,B,C,D\right]\f$
843  */
844  double coefs[4];
845  /**
846  * Evaluate a point in the plane's equation.
847  */
848  double evaluatePoint(const TPoint3D &point) const;
849  /**
850  * Check whether a point is contained into the plane.
851  */
852  bool contains(const TPoint3D &point) const;
853  /**
854  * Check whether a segment is fully contained into the plane.
855  */
856  inline bool contains(const TSegment3D &segment) const {
857  return contains(segment.point1)&&contains(segment.point2);
858  }
859  /**
860  * Check whether a line is fully contained into the plane.
861  */
862  bool contains(const TLine3D &line) const;
863  /**
864  * Distance to 3D point.
865  */
866  double distance(const TPoint3D &point) const;
867  /**
868  * Distance to 3D line. Will be zero if the line is not parallel to the plane.
869  */
870  double distance(const TLine3D &line) const;
871  /**
872  * Get plane's normal vector.
873  */
874  void getNormalVector(double (&vec)[3]) const;
875  /**
876  * Unitarize normal vector.
877  */
878  void unitarize();
879  /**
880  * Unitarize, then get normal vector.
881  */
882  inline void getUnitaryNormalVector(double (&vec)[3]) {
883  unitarize();
884  getNormalVector(vec);
885  }
886  /**
887  * Gets a pose whose XY plane corresponds to this plane.
888  */
889  void getAsPose3D(mrpt::poses::CPose3D &outPose);
890  /**
891  * Gets a pose whose XY plane corresponds to this plane.
892  */
893  inline void getAsPose3D(mrpt::poses::CPose3D &outPose) const {
894  TPlane p=*this;
895  p.getAsPose3D(outPose);
896  }
897  /**
898  * Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates.
899  * \throw std::logic_error if the point is not inside the plane.
900  */
901  void getAsPose3DForcingOrigin(const TPoint3D &newOrigin,mrpt::poses::CPose3D &pose);
902  /**
903  * Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates.
904  * \throw std::logic_error if the point is not inside the plane.
905  */
906  inline void getAsPose3DForcingOrigin(const TPoint3D &newOrigin,mrpt::poses::CPose3D &pose) const {
907  TPlane p=*this;
908  p.getAsPose3DForcingOrigin(newOrigin,pose);
909  }
910  /**
911  * Gets a plane which contains these three points.
912  * \throw std::logic_error if the points are linearly dependants.
913  */
914  TPlane(const TPoint3D &p1,const TPoint3D &p2,const TPoint3D &p3) throw(std::logic_error);
915  /**
916  * Gets a plane which contains this point and this line.
917  * \throw std::logic_error if the point is inside the line.
918  */
919  TPlane(const TPoint3D &p1,const TLine3D &r2) throw(std::logic_error);
920  /**
921  * Gets a plane which contains the two lines.
922  * \throw std::logic_error if the lines do not cross.
923  */
924  TPlane(const TLine3D &r1,const TLine3D &r2) throw(std::logic_error);
925  /**
926  * Fast default constructor. Initializes to garbage.
927  */
928  TPlane() {}
929  /**
930  * Constructor from plane coefficients.
931  */
932  inline TPlane(double A,double B,double C,double D) {
933  coefs[0]=A;
934  coefs[1]=B;
935  coefs[2]=C;
936  coefs[3]=D;
937  }
938  /**
939  * Constructor from an array of coefficients.
940  */
941  inline TPlane(const double (&vec)[4]) {
942  for (size_t i=0;i<4;i++) coefs[i]=vec[i];
943  }
944  };
945 
946  typedef TPlane TPlane3D;
947 
948  /**
949  * 2D polygon, inheriting from std::vector<TPoint2D>.
950  * \sa TPolygon3D,TSegment2D,TLine2D,TPoint2D, CPolygon
951  */
952  class BASE_IMPEXP TPolygon2D:public std::vector<TPoint2D> {
953  public:
954  /**
955  * Distance to a point.
956  */
957  double distance(const TPoint2D &point) const;
958  /**
959  * Check whether a point is inside the polygon.
960  */
961  bool contains(const TPoint2D &point) const;
962  /**
963  * Gets as set of segments, instead of points.
964  */
965  void getAsSegmentList(std::vector<TSegment2D> &v) const;
966  /**
967  * Projects into 3D space, zeroing the z.
968  */
969  void generate3DObject(TPolygon3D &p) const;
970  /**
971  * Polygon's central point.
972  */
973  void getCenter(TPoint2D &p) const;
974  /**
975  * Checks whether is convex.
976  */
977  bool isConvex() const;
978  /**
979  * Erase repeated vertices.
980  * \sa removeRedundantVertices
981  */
982  void removeRepeatedVertices();
983  /**
984  * Erase every redundant vertex from the polygon, saving space.
985  * \sa removeRepeatedVertices
986  */
987  void removeRedundantVertices();
988  /**
989  * Gets plot data, ready to use on a 2D plot.
990  * \sa mrpt::gui::CDisplayWindowPlots
991  */
992  void getPlotData(std::vector<double> &x,std::vector<double> &y) const;
993  /**
994  * Default constructor.
995  */
996  TPolygon2D():std::vector<TPoint2D>() {}
997  /**
998  * Constructor for a given number of vertices, intializing them as garbage.
999  */
1000  explicit TPolygon2D(size_t N):std::vector<TPoint2D>(N) {}
1001  /**
1002  * Implicit constructor from a vector of 2D points.
1003  */
1004  TPolygon2D(const std::vector<TPoint2D> &v):std::vector<TPoint2D>(v) {}
1005  /**
1006  * Constructor from a 3D object.
1007  */
1008  explicit TPolygon2D(const TPolygon3D &p);
1009  /**
1010  * Static method to create a regular polygon, given its size and radius.
1011  * \throw std::logic_error if radius is near zero or the number of edges is less than three.
1012  */
1013  static void createRegularPolygon(size_t numEdges,double radius,TPolygon2D &poly);
1014  /**
1015  * Static method to create a regular polygon from its size and radius. The center will correspond to the given pose.
1016  * \throw std::logic_error if radius is near zero or the number of edges is less than three.
1017  */
1018  static inline void createRegularPolygon(size_t numEdges,double radius,TPolygon2D &poly,const mrpt::poses::CPose2D &pose);
1019  };
1020 
1021  /**
1022  * 3D polygon, inheriting from std::vector<TPoint3D>
1023  * \sa TPolygon2D,TSegment3D,TLine3D,TPlane,TPoint3D
1024  */
1025  class BASE_IMPEXP TPolygon3D:public std::vector<TPoint3D> {
1026  public:
1027  /**
1028  * Distance to point.
1029  */
1030  double distance(const TPoint3D &point) const;
1031  /**
1032  * Check whether a point is inside the polygon.
1033  */
1034  bool contains(const TPoint3D &point) const;
1035  /**
1036  * Gets as set of segments, instead of set of points.
1037  */
1038  void getAsSegmentList(std::vector<TSegment3D> &v) const;
1039  /**
1040  * Gets a plane which contains the polygon. Returns false if the polygon is skew and cannot be fit inside a plane.
1041  */
1042  bool getPlane(TPlane &p) const;
1043  /**
1044  * Gets the best fitting plane, disregarding whether the polygon actually fits inside or not.
1045  * \sa getBestFittingPlane
1046  */
1047  void getBestFittingPlane(TPlane &p) const;
1048  /**
1049  * Projects into a 2D space, discarding the z.
1050  * \get getPlane,isSkew
1051  */
1052  inline void generate2DObject(TPolygon2D &p) const {
1053  p=TPolygon2D(*this);
1054  }
1055  /**
1056  * Get polygon's central point.
1057  */
1058  void getCenter(TPoint3D &p) const;
1059  /**
1060  * Check whether the polygon is skew. Returns true if there doesn't exist a plane in which the polygon can fit.
1061  * \sa getBestFittingPlane
1062  */
1063  bool isSkew() const;
1064  /**
1065  * Remove polygon's repeated vertices.
1066  */
1067  void removeRepeatedVertices();
1068  /**
1069  * Erase every redundant vertex, thus saving space.
1070  */
1071  void removeRedundantVertices();
1072  /**
1073  * Default constructor. Creates a polygon with no vertices.
1074  */
1075  TPolygon3D():std::vector<TPoint3D>() {}
1076  /**
1077  * Constructor for a given size. Creates a polygon with a fixed number of vertices, which are initialized to garbage.
1078  */
1079  explicit TPolygon3D(size_t N):std::vector<TPoint3D>(N) {}
1080  /**
1081  * Implicit constructor from a 3D points vector.
1082  */
1083  TPolygon3D(const std::vector<TPoint3D> &v):std::vector<TPoint3D>(v) {}
1084  /**
1085  * Constructor from a 2D object. Zeroes the z.
1086  */
1087  TPolygon3D(const TPolygon2D &p);
1088  /**
1089  * Static method to create a regular polygon, given its size and radius.
1090  * \throw std::logic_error if number of edges is less than three, or radius is near zero.
1091  */
1092  static void createRegularPolygon(size_t numEdges,double radius,TPolygon3D &poly);
1093  /**
1094  * Static method to create a regular polygon, given its size and radius. The center will be located on the given pose.
1095  * \throw std::logic_error if number of edges is less than three, or radius is near zero.
1096  */
1097  static inline void createRegularPolygon(size_t numEdges,double radius,TPolygon3D &poly,const mrpt::poses::CPose3D &pose);
1098  };
1099 
1100  /**
1101  * Object type identifier for TPoint2D or TPoint3D.
1102  * \sa TObject2D,TObject3D
1103  */
1104  const unsigned char GEOMETRIC_TYPE_POINT=0;
1105  /**
1106  * Object type identifier for TSegment2D or TSegment3D.
1107  * \sa TObject2D,TObject3D
1108  */
1109  const unsigned char GEOMETRIC_TYPE_SEGMENT=1;
1110  /**
1111  * Object type identifier for TLine2D or TLine3D.
1112  * \sa TObject2D,TObject3D
1113  */
1114  const unsigned char GEOMETRIC_TYPE_LINE=2;
1115  /**
1116  * Object type identifier for TPolygon2D or TPolygon3D.
1117  * \sa TObject2D,TObject3D
1118  */
1119  const unsigned char GEOMETRIC_TYPE_POLYGON=3;
1120  /**
1121  * Object type identifier for TPlane.
1122  * \sa TObject3D
1123  */
1124  const unsigned char GEOMETRIC_TYPE_PLANE=4;
1125  /**
1126  * Object type identifier for empty TObject2D or TObject3D.
1127  * \sa TObject2D,TObject3D
1128  */
1129  const unsigned char GEOMETRIC_TYPE_UNDEFINED=255;
1130 
1131  /**
1132  * Standard type for storing any lightweight 2D type. Do not inherit from this class.
1133  * \sa TPoint2D,TSegment2D,TLine2D,TPolygon2D
1134  */
1136  private:
1137  /**
1138  * Object type identifier.
1139  */
1140  unsigned char type;
1141  /**
1142  * Union type storing pointers to every allowed type.
1143  */
1149 
1150  tobject2d_data_t() : polygon(NULL) { }
1151  } data;
1152  /**
1153  * Destroys the object, releasing the pointer to the content (if any).
1154  */
1155  inline void destroy() {
1156  if (type==GEOMETRIC_TYPE_POLYGON) delete data.polygon;
1158  }
1159  public:
1160  /**
1161  * Implicit constructor from point.
1162  */
1163  inline TObject2D(const TPoint2D &p):type(GEOMETRIC_TYPE_POINT) {
1164  data.point=p;
1165  }
1166  /**
1167  * Implicit constructor from segment.
1168  */
1169  inline TObject2D(const TSegment2D &s):type(GEOMETRIC_TYPE_SEGMENT) {
1170  data.segment=s;
1171  }
1172  /**
1173  * Implicit constructor from line.
1174  */
1175  inline TObject2D(const TLine2D &r):type(GEOMETRIC_TYPE_LINE) {
1176  data.line=r;
1177  }
1178  /**
1179  * Implicit constructor from polygon.
1180  */
1181  inline TObject2D(const TPolygon2D &p):type(GEOMETRIC_TYPE_POLYGON) {
1182  data.polygon=new TPolygon2D(p);
1183  }
1184  /**
1185  * Implicit constructor from polygon.
1186  */
1187  TObject2D():type(GEOMETRIC_TYPE_UNDEFINED) {}
1188  /**
1189  * Object destruction.
1190  */
1192  destroy();
1193  }
1194  /**
1195  * Checks whether content is a point.
1196  */
1197  inline bool isPoint() const {
1198  return type==GEOMETRIC_TYPE_POINT;
1199  }
1200  /**
1201  * Checks whether content is a segment.
1202  */
1203  inline bool isSegment() const {
1204  return type==GEOMETRIC_TYPE_SEGMENT;
1205  }
1206  /**
1207  * Checks whether content is a line.
1208  */
1209  inline bool isLine() const {
1210  return type==GEOMETRIC_TYPE_LINE;
1211  }
1212  /**
1213  * Checks whether content is a polygon.
1214  */
1215  inline bool isPolygon() const {
1216  return type==GEOMETRIC_TYPE_POLYGON;
1217  }
1218  /**
1219  * Gets content type.
1220  */
1221  inline unsigned char getType() const {
1222  return type;
1223  }
1224  /**
1225  * Gets the content as a point, returning false if the type is inadequate.
1226  */
1227  inline bool getPoint(TPoint2D &p) const {
1228  if (isPoint()) {
1229  p=data.point;
1230  return true;
1231  } else return false;
1232  }
1233  /**
1234  * Gets the content as a segment, returning false if the type is inadequate.
1235  */
1236  inline bool getSegment(TSegment2D &s) const {
1237  if (isSegment()) {
1238  s=data.segment;
1239  return true;
1240  } else return false;
1241  }
1242  /**
1243  * Gets the content as a line, returning false if the type is inadequate.
1244  */
1245  inline bool getLine(TLine2D &r) const {
1246  if (isLine()) {
1247  r=data.line;
1248  return true;
1249  } else return false;
1250  }
1251  /**
1252  * Gets the content as a polygon, returning false if the type is inadequate.
1253  */
1254  inline bool getPolygon(TPolygon2D &p) const {
1255  if (isPolygon()) {
1256  p=*(data.polygon);
1257  return true;
1258  } else return false;
1259  }
1260  /**
1261  * Assign another TObject2D. Pointers are not shared.
1262  */
1264  if (this==&obj) return *this;
1265  destroy();
1266  switch (type=obj.type) {
1267  case GEOMETRIC_TYPE_POINT:
1268  data.point=obj.data.point;
1269  break;
1270  case GEOMETRIC_TYPE_SEGMENT:
1271  data.segment=obj.data.segment;
1272  break;
1273  case GEOMETRIC_TYPE_LINE:
1274  data.line=obj.data.line;
1275  break;
1276  case GEOMETRIC_TYPE_POLYGON:
1277  data.polygon=new TPolygon2D(*(obj.data.polygon));
1278  break;
1279  }
1280  return *this;
1281  }
1282  /**
1283  * Assign a point to this object.
1284  */
1285  inline void operator=(const TPoint2D &p) {
1286  destroy();
1287  type=GEOMETRIC_TYPE_POINT;
1288  data.point=p;
1289  }
1290  /**
1291  * Assign a segment to this object.
1292  */
1293  inline void operator=(const TSegment2D &s) {
1294  destroy();
1296  data.segment=s;
1297  }
1298  /**
1299  * Assign a line to this object.
1300  */
1301  inline void operator=(const TLine2D &l) {
1302  destroy();
1303  type=GEOMETRIC_TYPE_LINE;
1304  data.line=l;
1305  }
1306  /**
1307  * Assign a polygon to this object.
1308  */
1309  inline void operator=(const TPolygon2D &p) {
1310  destroy();
1312  data.polygon=new TPolygon2D(p);
1313  }
1314  /**
1315  * Project into 3D space.
1316  */
1317  void generate3DObject(TObject3D &obj) const;
1318  /**
1319  * Constructor from another TObject2D.
1320  */
1321  TObject2D(const TObject2D &obj):type(GEOMETRIC_TYPE_UNDEFINED) {
1322  operator=(obj);
1323  }
1324  /**
1325  * Static method to retrieve all the points in a vector of TObject2D.
1326  */
1327  static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts);
1328  /**
1329  * Static method to retrieve all the segments in a vector of TObject2D.
1330  */
1331  static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms);
1332  /**
1333  * Static method to retrieve all the lines in a vector of TObject2D.
1334  */
1335  static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins);
1336  /**
1337  * Static method to retrieve all the polygons in a vector of TObject2D.
1338  */
1339  static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys);
1340  /**
1341  * Static method to retrieve all the points in a vector of TObject2D, returning the remainder objects in another parameter.
1342  */
1343  static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts,std::vector<TObject2D> &remainder);
1344  /**
1345  * Static method to retrieve all the segments in a vector of TObject2D, returning the remainder objects in another parameter.
1346  */
1347  static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms,std::vector<TObject2D> &remainder);
1348  /**
1349  * Static method to retrieve all the lines in a vector of TObject2D, returning the remainder objects in another parameter.
1350  */
1351  static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins,std::vector<TObject2D> &remainder);
1352  /**
1353  * Static method to retrieve all the polygons in a vector of TObject2D, returning the remainder objects in another parameter.
1354  */
1355  static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys,std::vector<TObject2D> &remainder);
1356  };
1357  /**
1358  * Standard object for storing any 3D lightweight object. Do not inherit from this class.
1359  * \sa TPoint3D,TSegment3D,TLine3D,TPlane,TPolygon3D
1360  */
1362  private:
1363  /**
1364  * Object type identifier.
1365  */
1366  unsigned char type;
1367  /**
1368  * Union containing pointer to actual data.
1369  */
1376 
1377  tobject3d_data_t() : polygon(NULL) { }
1378  } data;
1379  /**
1380  * Destroys the object and releases the pointer, if any.
1381  */
1382  void destroy() {
1383  if (type==GEOMETRIC_TYPE_POLYGON) delete data.polygon;
1385  }
1386  public:
1387  /**
1388  * Constructor from point.
1389  */
1390  TObject3D(const TPoint3D &p):type(GEOMETRIC_TYPE_POINT) {
1391  data.point=p;
1392  }
1393  /**
1394  * Constructor from segment.
1395  */
1396  TObject3D(const TSegment3D &s):type(GEOMETRIC_TYPE_SEGMENT) {
1397  data.segment=s;
1398  }
1399  /**
1400  * Constructor from line.
1401  */
1402  TObject3D(const TLine3D &r):type(GEOMETRIC_TYPE_LINE) {
1403  data.line=r;
1404  }
1405  /**
1406  * Constructor from polygon.
1407  */
1408  TObject3D(const TPolygon3D &p):type(GEOMETRIC_TYPE_POLYGON) {
1409  data.polygon=new TPolygon3D(p);
1410  }
1411  /**
1412  * Constructor from plane.
1413  */
1414  TObject3D(const TPlane &p):type(GEOMETRIC_TYPE_PLANE) {
1415  data.plane=p;
1416  }
1417  /**
1418  * Empty constructor.
1419  */
1420  TObject3D():type(GEOMETRIC_TYPE_UNDEFINED) {}
1421  /**
1422  * Destructor.
1423  */
1425  destroy();
1426  }
1427  /**
1428  * Checks whether content is a point.
1429  */
1430  inline bool isPoint() const {
1431  return type==GEOMETRIC_TYPE_POINT;
1432  }
1433  /**
1434  * Checks whether content is a segment.
1435  */
1436  inline bool isSegment() const {
1437  return type==GEOMETRIC_TYPE_SEGMENT;
1438  }
1439  /**
1440  * Checks whether content is a line.
1441  */
1442  inline bool isLine() const {
1443  return type==GEOMETRIC_TYPE_LINE;
1444  }
1445  /**
1446  * Checks whether content is a polygon.
1447  */
1448  inline bool isPolygon() const {
1449  return type==GEOMETRIC_TYPE_POLYGON;
1450  }
1451  /**
1452  * Checks whether content is a plane.
1453  */
1454  inline bool isPlane() const {
1455  return type==GEOMETRIC_TYPE_PLANE;
1456  }
1457  /**
1458  * Gets object type.
1459  */
1460  inline unsigned char getType() const {
1461  return type;
1462  }
1463  /**
1464  * Gets the content as a point, returning false if the type is not adequate.
1465  */
1466  inline bool getPoint(TPoint3D &p) const {
1467  if (isPoint()) {
1468  p=data.point;
1469  return true;
1470  } else return false;
1471  }
1472  /**
1473  * Gets the content as a segment, returning false if the type is not adequate.
1474  */
1475  inline bool getSegment(TSegment3D &s) const {
1476  if (isSegment()) {
1477  s=data.segment;
1478  return true;
1479  } else return false;
1480  }
1481  /**
1482  * Gets the content as a line, returning false if the type is not adequate.
1483  */
1484  inline bool getLine(TLine3D &r) const {
1485  if (isLine()) {
1486  r=data.line;
1487  return true;
1488  } else return false;
1489  }
1490  /**
1491  * Gets the content as a polygon, returning false if the type is not adequate.
1492  */
1493  inline bool getPolygon(TPolygon3D &p) const {
1494  if (isPolygon()) {
1495  p=*(data.polygon);
1496  return true;
1497  } else return false;
1498  }
1499  /**
1500  * Gets the content as a plane, returning false if the type is not adequate.
1501  */
1502  inline bool getPlane(TPlane &p) const {
1503  if (isPlane()) {
1504  p=data.plane;
1505  return true;
1506  } else return false;
1507  }
1508  /**
1509  * Assigns another object, creating a new pointer if needed.
1510  */
1512  if (this==&obj) return *this;
1513  destroy();
1514  switch (type=obj.type) {
1515  case GEOMETRIC_TYPE_POINT:
1516  data.point=obj.data.point;
1517  break;
1518  case GEOMETRIC_TYPE_SEGMENT:
1519  data.segment=obj.data.segment;
1520  break;
1521  case GEOMETRIC_TYPE_LINE:
1522  data.line=obj.data.line;
1523  break;
1524  case GEOMETRIC_TYPE_POLYGON:
1525  data.polygon=new TPolygon3D(*(obj.data.polygon));
1526  break;
1527  case GEOMETRIC_TYPE_PLANE:
1528  data.plane=obj.data.plane;
1529  break;
1530  case GEOMETRIC_TYPE_UNDEFINED:
1531  break;
1532  default:
1533  THROW_EXCEPTION("Invalid TObject3D object");
1534  }
1535  return *this;
1536  }
1537  /**
1538  * Assigns a point to this object.
1539  */
1540  inline void operator=(const TPoint3D &p) {
1541  destroy();
1542  type=GEOMETRIC_TYPE_POINT;
1543  data.point=p;
1544  }
1545  /**
1546  * Assigns a segment to this object.
1547  */
1548  inline void operator=(const TSegment3D &s) {
1549  destroy();
1551  data.segment=s;
1552  }
1553  /**
1554  * Assigns a line to this object.
1555  */
1556  inline void operator=(const TLine3D &l) {
1557  destroy();
1558  type=GEOMETRIC_TYPE_LINE;
1559  data.line=l;
1560  }
1561  /**
1562  * Assigns a polygon to this object.
1563  */
1564  inline void operator=(const TPolygon3D &p) {
1565  destroy();
1567  data.polygon=new TPolygon3D(p);
1568  }
1569  /**
1570  * Assigns a plane to this object.
1571  */
1572  inline void operator=(const TPlane &p) {
1573  destroy();
1574  type=GEOMETRIC_TYPE_PLANE;
1575  data.plane=p;
1576  }
1577  /**
1578  * Projects into 2D space.
1579  * \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).
1580  */
1581  inline void generate2DObject(TObject2D &obj) const {
1582  switch (type) {
1583  case GEOMETRIC_TYPE_POINT:
1584  obj=TPoint2D(data.point);
1585  break;
1586  case GEOMETRIC_TYPE_SEGMENT:
1587  obj=TSegment2D(data.segment);
1588  break;
1589  case GEOMETRIC_TYPE_LINE:
1590  obj=TLine2D(data.line);
1591  break;
1592  case GEOMETRIC_TYPE_POLYGON:
1593  obj=TPolygon2D(*(data.polygon));
1594  break;
1595  case GEOMETRIC_TYPE_PLANE:
1596  throw std::logic_error("Too many dimensions");
1597  default:
1598  obj=TObject2D();
1599  break;
1600  }
1601  }
1602  /**
1603  * Constructs from another object.
1604  */
1605  TObject3D(const TObject3D &obj):type(GEOMETRIC_TYPE_UNDEFINED) {
1606  operator=(obj);
1607  }
1608  /**
1609  * Static method to retrieve every point included in a vector of objects.
1610  */
1611  static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts);
1612  /**
1613  * Static method to retrieve every segment included in a vector of objects.
1614  */
1615  static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms);
1616  /**
1617  * Static method to retrieve every line included in a vector of objects.
1618  */
1619  static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins);
1620  /**
1621  * Static method to retrieve every plane included in a vector of objects.
1622  */
1623  static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns);
1624  /**
1625  * Static method to retrieve every polygon included in a vector of objects.
1626  */
1627  static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys);
1628  /**
1629  * Static method to retrieve every point included in a vector of objects, returning the remaining objects in another argument.
1630  */
1631  static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts,std::vector<TObject3D> &remainder);
1632  /**
1633  * Static method to retrieve every segment included in a vector of objects, returning the remaining objects in another argument.
1634  */
1635  static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms,std::vector<TObject3D> &remainder);
1636  /**
1637  * Static method to retrieve every line included in a vector of objects, returning the remaining objects in another argument.
1638  */
1639  static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins,std::vector<TObject3D> &remainder);
1640  /**
1641  * Static method to retrieve every plane included in a vector of objects, returning the remaining objects in another argument.
1642  */
1643  static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns,std::vector<TObject3D> &remainder);
1644  /**
1645  * Static method to retrieve every polygon included in a vector of objects, returning the remaining objects in another argument.
1646  */
1647  static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder);
1648  };
1649 
1650 
1651 
1652  //Streaming functions
1653  /** TPoint2D binary input. */
1655  /** TPoint2D binary output. */
1657  /** TPoint3D binary input. */
1659  /** TPoint3D binary output. */
1661  /** TPose2D binary input. */
1663  /**TPose2D binary output. */
1665  /** TPose3D binary input. */
1667  /** TPose3D binary output. */
1669  /**TSegment2D binary input. */
1671  /** TSegment2D binary output. */
1673  /**TLine2D binary input. */
1675  /** TLine2D binary output. */
1677  /** TObject2D binary input. */
1679  /** TObject2D binary input. */
1681  /** TSegment3D binary input. */
1683  /**TSegment3D binary output. */
1685  /** TLine3D binary input. */
1687  /** TLine3D binary output. */
1689  /** TPlane binary input. */
1691  /** TPlane binary output. */
1693  /** TObject3D binary input. */
1695  /** TObject3D binary output. */
1697 
1698  /** @} */ // end of grouping
1699 
1700  } //end of namespace math
1701 
1702  namespace utils
1703  {
1704  // Specialization must occur in the same namespace
1718 
1719  } // end of namespace utils
1720 
1721 } //end of namespace
1722 #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.
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].
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.
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.
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
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
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.
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.
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.3.2 SVN: at Wed May 25 02:34:21 UTC 2016