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
void getCenter(TPoint2D &p) const
Segment&#39;s central point.
TSegment2D(const TPoint2D &p1, const TPoint2D &p2)
Constructor from both points.
TPoint2D & operator+=(const TPoint2D &p)
double & operator[](size_t i)
Coordinate access using operator[].
const unsigned char GEOMETRIC_TYPE_PLANE
Object type identifier for TPlane.
bool getPoint(TPoint2D &p) const
Gets the content as a point, returning false if the type is inadequate.
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:21
void getUnitaryNormalVector(double(&vector)[2])
Get line&#39;s normal vector after unitarizing line.
TPolygon3D(const std::vector< TPoint3D > &v)
Implicit constructor from a 3D points vector.
TPoint2D operator/(double d) const
#define MRPT_DECLARE_TTYPENAME_NAMESPACE(_TYPE, __NS)
Definition: TTypeName.h:64
bool getPolygon(TPolygon2D &p) const
Gets the content as a polygon, returning false if the type is inadequate.
TPoint2D & operator*=(double d)
double y
X,Y coordinates.
const unsigned char GEOMETRIC_TYPE_LINE
Object type identifier for TLine2D or TLine3D.
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[0.02 1.04]" ) ...
unsigned char getType() const
Gets content type.
bool isPolygon() const
Checks whether content is a polygon.
void destroy()
Destroys the object and releases the pointer, if any.
const unsigned char GEOMETRIC_TYPE_POINT
Object type identifier for TPoint2D or TPoint3D.
T square(const T x)
Inline function for the square of a number.
TObject2D(const TPoint2D &p)
Implicit constructor from point.
TPoint3Df(const float xx, const float yy, const float zz)
TLine2D()
Fast default constructor.
bool getSegment(TSegment3D &s) const
Gets the content as a segment, returning false if the type is not adequate.
bool contains(const TSegment3D &segment) const
Check whether a segment is fully contained into the plane.
double roll
Roll coordinate (rotation angle over X coordinate).
TPoint3D(double xx, double yy, double zz)
Constructor from coordinates.
std::ostream BASE_IMPEXP & operator<<(std::ostream &o, const TPoint2D &p)
TPolygon3D()
Default constructor.
#define THROW_EXCEPTION(msg)
void operator=(const TPoint2D &p)
Assign a point to this object.
double norm() const
Point norm.
const float & operator[](size_t i) const
Coordinate access using operator[].
void operator=(const TSegment2D &s)
Assign a segment to this object.
std::string asString() const
Union containing pointer to actual data.
TPoint3D()
Default fast constructor.
TPoint3D operator+(const TPoint3D &p) const
Points addition.
Standard type for storing any lightweight 2D type.
TLine3D()
Fast default constructor.
bool getLine(TLine2D &r) const
Gets the content as a line, returning false if the type is inadequate.
TObject2D()
Implicit constructor from polygon.
void getAsPose3D(mrpt::poses::CPose3D &outPose)
Gets a pose whose XY plane corresponds to this plane.
bool getPlane(TPlane &p) const
Gets the content as a plane, returning false if the type is not adequate.
TPoint3D pBase
Base point.
Standard object for storing any 3D lightweight object.
STL namespace.
bool getSegment(TSegment2D &s) const
Gets the content as a segment, returning false if the type is inadequate.
TSegment3D()
Fast default constructor.
TObject3D()
Empty constructor.
double z
X,Y,Z coordinates.
TPoint3D & operator+=(const TPoint3D &p)
Translation.
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.
TPose2D(double xx, double yy, double pphi)
Constructor from coordinates.
TSegment3D(const TSegment2D &s)
Constructor from 2D object.
double norm() const
Pose&#39;s spatial coordinates norm.
void getUnitaryDirectorVector(double(&vector)[2])
Unitarize line and then get director vector.
double & operator[](size_t i)
Coordinate access using operator[].
TPoint2D(const mrpt::poses::CPoseOrPoint< DERIVEDCLASS > &p)
Constructor from CPoseOrPoint, perhaps losing 3D information.
TObject3D(const TPlane &p)
Constructor from plane.
TPoint3D point1
Origin point.
void getUnitaryDirectorVector(double(&vector)[3])
Unitarize and then get director vector.
void generate2DObject(TLine2D &l) const
Project into 2D space, discarding the Z coordinate.
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
bool getPolygon(TPolygon3D &p) const
Gets the content as a polygon, returning false if the type is not adequate.
This base provides a set of functions for maths stuff.
Definition: CArray.h:18
2D segment, consisting of two points.
bool isPoint() const
Checks whether content is a point.
TPlane(const double(&vec)[4])
Constructor from an array of coefficients.
3D segment, consisting of two points.
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.
const double & operator[](size_t i) const
Coordinate access using operator[].
TPoint3D point2
Destiny point.
float & operator[](size_t i)
Coordinate access using operator[].
std::string asString() const
double distanceTo(const TPoint3D &p) const
Point-to-point distance.
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.
const double & operator[](size_t i) const
Coordinate access using operator[].
const unsigned char GEOMETRIC_TYPE_SEGMENT
Object type identifier for TSegment2D or TSegment3D.
const TPoint3D & operator[](size_t i) const
Access to points using operator[0-1].
bool operator==(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:279
TPoint3D operator/(double d) const
T wrapTo2Pi(T a)
Modifies the given angle to translate it into the [0,2pi[ range.
Definition: wrap2pi.h:40
void generate2DObject(TPolygon2D &p) const
Projects into a 2D space, discarding the z.
3D Plane, represented by its equation
void getDirectorVector(double(&vector)[3]) const
Get director vector.
class BASE_IMPEXP TPolygon3D
TPoint2D operator*(double d) const
TPolygon2D()
Default constructor.
void operator=(const TPolygon3D &p)
Assigns a polygon to this object.
void getCenter(TPoint3D &p) const
Segment&#39;s central point.
TPoint3D operator*(double d) const
unsigned char type
Object type identifier.
TPose3DQuat()
Default fast constructor.
TPoint2D point2
Destiny point.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
bool isSegment() const
Checks whether content is a segment.
TObject2D(const TObject2D &obj)
Constructor from another TObject2D.
The base template class for 2D & 3D points and poses.
Definition: CPoseOrPoint.h:107
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
TPoint2D & operator[](size_t i)
Access to points using operator[0-1].
void getAsVector(std::vector< double > &v) const
Gets the pose as a vector of doubles.
TObject2D & operator=(const TObject2D &obj)
Assign another TObject2D.
double qz
Unit quaternion part, qr,qx,qy,qz.
bool isPlane() const
Checks whether content is a plane.
A class used to store a 2D point.
Definition: CPoint2D.h:36
A class used to store a 3D point.
Definition: CPoint3D.h:32
void operator=(const TPlane &p)
Assigns a plane to this object.
TObject2D(const TSegment2D &s)
Implicit constructor from segment.
TPolygon2D(size_t N)
Constructor for a given number of vertices, intializing them as garbage.
bool getPoint(TPoint3D &p) const
Gets the content as a point, returning false if the type is not adequate.
double pitch
Pitch coordinate (rotation angle over Y axis).
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
void getAsPose3DForcingOrigin(const TPoint3D &newOrigin, mrpt::poses::CPose3D &pose) const
Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates...
TPoint2D point1
Origin point.
double norm() const
Point norm.
TPoint2D()
Default fast constructor.
bool operator<(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:283
double y
X,Y coordinates.
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
TPoint2D operator+(const TPoint2D &p) const
TObject3D(const TLine3D &r)
Constructor from line.
bool isLine() const
Checks whether content is a line.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const double & operator[](size_t i) const
Coordinate access using operator[].
TPlane()
Fast default constructor.
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[0.02 1.04 -0.8]" )
std::string asString() const
TPose3DQuat(double _x, double _y, double _z, double _qr, double _qx, double _qy, double _qz)
Constructor from coordinates.
void operator=(const TPolygon2D &p)
Assign a polygon to this object.
TPoint3D operator-(const TPoint3D &p) const
Points substraction.
A class used to store a 2D pose.
Definition: CPose2D.h:36
TPoint2D operator-(const TPoint2D &p) const
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
struct BASE_IMPEXP TLine3D
double sqrDistanceTo(const TPoint3D &p) const
Point-to-point distance, squared.
void getUnitaryNormalVector(double(&vec)[3])
Unitarize, then get normal vector.
double norm() const
Pose&#39;s spatial coordinates norm.
TSegment2D()
Fast default constructor.
unsigned char type
Object type identifier.
~TObject2D()
Object destruction.
const TPoint2D & operator[](size_t i) const
Access to points using operator[0-1].
TPoint3D & operator[](size_t i)
Access to points using operator[0-1].
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Lightweight 2D pose.
bool getLine(TLine3D &r) const
Gets the content as a line, returning false if the type is not adequate.
void getAsVector(std::vector< double > &v) const
Transformation into vector.
std::string asString() const
void generate2DObject(TSegment2D &s) const
Projection into 2D space, discarding the z.
void generate2DObject(TObject2D &obj) const
Projects into 2D space.
const double & operator[](size_t i) const
Coordinate access using operator[].
bool isLine() const
Checks whether content is a line.
bool isSegment() const
Checks whether content is a segment.
TObject2D(const TPolygon2D &p)
Implicit constructor from polygon.
TSegment3D(const TPoint3D &p1, const TPoint3D &p2)
Constructor from both points.
void getAsPose3D(mrpt::poses::CPose3D &outPose) const
Gets a pose whose XY plane corresponds to this plane.
TObject3D(const TObject3D &obj)
Constructs from another object.
TPose2D()
Default fast constructor.
unsigned char getType() const
Gets object type.
const unsigned char GEOMETRIC_TYPE_UNDEFINED
Object type identifier for empty TObject2D or TObject3D.
TPoint2D & operator-=(const TPoint2D &p)
void operator=(const TLine3D &l)
Assigns a line to this object.
double & operator[](size_t i)
Coordinate access using operator[].
Lightweight 3D point.
TPolygon3D(size_t N)
Constructor for a given size.
void asString(std::string &s) const
Returns a human-readable textual representation of the object as "[x y z qr qx qy qz]"...
void operator=(const TLine2D &l)
Assign a line to this object.
bool isPolygon() const
Checks whether content is a polygon.
void destroy()
Destroys the object, releasing the pointer to the content (if any).
TObject3D(const TSegment3D &s)
Constructor from segment.
std::string asString() const
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.
TPoint3D operator-(const TPoint3D &p1)
Unary minus operator for 3D points.
void getAsVector(std::vector< double > &v) const
Gets the pose as a vector of doubles.
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)
double & operator[](size_t i)
Coordinate access using operator[].
void getAsVector(VECTORLIKE &v) const
Transformation into vector.
2D polygon, inheriting from std::vector<TPoint2D>.
TPoint3D & operator-=(const TPoint3D &p)
Difference between points.
3D polygon, inheriting from std::vector<TPoint3D>
Union type storing pointers to every allowed type.
void getAsVector(std::vector< double > &v) const
Transformation into vector.
double & operator[](size_t i)
Coordinate access using operator[].
std::vector< T1 > operator*(const std::vector< T1 > &a, const std::vector< T2 > &b)
a*b (element-wise multiplication)
Definition: ops_vectors.h:59
TPoint3D(const TPoint3Df &p)
Explicit constructor from coordinates.
TPoint2D(const mrpt::utils::TPixelCoordf &p)
Implicit transformation constructor from TPixelCoordf.
bool isPoint() const
Checks whether content is a point.
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
const double & operator[](size_t i) const
Coordinate access using operator[].
TPoint2D & operator/=(double d)
3D line, represented by a base point and a director vector.
TPolygon2D(const std::vector< TPoint2D > &v)
Implicit constructor from a vector of 2D points.
2D line without bounds, represented by its equation .



Page generated by Doxygen 1.8.12 for MRPT 1.3.2 SVN: at Mon Oct 3 19:22:36 UTC 2016