Main MRPT website > C++ reference
MRPT logo
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-2014, 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>
14 #include <mrpt/utils/TPixelCoord.h>
15 
16 #include <mrpt/math/math_frwds.h> // Fordward declarations
17 
18 
19 
20 namespace mrpt {
21  namespace poses {
22  template <class DERIVEDCLASS> class CPoseOrPoint;
23  class CPoint2D;
24  class CPoint3D;
25  class CPose2D;
26  class CPose3D;
27  }
28  namespace utils { class CStream; }
29 }
30 
31 namespace mrpt {
32 namespace math {
33  using namespace mrpt::utils; // For "square"
34 
35  struct TPoint2D;
36  struct TPose2D;
37  struct TPoint3D;
38  struct TPose3D;
39  struct TPose3DQuat;
40 
41  /** \addtogroup geometry_grp
42  * @{ */
43 
44 
45  //Pragma defined to ensure no structure packing
46 #pragma pack(push,1)
47  //Set of typedefs for lightweight geometric items.
48  /**
49  * Lightweight 2D point. Allows coordinate access using [] operator.
50  * \sa mrpt::poses::CPoint2D
51  */
52 //#define TOBJECTS_USE_UNIONS
54  /**
55  * X coordinate.
56  */
57  double x;
58  /**
59  * Y coordinate.
60  */
61  double y;
62  /**
63  * Constructor from TPose2D, discarding phi.
64  * \sa TPose2D
65  */
66  explicit TPoint2D(const TPose2D &p);
67  /**
68  * Constructor from TPoint3D, discarding z.
69  * \sa TPoint3D
70  */
71  explicit TPoint2D(const TPoint3D &p);
72  /**
73  * Constructor from TPose3D, discarding z and the angular coordinates.
74  * \sa TPose3D
75  */
76  explicit TPoint2D(const TPose3D &p);
77  /**
78  * Constructor from CPoseOrPoint, perhaps losing 3D information
79  * \sa CPoseOrPoint,CPoint3D,CPose2D,CPose3D
80  */
81  template <class DERIVEDCLASS>
82  explicit TPoint2D(const mrpt::poses::CPoseOrPoint<DERIVEDCLASS> &p) :x(p.x()),y(p.y()) {}
83 
84  /** Implicit transformation constructor from TPixelCoordf */
85  inline TPoint2D(const mrpt::utils::TPixelCoordf &p) :x(p.x),y(p.y) {}
86 
87  /** Implicit constructor from CPoint2D */
89  /**
90  * Constructor from coordinates.
91  */
92  inline TPoint2D(double xx,double yy):x(xx),y(yy) {}
93  /**
94  * Default fast constructor. Initializes to garbage.
95  */
96  inline TPoint2D() {}
97  /**
98  * Unsafe coordinate access using operator[]. Intended for loops.
99  */
100  inline double &operator[](size_t i) {
101  return (&x)[i];
102  }
103  /**
104  * Unsafe coordinate access using operator[]. Intended for loops.
105  */
106  inline const double &operator[](size_t i) const {
107  return (&x)[i];
108  }
109  /**
110  * Transformation into vector.
111  */
112  inline void getAsVector(vector_double &v) const {
113  v.resize(2);
114  v[0]=x; v[1]=y;
115  }
116 
117  bool operator<(const TPoint2D &p) const;
118 
119  inline TPoint2D &operator+=(const TPoint2D &p) {
120  x+=p.x;
121  y+=p.y;
122  return *this;
123  }
124 
125  inline TPoint2D &operator-=(const TPoint2D &p) {
126  x-=p.x;
127  y-=p.y;
128  return *this;
129  }
130 
131  inline TPoint2D &operator*=(double d) {
132  x*=d;
133  y*=d;
134  return *this;
135  }
136 
137  inline TPoint2D &operator/=(double d) {
138  x/=d;
139  y/=d;
140  return *this;
141  }
142 
143  inline TPoint2D operator+(const TPoint2D &p) const {
144  TPoint2D r(*this);
145  return r+=p;
146  }
147 
148  inline TPoint2D operator-(const TPoint2D &p) const {
149  TPoint2D r(*this);
150  return r-=p;
151  }
152 
153  inline TPoint2D operator*(double d) const {
154  TPoint2D r(*this);
155  return r*=d;
156  }
157 
158  inline TPoint2D operator/(double d) const {
159  TPoint2D r(*this);
160  return r/=d;
161  }
162  /** Returns a human-readable textual representation of the object (eg: "[0.02 1.04]" )
163  * \sa fromString
164  */
165  void asString(std::string &s) const { s = mrpt::format("[%f %f]",x,y); }
166  inline std::string asString() const { std::string s; asString(s); return s; }
167 
168  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04]" )
169  * \sa asString
170  * \exception std::exception On invalid format
171  */
172  void fromString(const std::string &s);
173  static size_t size() { return 2; }
174 
175  /** Point norm. */
176  inline double norm() const { return sqrt(square(x)+square(y)); }
177  };
178 
179  /**
180  * Lightweight 2D pose. Allows coordinate access using [] operator.
181  * \sa mrpt::poses::CPose2D
182  */
184  /**
185  * X coordinate.
186  */
187  double x;
188  /**
189  * Y coordinate.
190  */
191  double y;
192  /**
193  * Phi coordinate.
194  */
195  double phi;
196  /**
197  * Implicit constructor from TPoint2D. Zeroes the phi coordinate.
198  * \sa TPoint2D
199  */
200  TPose2D(const TPoint2D &p);
201  /**
202  * Constructor from TPoint3D, losing information. Zeroes the phi coordinate.
203  * \sa TPoint3D
204  */
205  explicit TPose2D(const TPoint3D &p);
206  /**
207  * Constructor from TPose3D, losing information. The phi corresponds to the original pose's yaw.
208  * \sa TPose3D
209  */
210  explicit TPose2D(const TPose3D &p);
211  /**
212  * Implicit constructor from heavyweight type.
213  * \sa mrpt::poses::CPose2D
214  */
215  TPose2D(const mrpt::poses::CPose2D &p);
216  /**
217  * Constructor from coordinates.
218  */
219  inline TPose2D(double xx,double yy,double pphi):x(xx),y(yy),phi(pphi) {}
220  /**
221  * Default fast constructor. Initializes to garbage.
222  */
223  inline TPose2D() {}
224  /**
225  * Unsafe coordinate access using operator[]. Intended for loops.
226  */
227  inline double &operator[](size_t i) {
228  return (&x)[i];
229  }
230  /**
231  * Unsafe coordinate access using operator[]. Intended for loops.
232  */
233  inline const double &operator[](size_t i) const {
234  return (&x)[i];
235  }
236  /**
237  * Transformation into vector.
238  */
239  inline void getAsVector(vector_double &v) const {
240  v.resize(3);
241  v[0]=x; v[1]=y; v[2]=phi;
242  }
243  /** Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees)
244  * \sa fromString
245  */
246  void asString(std::string &s) const { s = mrpt::format("[%f %f %f]",x,y,RAD2DEG(phi)); }
247  inline std::string asString() const { std::string s; asString(s); return s; }
248 
249  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -45.0]" )
250  * \sa asString
251  * \exception std::exception On invalid format
252  */
253  void fromString(const std::string &s);
254  static size_t size() { return 3; }
255  };
256 
257 #pragma pack(pop) // NOTE: Don't force TPoint3Df to be mem aligned (may break CPU mem access alignment in ARM)
258 
259  /** Lightweight 3D point (float version).
260  * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3D
261  */
263  {
264  float x;
265  float y;
266  float z;
267 
268  inline TPoint3Df() { }
269  inline TPoint3Df(const float xx,const float yy,const float zz) : x(xx), y(yy),z(zz) { }
270  inline TPoint3Df & operator +=(const TPoint3Df &p) { x+=p.x; y+=p.y; z+=p.z; return *this; }
271  inline TPoint3Df operator *(const float s) { return TPoint3Df(x*s,y*s,z*s); }
272  };
273 
274 #pragma pack(push,1) //Pragma defined to ensure no structure packing
275  /**
276  * Lightweight 3D point. Allows coordinate access using [] operator.
277  * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3Df
278  */
280  double x; //!< X coordinate
281  double y; //!< Y coordinate
282  double z; //!< Z coordinate
283 
284  /** Constructor from coordinates. */
285  inline TPoint3D(double xx,double yy,double zz):x(xx),y(yy),z(zz) {}
286  /** Default fast constructor. Initializes to garbage. */
287  inline TPoint3D() {}
288  /** Explicit constructor from coordinates. */
289  explicit inline TPoint3D(const TPoint3Df &p):x(p.x),y(p.y),z(p.z) {}
290 
291  /** Implicit constructor from TPoint2D. Zeroes the z.
292  * \sa TPoint2D
293  */
294  TPoint3D(const TPoint2D &p);
295  /**
296  * Constructor from TPose2D, losing information. Zeroes the z.
297  * \sa TPose2D
298  */
299  explicit TPoint3D(const TPose2D &p);
300  /**
301  * Constructor from TPose3D, losing information.
302  * \sa TPose3D
303  */
304  explicit TPoint3D(const TPose3D &p);
305  /**
306  * Implicit constructor from heavyweight type.
307  * \sa mrpt::poses::CPoint3D
308  */
310  /**
311  * Constructor from heavyweight 3D pose.
312  * \sa mrpt::poses::CPose3D.
313  */
314  explicit TPoint3D(const mrpt::poses::CPose3D &p);
315  /**
316  * Unsafe coordinate access using operator[]. Intended for loops.
317  */
318  inline double &operator[](size_t i) {
319  return (&x)[i];
320  }
321  /**
322  * Unsafe coordinate access using operator[]. Intended for loops.
323  */
324  inline const double &operator[](size_t i) const {
325  return (&x)[i];
326  }
327  /**
328  * Point-to-point distance.
329  */
330  inline double distanceTo(const TPoint3D &p) const {
331  return sqrt(square(p.x-x)+square(p.y-y)+square(p.z-z));
332  }
333  /**
334  * Point-to-point distance, squared.
335  */
336  inline double sqrDistanceTo(const TPoint3D &p) const {
337  return square(p.x-x)+square(p.y-y)+square(p.z-z);
338  }
339  /**
340  * Point norm.
341  */
342  inline double norm() const {
343  return sqrt(square(x)+square(y)+square(z));
344  }
345  /**
346  * Point scale.
347  */
348  inline TPoint3D &operator*=(const double f) {
349  x*=f;y*=f;z*=f;
350  return *this;
351  }
352  /**
353  * Transformation into vector.
354  */
355  void getAsVector(vector_double &v) const {
356  v.resize(3);
357  v[0]=x; v[1]=y; v[2]=z;
358  }
359  /**
360  * Translation.
361  */
362  inline TPoint3D &operator+=(const TPoint3D &p) {
363  x+=p.x;
364  y+=p.y;
365  z+=p.z;
366  return *this;
367  }
368  /**
369  * Difference between points.
370  */
371  inline TPoint3D &operator-=(const TPoint3D &p) {
372  x-=p.x;
373  y-=p.y;
374  z-=p.z;
375  return *this;
376  }
377  /**
378  * Points addition.
379  */
380  inline TPoint3D operator+(const TPoint3D &p) const {
381  return TPoint3D(x+p.x,y+p.y,z+p.z);
382  }
383  /**
384  * Points substraction.
385  */
386  inline TPoint3D operator-(const TPoint3D &p) const {
387  return TPoint3D(x-p.x,y-p.y,z-p.z);
388  }
389 
390  inline TPoint3D operator*(double d) const {
391  return TPoint3D(x*d,y*d,z*d);
392  }
393 
394  inline TPoint3D operator/(double d) const {
395  return TPoint3D(x/d,y/d,z/d);
396  }
397 
398  bool operator<(const TPoint3D &p) const;
399 
400  /** Returns a human-readable textual representation of the object (eg: "[0.02 1.04 -0.8]" )
401  * \sa fromString
402  */
403  void asString(std::string &s) const { s = mrpt::format("[%f %f %f]",x,y,z); }
404  inline std::string asString() const { std::string s; asString(s); return s; }
405 
406  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" )
407  * \sa asString
408  * \exception std::exception On invalid format
409  */
410  void fromString(const std::string &s);
411  static size_t size() { return 3; }
412  };
413 
414  /**
415  * Lightweight 3D pose (three spatial coordinates, plus three angular coordinates). Allows coordinate access using [] operator.
416  * \sa mrpt::poses::CPose3D
417  */
419  /**
420  * X coordinate.
421  */
422  double x;
423  /**
424  * Y coordinate.
425  */
426  double y;
427  /**
428  * Z coordinate.
429  */
430  double z;
431  /**
432  * Yaw coordinate (rotation angle over Z axis).
433  */
434  double yaw;
435  /**
436  * Pitch coordinate (rotation angle over Y axis).
437  */
438  double pitch;
439  /**
440  * Roll coordinate (rotation angle over X coordinate).
441  */
442  double roll;
443  /**
444  * Implicit constructor from TPoint2D. Zeroes all the unprovided information.
445  * \sa TPoint2D
446  */
447  TPose3D(const TPoint2D &p);
448  /**
449  * Implicit constructor from TPose2D. Gets the yaw from the 2D pose's phi, zeroing all the unprovided information.
450  * \sa TPose2D
451  */
452  TPose3D(const TPose2D &p);
453  /**
454  * Implicit constructor from TPoint3D. Zeroes angular information.
455  * \sa TPoint3D
456  */
457  TPose3D(const TPoint3D &p);
458  /**
459  * Implicit constructor from heavyweight type.
460  * \sa mrpt::poses::CPose3D
461  */
462  TPose3D(const mrpt::poses::CPose3D &p);
463  /**
464  * Constructor from coordinates.
465  */
466  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) {}
467  /**
468  * Default fast constructor. Initializes to garbage.
469  */
470  inline TPose3D() {}
471  /**
472  * Unsafe coordinate access using operator[]. Intended for loops.
473  */
474  inline double &operator[](size_t i) {
475  return (&x)[i];
476  }
477  /**
478  * Unsafe coordinate access using operator[]. Intended for loops.
479  */
480  inline const double &operator[](size_t i) const {
481  return (&x)[i];
482  }
483  /**
484  * Pose's spatial coordinates norm.
485  */
486  double norm() const {
487  return sqrt(square(x)+square(y)+square(z));
488  }
489  /**
490  * Gets the pose as a vector of doubles.
491  */
492  void getAsVector(vector_double &v) const {
493  v.resize(6);
494  v[0]=x; v[1]=y; v[2]=z; v[3]=yaw; v[4]=pitch; v[5]=roll;
495  }
496  /** Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]", angles in degrees.)
497  * \sa fromString
498  */
499  void asString(std::string &s) const { s = mrpt::format("[%f %f %f %f %f %f]",x,y,z,RAD2DEG(yaw),RAD2DEG(pitch),RAD2DEG(roll)); }
500  inline std::string asString() const { std::string s; asString(s); return s; }
501 
502  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" )
503  * \sa asString
504  * \exception std::exception On invalid format
505  */
506  void fromString(const std::string &s);
507  static size_t size() { return 6; }
508  };
509 
510  /** Lightweight 3D pose (three spatial coordinates, plus a quaternion ). Allows coordinate access using [] operator.
511  * \sa mrpt::poses::CPose3DQuat
512  */
514  double x; //!< Translation in x
515  double y; //!< Translation in y
516  double z; //!< Translation in z
517  double qr; //!< Quaternion part, r
518  double qx; //!< Quaternion part, x
519  double qy; //!< Quaternion part, y
520  double qz; //!< Quaternion part, z
521 
522  /** Constructor from coordinates. */
523  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) { }
524  /** Default fast constructor. Initializes to garbage. */
525  inline TPose3DQuat() {}
526  /** Constructor from a CPose3DQuat */
528 
529  /** Unsafe coordinate access using operator[]. Intended for loops. */
530  inline double &operator[](size_t i) {
531  return (&x)[i];
532  }
533  /** Unsafe coordinate access using operator[]. Intended for loops. */
534  inline const double &operator[](size_t i) const {
535  return (&x)[i];
536  }
537  /** Pose's spatial coordinates norm. */
538  double norm() const {
539  return sqrt(square(x)+square(y)+square(z));
540  }
541  /** Gets the pose as a vector of doubles. */
542  void getAsVector(vector_double &v) const {
543  v.resize(7);
544  for (size_t i=0;i<7;i++) v[i]=(*this)[i];
545  }
546  /** Returns a human-readable textual representation of the object as "[x y z qr qx qy qz]"
547  * \sa fromString
548  */
549  void asString(std::string &s) const { s = mrpt::format("[%f %f %f %f %f %f %f]",x,y,z,qr,qx,qy,qz); }
550  inline std::string asString() const { std::string s; asString(s); return s; }
551 
552  /** 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]" )
553  * \sa asString
554  * \exception std::exception On invalid format
555  */
556  void fromString(const std::string &s);
557  static size_t size() { return 7; }
558  };
559 #pragma pack(pop)
560 
561  // Text streaming functions:
562  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPoint2D & p);
563  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPoint3D & p);
564  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose2D & p);
565  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose3D & p);
566  std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose3DQuat & p);
567 
568 
569  /**
570  * Unary minus operator for 3D points.
571  */
572  inline TPoint3D operator-(const TPoint3D &p1) {
573  return TPoint3D(-p1.x,-p1.y,-p1.z);
574  }
575  /**
576  * Exact comparison between 2D points.
577  */
578  inline bool operator==(const TPoint2D &p1,const TPoint2D &p2) {
579  return (p1.x==p2.x)&&(p1.y==p2.y);
580  }
581  /**
582  * Exact comparison between 2D points.
583  */
584  inline bool operator!=(const TPoint2D &p1,const TPoint2D &p2) {
585  return (p1.x!=p2.x)||(p1.y!=p2.y);
586  }
587  /**
588  * Exact comparison between 3D points.
589  */
590  inline bool operator==(const TPoint3D &p1,const TPoint3D &p2) {
591  return (p1.x==p2.x)&&(p1.y==p2.y)&&(p1.z==p2.z);
592  }
593  /**
594  * Exact comparison between 3D points.
595  */
596  inline bool operator!=(const TPoint3D &p1,const TPoint3D &p2) {
597  return (p1.x!=p2.x)||(p1.y!=p2.y)||(p1.z!=p2.z);
598  }
599  /**
600  * Exact comparison between 2D poses, taking possible cycles into account.
601  */
602  inline bool operator==(const TPose2D &p1,const TPose2D &p2) {
603  return (p1.x==p2.x)&&(p1.y==p2.y)&&(mrpt::math::wrapTo2Pi(p1.phi)==mrpt::math::wrapTo2Pi(p2.phi));
604  }
605  /**
606  * Exact comparison between 2D poses, taking possible cycles into account.
607  */
608  inline bool operator!=(const TPose2D &p1,const TPose2D &p2) {
609  return (p1.x!=p2.x)||(p1.y!=p2.y)||(mrpt::math::wrapTo2Pi(p1.phi)!=mrpt::math::wrapTo2Pi(p2.phi));
610  }
611  /**
612  * Exact comparison between 3D poses, taking possible cycles into account.
613  */
614  inline bool operator==(const TPose3D &p1,const TPose3D &p2) {
616  }
617  /**
618  * Exact comparison between 3D poses, taking possible cycles into account.
619  */
620  inline bool operator!=(const TPose3D &p1,const TPose3D &p2) {
622  }
623  //Forward declarations
628 
629  //Pragma defined to ensure no structure packing
630 #pragma pack(push,1)
631  /**
632  * 2D segment, consisting of two points.
633  * \sa TSegment3D,TLine2D,TPolygon2D,TPoint2D
634  */
636  public:
637  /**
638  * Origin point.
639  */
641  /**
642  * Destiny point.
643  */
645  /**
646  * Segment length.
647  */
648  double length() const;
649  /**
650  * Distance to point.
651  */
652  double distance(const TPoint2D &point) const;
653  /**
654  * Distance with sign to point (sign indicates which side the point is).
655  */
656  double signedDistance(const TPoint2D &point) const;
657  /**
658  * Check whether a point is inside a segment.
659  */
660  bool contains(const TPoint2D &point) const;
661  /**
662  * Unsafe point access using [] operator, intended for loops.
663  */
664  inline TPoint2D &operator[](size_t i) {
665  return (&point1)[i];
666  }
667  /**
668  * Unsafe point access using [] operator, intended for loops.
669  */
670  inline const TPoint2D &operator[](size_t i) const {
671  return (&point1)[i];
672  }
673  /**
674  * Project into 3D space, setting the z to 0.
675  */
676  void generate3DObject(TSegment3D &s) const;
677  /**
678  * Segment's central point.
679  */
680  inline void getCenter(TPoint2D &p) const {
681  p.x=(point1.x+point2.x)/2;
682  p.y=(point1.y+point2.y)/2;
683  }
684  /**
685  * Constructor from both points.
686  */
687  TSegment2D(const TPoint2D &p1,const TPoint2D &p2):point1(p1),point2(p2) {}
688  /**
689  * Fast default constructor. Initializes to garbage.
690  */
692  /**
693  * Explicit constructor from 3D object, discarding the z.
694  */
695  explicit TSegment2D(const TSegment3D &s);
696 
697  bool operator<(const TSegment2D &s) const;
698  };
699  /**
700  * 3D segment, consisting of two points.
701  * \sa TSegment2D,TLine3D,TPlane,TPolygon3D,TPoint3D
702  */
704  public:
705  /**
706  * Origin point.
707  */
709  /**
710  * Destiny point.
711  */
713  /**
714  * Segment length.
715  */
716  double length() const;
717  /**
718  * Distance to point.
719  */
720  double distance(const TPoint3D &point) const;
721  /**
722  * Distance to another segment.
723  */
724  double distance(const TSegment3D &segment) const;
725  /**
726  * Check whether a point is inside the segment.
727  */
728  bool contains(const TPoint3D &point) const;
729  /**
730  * Unsafe point access using [] operator, intended for loops.
731  */
732  inline TPoint3D &operator[](size_t i) {
733  return (&point1)[i];
734  }
735  /**
736  * Unsafe point access using [] operator, intended for loops.
737  */
738  inline const TPoint3D &operator[](size_t i) const {
739  return (&point1)[i];
740  }
741  /**
742  * Projection into 2D space, discarding the z.
743  */
744  inline void generate2DObject(TSegment2D &s) const {
745  s=TSegment2D(*this);
746  }
747  /**
748  * Segment's central point.
749  */
750  inline void getCenter(TPoint3D &p) const {
751  p.x=(point1.x+point2.x)/2;
752  p.y=(point1.y+point2.y)/2;
753  p.z=(point1.z+point2.z)/2;
754  }
755  /**
756  * Constructor from both points.
757  */
758  TSegment3D(const TPoint3D &p1,const TPoint3D &p2):point1(p1),point2(p2) {}
759  /**
760  * Fast default constructor. Initializes to garbage.
761  */
763  /**
764  * Constructor from 2D object. Sets the z to zero.
765  */
766  TSegment3D(const TSegment2D &s):point1(s.point1),point2(s.point2) {}
767 
768  bool operator<(const TSegment3D &s) const;
769  };
770 #pragma pack(pop)
771 
772  inline bool operator==(const TSegment2D &s1,const TSegment2D &s2) {
773  return (s1.point1==s2.point1)&&(s1.point2==s2.point2);
774  }
775 
776  inline bool operator!=(const TSegment2D &s1,const TSegment2D &s2) {
777  return (s1.point1!=s1.point1)||(s1.point2!=s2.point2);
778  }
779 
780  inline bool operator==(const TSegment3D &s1,const TSegment3D &s2) {
781  return (s1.point1==s2.point1)&&(s1.point2==s2.point2);
782  }
783 
784  inline bool operator!=(const TSegment3D &s1,const TSegment3D &s2) {
785  return (s1.point1!=s1.point1)||(s1.point2!=s2.point2);
786  }
787 
788  /**
789  * 2D line without bounds, represented by its equation \f$Ax+By+C=0\f$.
790  * \sa TLine3D,TSegment2D,TPolygon2D,TPoint2D
791  */
793  public:
794  /**
795  * Line coefficients, stored as an array: \f$\left[A,B,C\right]\f$.
796  */
797  double coefs[3];
798  /**
799  * Evaluate point in the line's equation.
800  */
801  double evaluatePoint(const TPoint2D &point) const;
802  /**
803  * Check whether a point is inside the line.
804  */
805  bool contains(const TPoint2D &point) const;
806  /**
807  * Distance from a given point.
808  */
809  double distance(const TPoint2D &point) const;
810  /**
811  * Distance with sign from a given point (sign indicates side).
812  */
813  double signedDistance(const TPoint2D &point) const;
814  /**
815  * Get line's normal vector.
816  */
817  void getNormalVector(double (&vector)[2]) const;
818  /**
819  * Unitarize line's normal vector.
820  */
821  void unitarize();
822  /**
823  * Get line's normal vector after unitarizing line.
824  */
825  inline void getUnitaryNormalVector(double (&vector)[2]) {
826  unitarize();
827  getNormalVector(vector);
828  }
829  /**
830  * Get line's director vector.
831  */
832  void getDirectorVector(double (&vector)[2]) const;
833  /**
834  * Unitarize line and then get director vector.
835  */
836  inline void getUnitaryDirectorVector(double (&vector)[2]) {
837  unitarize();
838  getDirectorVector(vector);
839  }
840  /**
841  * Project into 3D space, setting the z to 0.
842  */
843  void generate3DObject(TLine3D &l) const;
844  /**
845  * Get a pose2D whose X axis corresponds to the line.
846  * \sa mrpt::poses::CPose2D.
847  */
848  void getAsPose2D(mrpt::poses::CPose2D &outPose) const;
849  /**
850  * Get a pose2D whose X axis corresponds to the line, forcing the base point to one given.
851  * \throw logic_error if the point is not inside the line.
852  * \sa mrpt::poses::CPose2D.
853  */
854  void getAsPose2DForcingOrigin(const TPoint2D &origin,mrpt::poses::CPose2D &outPose) const;
855  /**
856  * Constructor from two points, through which the line will pass.
857  * \throw logic_error if both points are the same
858  */
859  TLine2D(const TPoint2D &p1,const TPoint2D &p2) throw(std::logic_error);
860  /**
861  * Constructor from a segment.
862  */
863  explicit TLine2D(const TSegment2D &s);
864  /**
865  * Fast default constructor. Initializes to garbage.
866  */
867  TLine2D() {}
868  /**
869  * Constructor from line's coefficients.
870  */
871  inline TLine2D(double A,double B,double C) {
872  coefs[0]=A;
873  coefs[1]=B;
874  coefs[2]=C;
875  }
876  /**
877  * Construction from 3D object, discarding the Z.
878  * \throw std::logic_error if the line is normal to the XY plane.
879  */
880  explicit TLine2D(const TLine3D &l);
881  };
882 
883  /**
884  * 3D line, represented by a base point and a director vector.
885  * \sa TLine2D,TSegment3D,TPlane,TPolygon3D,TPoint3D
886  */
888  public:
889  /**
890  * Base point.
891  */
893  /**
894  * Director vector.
895  */
896  double director[3];
897  /**
898  * Check whether a point is inside the line.
899  */
900  bool contains(const TPoint3D &point) const;
901  /**
902  * Distance between the line and a point.
903  */
904  double distance(const TPoint3D &point) const;
905  /**
906  * Unitarize director vector.
907  */
908  void unitarize();
909  /**
910  * Get director vector.
911  */
912  inline void getDirectorVector(double (&vector)[3]) const {
913  for (size_t i=0;i<3;i++) vector[i]=director[i];
914  }
915  /**
916  * Unitarize and then get director vector.
917  */
918  inline void getUnitaryDirectorVector(double (&vector)[3]) {
919  unitarize();
920  getDirectorVector(vector);
921  }
922  /**
923  * Project into 2D space, discarding the Z coordinate.
924  * \throw std::logic_error if the line's director vector is orthogonal to the XY plane.
925  */
926  inline void generate2DObject(TLine2D &l) const {
927  l=TLine2D(*this);
928  }
929  /**
930  * Constructor from two points, through which the line will pass.
931  * \throw std::logic_error if both points are the same.
932  */
933  TLine3D(const TPoint3D &p1,const TPoint3D &p2) throw(std::logic_error);
934  /**
935  * Constructor from 3D segment.
936  */
937  explicit TLine3D(const TSegment3D &s);
938  /**
939  * Fast default constructor. Initializes to garbage.
940  */
941  TLine3D() {}
942  /**
943  * Implicit constructor from 2D object. Zeroes the z.
944  */
945  TLine3D(const TLine2D &l);
946  };
947 
948  /**
949  * 3D Plane, represented by its equation \f$Ax+By+Cz+D=0\f$
950  * \sa TSegment3D,TLine3D,TPolygon3D,TPoint3D
951  */
953  public:
954  /**
955  * Plane coefficients, stored as an array: \f$\left[A,B,C,D\right]\f$
956  */
957  double coefs[4];
958  /**
959  * Evaluate a point in the plane's equation.
960  */
961  double evaluatePoint(const TPoint3D &point) const;
962  /**
963  * Check whether a point is contained into the plane.
964  */
965  bool contains(const TPoint3D &point) const;
966  /**
967  * Check whether a segment is fully contained into the plane.
968  */
969  inline bool contains(const TSegment3D &segment) const {
970  return contains(segment.point1)&&contains(segment.point2);
971  }
972  /**
973  * Check whether a line is fully contained into the plane.
974  */
975  bool contains(const TLine3D &line) const;
976  /**
977  * Distance to 3D point.
978  */
979  double distance(const TPoint3D &point) const;
980  /**
981  * Distance to 3D line. Will be zero if the line is not parallel to the plane.
982  */
983  double distance(const TLine3D &line) const;
984  /**
985  * Get plane's normal vector.
986  */
987  void getNormalVector(double (&vec)[3]) const;
988  /**
989  * Unitarize normal vector.
990  */
991  void unitarize();
992  /**
993  * Unitarize, then get normal vector.
994  */
995  inline void getUnitaryNormalVector(double (&vec)[3]) {
996  unitarize();
997  getNormalVector(vec);
998  }
999  /**
1000  * Gets a pose whose XY plane corresponds to this plane.
1001  */
1002  void getAsPose3D(mrpt::poses::CPose3D &outPose);
1003  /**
1004  * Gets a pose whose XY plane corresponds to this plane.
1005  */
1006  inline void getAsPose3D(mrpt::poses::CPose3D &outPose) const {
1007  TPlane p=*this;
1008  p.getAsPose3D(outPose);
1009  }
1010  /**
1011  * Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates.
1012  * \throw std::logic_error if the point is not inside the plane.
1013  */
1014  void getAsPose3DForcingOrigin(const TPoint3D &newOrigin,mrpt::poses::CPose3D &pose);
1015  /**
1016  * Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates.
1017  * \throw std::logic_error if the point is not inside the plane.
1018  */
1019  inline void getAsPose3DForcingOrigin(const TPoint3D &newOrigin,mrpt::poses::CPose3D &pose) const {
1020  TPlane p=*this;
1021  p.getAsPose3DForcingOrigin(newOrigin,pose);
1022  }
1023  /**
1024  * Gets a plane which contains these three points.
1025  * \throw std::logic_error if the points are linearly dependants.
1026  */
1027  TPlane(const TPoint3D &p1,const TPoint3D &p2,const TPoint3D &p3) throw(std::logic_error);
1028  /**
1029  * Gets a plane which contains this point and this line.
1030  * \throw std::logic_error if the point is inside the line.
1031  */
1032  TPlane(const TPoint3D &p1,const TLine3D &r2) throw(std::logic_error);
1033  /**
1034  * Gets a plane which contains the two lines.
1035  * \throw std::logic_error if the lines do not cross.
1036  */
1037  TPlane(const TLine3D &r1,const TLine3D &r2) throw(std::logic_error);
1038  /**
1039  * Fast default constructor. Initializes to garbage.
1040  */
1041  TPlane() {}
1042  /**
1043  * Constructor from plane coefficients.
1044  */
1045  inline TPlane(double A,double B,double C,double D) {
1046  coefs[0]=A;
1047  coefs[1]=B;
1048  coefs[2]=C;
1049  coefs[3]=D;
1050  }
1051  /**
1052  * Constructor from an array of coefficients.
1053  */
1054  inline TPlane(const double (&vec)[4]) {
1055  for (size_t i=0;i<4;i++) coefs[i]=vec[i];
1056  }
1057  };
1058 
1059  typedef TPlane TPlane3D;
1060 
1061  /**
1062  * 2D polygon, inheriting from std::vector<TPoint2D>.
1063  * \sa TPolygon3D,TSegment2D,TLine2D,TPoint2D, CPolygon
1064  */
1065  class BASE_IMPEXP TPolygon2D:public std::vector<TPoint2D> {
1066  public:
1067  /**
1068  * Distance to a point.
1069  */
1070  double distance(const TPoint2D &point) const;
1071  /**
1072  * Check whether a point is inside the polygon.
1073  */
1074  bool contains(const TPoint2D &point) const;
1075  /**
1076  * Gets as set of segments, instead of points.
1077  */
1078  void getAsSegmentList(std::vector<TSegment2D> &v) const;
1079  /**
1080  * Projects into 3D space, zeroing the z.
1081  */
1082  void generate3DObject(TPolygon3D &p) const;
1083  /**
1084  * Polygon's central point.
1085  */
1086  void getCenter(TPoint2D &p) const;
1087  /**
1088  * Checks whether is convex.
1089  */
1090  bool isConvex() const;
1091  /**
1092  * Erase repeated vertices.
1093  * \sa removeRedundantVertices
1094  */
1095  void removeRepeatedVertices();
1096  /**
1097  * Erase every redundant vertex from the polygon, saving space.
1098  * \sa removeRepeatedVertices
1099  */
1100  void removeRedundantVertices();
1101  /**
1102  * Gets plot data, ready to use on a 2D plot.
1103  * \sa mrpt::gui::CDisplayWindowPlots
1104  */
1105  void getPlotData(std::vector<double> &x,std::vector<double> &y) const;
1106  /**
1107  * Default constructor.
1108  */
1109  TPolygon2D():std::vector<TPoint2D>() {}
1110  /**
1111  * Constructor for a given number of vertices, intializing them as garbage.
1112  */
1113  explicit TPolygon2D(size_t N):std::vector<TPoint2D>(N) {}
1114  /**
1115  * Implicit constructor from a vector of 2D points.
1116  */
1117  TPolygon2D(const std::vector<TPoint2D> &v):std::vector<TPoint2D>(v) {}
1118  /**
1119  * Constructor from a 3D object.
1120  */
1121  explicit TPolygon2D(const TPolygon3D &p);
1122  /**
1123  * Static method to create a regular polygon, given its size and radius.
1124  * \throw std::logic_error if radius is near zero or the number of edges is less than three.
1125  */
1126  static void createRegularPolygon(size_t numEdges,double radius,TPolygon2D &poly);
1127  /**
1128  * Static method to create a regular polygon from its size and radius. The center will correspond to the given pose.
1129  * \throw std::logic_error if radius is near zero or the number of edges is less than three.
1130  */
1131  static inline void createRegularPolygon(size_t numEdges,double radius,TPolygon2D &poly,const mrpt::poses::CPose2D &pose);
1132  };
1133 
1134  /**
1135  * 3D polygon, inheriting from std::vector<TPoint3D>
1136  * \sa TPolygon2D,TSegment3D,TLine3D,TPlane,TPoint3D
1137  */
1138  class BASE_IMPEXP TPolygon3D:public std::vector<TPoint3D> {
1139  public:
1140  /**
1141  * Distance to point.
1142  */
1143  double distance(const TPoint3D &point) const;
1144  /**
1145  * Check whether a point is inside the polygon.
1146  */
1147  bool contains(const TPoint3D &point) const;
1148  /**
1149  * Gets as set of segments, instead of set of points.
1150  */
1151  void getAsSegmentList(std::vector<TSegment3D> &v) const;
1152  /**
1153  * Gets a plane which contains the polygon. Returns false if the polygon is skew and cannot be fit inside a plane.
1154  */
1155  bool getPlane(TPlane &p) const;
1156  /**
1157  * Gets the best fitting plane, disregarding whether the polygon actually fits inside or not.
1158  * \sa getBestFittingPlane
1159  */
1160  void getBestFittingPlane(TPlane &p) const;
1161  /**
1162  * Projects into a 2D space, discarding the z.
1163  * \get getPlane,isSkew
1164  */
1165  inline void generate2DObject(TPolygon2D &p) const {
1166  p=TPolygon2D(*this);
1167  }
1168  /**
1169  * Get polygon's central point.
1170  */
1171  void getCenter(TPoint3D &p) const;
1172  /**
1173  * Check whether the polygon is skew. Returns true if there doesn't exist a plane in which the polygon can fit.
1174  * \sa getBestFittingPlane
1175  */
1176  bool isSkew() const;
1177  /**
1178  * Remove polygon's repeated vertices.
1179  */
1180  void removeRepeatedVertices();
1181  /**
1182  * Erase every redundant vertex, thus saving space.
1183  */
1184  void removeRedundantVertices();
1185  /**
1186  * Default constructor. Creates a polygon with no vertices.
1187  */
1188  TPolygon3D():std::vector<TPoint3D>() {}
1189  /**
1190  * Constructor for a given size. Creates a polygon with a fixed number of vertices, which are initialized to garbage.
1191  */
1192  explicit TPolygon3D(size_t N):std::vector<TPoint3D>(N) {}
1193  /**
1194  * Implicit constructor from a 3D points vector.
1195  */
1196  TPolygon3D(const std::vector<TPoint3D> &v):std::vector<TPoint3D>(v) {}
1197  /**
1198  * Constructor from a 2D object. Zeroes the z.
1199  */
1200  TPolygon3D(const TPolygon2D &p);
1201  /**
1202  * Static method to create a regular polygon, given its size and radius.
1203  * \throw std::logic_error if number of edges is less than three, or radius is near zero.
1204  */
1205  static void createRegularPolygon(size_t numEdges,double radius,TPolygon3D &poly);
1206  /**
1207  * Static method to create a regular polygon, given its size and radius. The center will be located on the given pose.
1208  * \throw std::logic_error if number of edges is less than three, or radius is near zero.
1209  */
1210  static inline void createRegularPolygon(size_t numEdges,double radius,TPolygon3D &poly,const mrpt::poses::CPose3D &pose);
1211  };
1212 
1213  /**
1214  * Object type identifier for TPoint2D or TPoint3D.
1215  * \sa TObject2D,TObject3D
1216  */
1217  const unsigned char GEOMETRIC_TYPE_POINT=0;
1218  /**
1219  * Object type identifier for TSegment2D or TSegment3D.
1220  * \sa TObject2D,TObject3D
1221  */
1222  const unsigned char GEOMETRIC_TYPE_SEGMENT=1;
1223  /**
1224  * Object type identifier for TLine2D or TLine3D.
1225  * \sa TObject2D,TObject3D
1226  */
1227  const unsigned char GEOMETRIC_TYPE_LINE=2;
1228  /**
1229  * Object type identifier for TPolygon2D or TPolygon3D.
1230  * \sa TObject2D,TObject3D
1231  */
1232  const unsigned char GEOMETRIC_TYPE_POLYGON=3;
1233  /**
1234  * Object type identifier for TPlane.
1235  * \sa TObject3D
1236  */
1237  const unsigned char GEOMETRIC_TYPE_PLANE=4;
1238  /**
1239  * Object type identifier for empty TObject2D or TObject3D.
1240  * \sa TObject2D,TObject3D
1241  */
1242  const unsigned char GEOMETRIC_TYPE_UNDEFINED=255;
1243 
1244  /**
1245  * Standard type for storing any lightweight 2D type. Do not inherit from this class.
1246  * \sa TPoint2D,TSegment2D,TLine2D,TPolygon2D
1247  */
1248 #ifdef TOBJECTS_USE_UNIONS
1249  struct BASE_IMPEXP TObject2D {
1250  private:
1251  /**
1252  * Object type identifier.
1253  */
1254  unsigned char type;
1255  /**
1256  * Union type storing pointers to every allowed type.
1257  */
1258  union {
1259  TPoint2D *point;
1260  TSegment2D *segment;
1261  TLine2D *line;
1262  TPolygon2D *polygon;
1263  } data;
1264  /**
1265  * Destroys the object, releasing the pointer to the content (if any).
1266  */
1267  void destroy() {
1268  switch(type) {
1269  case GEOMETRIC_TYPE_POINT:
1270  delete data.point;
1271  break;
1273  delete data.segment;
1274  break;
1275  case GEOMETRIC_TYPE_LINE:
1276  delete data.line;
1277  break;
1279  delete data.polygon;
1280  break;
1281  }
1283  }
1284  public:
1285  /**
1286  * Implicit constructor from point.
1287  */
1288  TObject2D(const TPoint2D &p):type(GEOMETRIC_TYPE_POINT) {
1289  data.point=new TPoint2D(p);
1290  }
1291  /**
1292  * Implicit constructor from segment.
1293  */
1294  TObject2D(const TSegment2D &s):type(GEOMETRIC_TYPE_SEGMENT) {
1295  data.segment=new TSegment2D(s);
1296  }
1297  /**
1298  * Implicit constructor from line.
1299  */
1300  TObject2D(const TLine2D &r):type(GEOMETRIC_TYPE_LINE) {
1301  data.line=new TLine2D(r);
1302  }
1303  /**
1304  * Implicit constructor from polygon.
1305  */
1306  TObject2D(const TPolygon2D &p):type(GEOMETRIC_TYPE_POLYGON) {
1307  data.polygon=new TPolygon2D(p);
1308  }
1309  /**
1310  * Implicit constructor from polygon.
1311  */
1313  /**
1314  * Object destruction.
1315  */
1316  ~TObject2D() {
1317  destroy();
1318  }
1319  /**
1320  * Checks whether content is a point.
1321  */
1322  inline bool isPoint() const {
1323  return type==GEOMETRIC_TYPE_POINT;
1324  }
1325  /**
1326  * Checks whether content is a segment.
1327  */
1328  inline bool isSegment() const {
1329  return type==GEOMETRIC_TYPE_SEGMENT;
1330  }
1331  /**
1332  * Checks whether content is a line.
1333  */
1334  inline bool isLine() const {
1335  return type==GEOMETRIC_TYPE_LINE;
1336  }
1337  /**
1338  * Checks whether content is a polygon.
1339  */
1340  inline bool isPolygon() const {
1341  return type==GEOMETRIC_TYPE_POLYGON;
1342  }
1343  /**
1344  * Gets content type.
1345  */
1346  inline unsigned char getType() const {
1347  return type;
1348  }
1349  /**
1350  * Gets the content as a point, returning false if the type is inadequate.
1351  */
1352  inline bool getPoint(TPoint2D &p) const {
1353  if (isPoint()) {
1354  p=*(data.point);
1355  return true;
1356  } else return false;
1357  }
1358  /**
1359  * Gets the content as a segment, returning false if the type is inadequate.
1360  */
1361  inline bool getSegment(TSegment2D &s) const {
1362  if (isSegment()) {
1363  s=*(data.segment);
1364  return true;
1365  } else return false;
1366  }
1367  /**
1368  * Gets the content as a line, returning false if the type is inadequate.
1369  */
1370  inline bool getLine(TLine2D &r) const {
1371  if (isLine()) {
1372  r=*(data.line);
1373  return true;
1374  } else return false;
1375  }
1376  /**
1377  * Gets the content as a polygon, returning false if the type is inadequate.
1378  */
1379  inline bool getPolygon(TPolygon2D &p) const {
1380  if (isPolygon()) {
1381  p=*(data.polygon);
1382  return true;
1383  } else return false;
1384  }
1385  /**
1386  * Assign another TObject2D. Pointers are not shared.
1387  */
1388  void operator=(const TObject2D &obj) {
1389  if (this==&obj) return;
1390  destroy();
1391  switch (type=obj.type) {
1392  case GEOMETRIC_TYPE_POINT:
1393  data.point=new TPoint2D(*(obj.data.point));
1394  break;
1396  data.segment=new TSegment2D(*(obj.data.segment));
1397  break;
1398  case GEOMETRIC_TYPE_LINE:
1399  data.line=new TLine2D(*(obj.data.line));
1400  break;
1402  data.polygon=new TPolygon2D(*(obj.data.polygon));
1403  break;
1404  }
1405  }
1406  /**
1407  * Assign a point to this object.
1408  */
1409  inline void operator=(const TPoint2D &p) {
1410  destroy();
1411  type=GEOMETRIC_TYPE_POINT;
1412  data.point=new TPoint2D(p);
1413  }
1414  /**
1415  * Assign a segment to this object.
1416  */
1417  inline void operator=(const TSegment2D &s) {
1418  destroy();
1420  data.segment=new TSegment2D(s);
1421  }
1422  /**
1423  * Assign a line to this object.
1424  */
1425  inline void operator=(const TLine2D &l) {
1426  destroy();
1427  type=GEOMETRIC_TYPE_LINE;
1428  data.line=new TLine2D(l);
1429  }
1430  /**
1431  * Assign a polygon to this object.
1432  */
1433  inline void operator=(const TPolygon2D &p) {
1434  destroy();
1436  data.polygon=new TPolygon2D(p);
1437  }
1438  /**
1439  * Project into 3D space.
1440  */
1441  void generate3DObject(TObject3D &obj) const;
1442  /**
1443  * Constructor from another TObject2D.
1444  */
1445  TObject2D(const TObject2D &obj):type(GEOMETRIC_TYPE_UNDEFINED) {
1446  operator=(obj);
1447  }
1448  /**
1449  * Static method to retrieve all the points in a vector of TObject2D.
1450  */
1451  static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts);
1452  /**
1453  * Static method to retrieve all the segments in a vector of TObject2D.
1454  */
1455  static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms);
1456  /**
1457  * Static method to retrieve all the lines in a vector of TObject2D.
1458  */
1459  static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins);
1460  /**
1461  * Static method to retrieve all the polygons in a vector of TObject2D.
1462  */
1463  static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys);
1464  /**
1465  * Static method to retrieve all the points in a vector of TObject2D, returning the remainder objects in another parameter.
1466  */
1467  static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts,std::vector<TObject2D> &remainder);
1468  /**
1469  * Static method to retrieve all the segments in a vector of TObject2D, returning the remainder objects in another parameter.
1470  */
1471  static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms,std::vector<TObject2D> &remainder);
1472  /**
1473  * Static method to retrieve all the lines in a vector of TObject2D, returning the remainder objects in another parameter.
1474  */
1475  static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins,std::vector<TObject2D> &remainder);
1476  /**
1477  * Static method to retrieve all the polygons in a vector of TObject2D, returning the remainder objects in another parameter.
1478  */
1479  static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys,std::vector<TObject2D> &remainder);
1480  };
1481  /**
1482  * Standard object for storing any 3D lightweight object. Do not inherit from this class.
1483  * \sa TPoint3D,TSegment3D,TLine3D,TPlane,TPolygon3D
1484  */
1485  struct BASE_IMPEXP TObject3D {
1486  private:
1487  /**
1488  * Object type identifier.
1489  */
1490  unsigned char type;
1491  /**
1492  * Union containing pointer to actual data.
1493  */
1494  union {
1495  TPoint3D *point;
1496  TSegment3D *segment;
1497  TLine3D *line;
1498  TPolygon3D *polygon;
1499  TPlane *plane;
1500  } data;
1501  /**
1502  * Destroys the object and releases the pointer, if any.
1503  */
1504  void destroy() {
1505  switch (type) {
1506  case GEOMETRIC_TYPE_POINT:
1507  delete data.point;
1508  break;
1510  delete data.segment;
1511  break;
1512  case GEOMETRIC_TYPE_LINE:
1513  delete data.line;
1514  break;
1516  delete data.polygon;
1517  break;
1518  case GEOMETRIC_TYPE_PLANE:
1519  delete data.plane;
1520  break;
1522  break;
1523  default:
1524  THROW_EXCEPTION("Invalid TObject2D object");
1525  }
1527  }
1528  public:
1529  /**
1530  * Constructor from point.
1531  */
1532  TObject3D(const TPoint3D &p):type(GEOMETRIC_TYPE_POINT) {
1533  data.point=new TPoint3D(p);
1534  }
1535  /**
1536  * Constructor from segment.
1537  */
1538  TObject3D(const TSegment3D &s):type(GEOMETRIC_TYPE_SEGMENT) {
1539  data.segment=new TSegment3D(s);
1540  }
1541  /**
1542  * Constructor from line.
1543  */
1544  TObject3D(const TLine3D &r):type(GEOMETRIC_TYPE_LINE) {
1545  data.line=new TLine3D(r);
1546  }
1547  /**
1548  * Constructor from polygon.
1549  */
1550  TObject3D(const TPolygon3D &p):type(GEOMETRIC_TYPE_POLYGON) {
1551  data.polygon=new TPolygon3D(p);
1552  }
1553  /**
1554  * Constructor from plane.
1555  */
1556  TObject3D(const TPlane &p):type(GEOMETRIC_TYPE_PLANE) {
1557  data.plane=new TPlane(p);
1558  }
1559  /**
1560  * Empty constructor.
1561  */
1563  /**
1564  * Destructor.
1565  */
1566  ~TObject3D() {
1567  destroy();
1568  }
1569  /**
1570  * Checks whether content is a point.
1571  */
1572  inline bool isPoint() const {
1573  return type==GEOMETRIC_TYPE_POINT;
1574  }
1575  /**
1576  * Checks whether content is a segment.
1577  */
1578  inline bool isSegment() const {
1579  return type==GEOMETRIC_TYPE_SEGMENT;
1580  }
1581  /**
1582  * Checks whether content is a line.
1583  */
1584  inline bool isLine() const {
1585  return type==GEOMETRIC_TYPE_LINE;
1586  }
1587  /**
1588  * Checks whether content is a polygon.
1589  */
1590  inline bool isPolygon() const {
1591  return type==GEOMETRIC_TYPE_POLYGON;
1592  }
1593  /**
1594  * Checks whether content is a plane.
1595  */
1596  inline bool isPlane() const {
1597  return type==GEOMETRIC_TYPE_PLANE;
1598  }
1599  /**
1600  * Gets object type.
1601  */
1602  inline unsigned char getType() const {
1603  return type;
1604  }
1605  /**
1606  * Gets the content as a point, returning false if the type is not adequate.
1607  */
1608  inline bool getPoint(TPoint3D &p) const {
1609  if (isPoint()) {
1610  p=*(data.point);
1611  return true;
1612  } else return false;
1613  }
1614  /**
1615  * Gets the content as a segment, returning false if the type is not adequate.
1616  */
1617  inline bool getSegment(TSegment3D &s) const {
1618  if (isSegment()) {
1619  s=*(data.segment);
1620  return true;
1621  } else return false;
1622  }
1623  /**
1624  * Gets the content as a line, returning false if the type is not adequate.
1625  */
1626  inline bool getLine(TLine3D &r) const {
1627  if (isLine()) {
1628  r=*(data.line);
1629  return true;
1630  } else return false;
1631  }
1632  /**
1633  * Gets the content as a polygon, returning false if the type is not adequate.
1634  */
1635  inline bool getPolygon(TPolygon3D &p) const {
1636  if (isPolygon()) {
1637  p=*(data.polygon);
1638  return true;
1639  } else return false;
1640  }
1641  /**
1642  * Gets the content as a plane, returning false if the type is not adequate.
1643  */
1644  inline bool getPlane(TPlane &p) const {
1645  if (isPlane()) {
1646  p=*(data.plane);
1647  return true;
1648  } else return false;
1649  }
1650  /**
1651  * Assigns another object, creating a new pointer if needed.
1652  */
1653  void operator=(const TObject3D &obj) {
1654  if (this==&obj) return;
1655  destroy();
1656  switch (type=obj.type) {
1657  case GEOMETRIC_TYPE_POINT:
1658  data.point=new TPoint3D(*(obj.data.point));
1659  break;
1661  data.segment=new TSegment3D(*(obj.data.segment));
1662  break;
1663  case GEOMETRIC_TYPE_LINE:
1664  data.line=new TLine3D(*(obj.data.line));
1665  break;
1667  data.polygon=new TPolygon3D(*(obj.data.polygon));
1668  break;
1669  case GEOMETRIC_TYPE_PLANE:
1670  data.plane=new TPlane(*(obj.data.plane));
1671  break;
1673  break;
1674  default:
1675  THROW_EXCEPTION("Invalid TObject3D object");
1676  }
1677  }
1678  /**
1679  * Assigns a point to this object.
1680  */
1681  inline void operator=(const TPoint3D &p) {
1682  destroy();
1683  type=GEOMETRIC_TYPE_POINT;
1684  data.point=new TPoint3D(p);
1685  }
1686  /**
1687  * Assigns a segment to this object.
1688  */
1689  inline void operator=(const TSegment3D &s) {
1690  destroy();
1692  data.segment=new TSegment3D(s);
1693  }
1694  /**
1695  * Assigns a line to this object.
1696  */
1697  inline void operator=(const TLine3D &l) {
1698  destroy();
1699  type=GEOMETRIC_TYPE_LINE;
1700  data.line=new TLine3D(l);
1701  }
1702  /**
1703  * Assigns a polygon to this object.
1704  */
1705  inline void operator=(const TPolygon3D &p) {
1706  destroy();
1708  data.polygon=new TPolygon3D(p);
1709  }
1710  /**
1711  * Assigns a plane to this object.
1712  */
1713  inline void operator=(const TPlane &p) {
1714  destroy();
1715  type=GEOMETRIC_TYPE_PLANE;
1716  data.plane=new TPlane(p);
1717  }
1718  /**
1719  * Projects into 2D space.
1720  * \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).
1721  */
1722  inline void generate2DObject(TObject2D &obj) const {
1723  switch (type) {
1724  case GEOMETRIC_TYPE_POINT:
1725  obj=TPoint2D(*(data.point));
1726  break;
1728  obj=TSegment2D(*(data.segment));
1729  break;
1730  case GEOMETRIC_TYPE_LINE:
1731  obj=TLine2D(*(data.line));
1732  break;
1734  obj=TPolygon2D(*(data.polygon));
1735  break;
1736  case GEOMETRIC_TYPE_PLANE:
1737  throw std::logic_error("Too many dimensions");
1738  default:
1739  obj=TObject2D();
1740  break;
1741  }
1742  }
1743  /**
1744  * Constructs from another object.
1745  */
1746  TObject3D(const TObject3D &obj):type(GEOMETRIC_TYPE_UNDEFINED) {
1747  operator=(obj);
1748  }
1749  /**
1750  * Static method to retrieve every point included in a vector of objects.
1751  */
1752  static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts);
1753  /**
1754  * Static method to retrieve every segment included in a vector of objects.
1755  */
1756  static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms);
1757  /**
1758  * Static method to retrieve every line included in a vector of objects.
1759  */
1760  static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins);
1761  /**
1762  * Static method to retrieve every plane included in a vector of objects.
1763  */
1764  static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns);
1765  /**
1766  * Static method to retrieve every polygon included in a vector of objects.
1767  */
1768  static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys);
1769  /**
1770  * Static method to retrieve every point included in a vector of objects, returning the remaining objects in another argument.
1771  */
1772  static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts,std::vector<TObject3D> &remainder);
1773  /**
1774  * Static method to retrieve every segment included in a vector of objects, returning the remaining objects in another argument.
1775  */
1776  static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms,std::vector<TObject3D> &remainder);
1777  /**
1778  * Static method to retrieve every line included in a vector of objects, returning the remaining objects in another argument.
1779  */
1780  static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins,std::vector<TObject3D> &remainder);
1781  /**
1782  * Static method to retrieve every plane included in a vector of objects, returning the remaining objects in another argument.
1783  */
1784  static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns,std::vector<TObject3D> &remainder);
1785  /**
1786  * Static method to retrieve every polygon included in a vector of objects, returning the remaining objects in another argument.
1787  */
1788  static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder);
1789  };
1790 #else
1792  private:
1793  /**
1794  * Object type identifier.
1795  */
1796  unsigned char type;
1797  /**
1798  * Union type storing pointers to every allowed type.
1799  */
1800  struct {
1805  } data;
1806  /**
1807  * Destroys the object, releasing the pointer to the content (if any).
1808  */
1809  inline void destroy() {
1810  if (type==GEOMETRIC_TYPE_POLYGON) delete data.polygon;
1812  }
1813  public:
1814  /**
1815  * Implicit constructor from point.
1816  */
1817  inline TObject2D(const TPoint2D &p):type(GEOMETRIC_TYPE_POINT) {
1818  data.point=p;
1819  }
1820  /**
1821  * Implicit constructor from segment.
1822  */
1824  data.segment=s;
1825  }
1826  /**
1827  * Implicit constructor from line.
1828  */
1829  inline TObject2D(const TLine2D &r):type(GEOMETRIC_TYPE_LINE) {
1830  data.line=r;
1831  }
1832  /**
1833  * Implicit constructor from polygon.
1834  */
1836  data.polygon=new TPolygon2D(p);
1837  }
1838  /**
1839  * Implicit constructor from polygon.
1840  */
1842  /**
1843  * Object destruction.
1844  */
1846  destroy();
1847  }
1848  /**
1849  * Checks whether content is a point.
1850  */
1851  inline bool isPoint() const {
1852  return type==GEOMETRIC_TYPE_POINT;
1853  }
1854  /**
1855  * Checks whether content is a segment.
1856  */
1857  inline bool isSegment() const {
1858  return type==GEOMETRIC_TYPE_SEGMENT;
1859  }
1860  /**
1861  * Checks whether content is a line.
1862  */
1863  inline bool isLine() const {
1864  return type==GEOMETRIC_TYPE_LINE;
1865  }
1866  /**
1867  * Checks whether content is a polygon.
1868  */
1869  inline bool isPolygon() const {
1870  return type==GEOMETRIC_TYPE_POLYGON;
1871  }
1872  /**
1873  * Gets content type.
1874  */
1875  inline unsigned char getType() const {
1876  return type;
1877  }
1878  /**
1879  * Gets the content as a point, returning false if the type is inadequate.
1880  */
1881  inline bool getPoint(TPoint2D &p) const {
1882  if (isPoint()) {
1883  p=data.point;
1884  return true;
1885  } else return false;
1886  }
1887  /**
1888  * Gets the content as a segment, returning false if the type is inadequate.
1889  */
1890  inline bool getSegment(TSegment2D &s) const {
1891  if (isSegment()) {
1892  s=data.segment;
1893  return true;
1894  } else return false;
1895  }
1896  /**
1897  * Gets the content as a line, returning false if the type is inadequate.
1898  */
1899  inline bool getLine(TLine2D &r) const {
1900  if (isLine()) {
1901  r=data.line;
1902  return true;
1903  } else return false;
1904  }
1905  /**
1906  * Gets the content as a polygon, returning false if the type is inadequate.
1907  */
1908  inline bool getPolygon(TPolygon2D &p) const {
1909  if (isPolygon()) {
1910  p=*(data.polygon);
1911  return true;
1912  } else return false;
1913  }
1914  /**
1915  * Assign another TObject2D. Pointers are not shared.
1916  */
1917  void operator=(const TObject2D &obj) {
1918  if (this==&obj) return;
1919  destroy();
1920  switch (type=obj.type) {
1921  case GEOMETRIC_TYPE_POINT:
1922  data.point=obj.data.point;
1923  break;
1925  data.segment=obj.data.segment;
1926  break;
1927  case GEOMETRIC_TYPE_LINE:
1928  data.line=obj.data.line;
1929  break;
1931  data.polygon=new TPolygon2D(*(obj.data.polygon));
1932  break;
1933  }
1934  }
1935  /**
1936  * Assign a point to this object.
1937  */
1938  inline void operator=(const TPoint2D &p) {
1939  destroy();
1940  type=GEOMETRIC_TYPE_POINT;
1941  data.point=p;
1942  }
1943  /**
1944  * Assign a segment to this object.
1945  */
1946  inline void operator=(const TSegment2D &s) {
1947  destroy();
1949  data.segment=s;
1950  }
1951  /**
1952  * Assign a line to this object.
1953  */
1954  inline void operator=(const TLine2D &l) {
1955  destroy();
1956  type=GEOMETRIC_TYPE_LINE;
1957  data.line=l;
1958  }
1959  /**
1960  * Assign a polygon to this object.
1961  */
1962  inline void operator=(const TPolygon2D &p) {
1963  destroy();
1965  data.polygon=new TPolygon2D(p);
1966  }
1967  /**
1968  * Project into 3D space.
1969  */
1970  void generate3DObject(TObject3D &obj) const;
1971  /**
1972  * Constructor from another TObject2D.
1973  */
1975  operator=(obj);
1976  }
1977  /**
1978  * Static method to retrieve all the points in a vector of TObject2D.
1979  */
1980  static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts);
1981  /**
1982  * Static method to retrieve all the segments in a vector of TObject2D.
1983  */
1984  static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms);
1985  /**
1986  * Static method to retrieve all the lines in a vector of TObject2D.
1987  */
1988  static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins);
1989  /**
1990  * Static method to retrieve all the polygons in a vector of TObject2D.
1991  */
1992  static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys);
1993  /**
1994  * Static method to retrieve all the points in a vector of TObject2D, returning the remainder objects in another parameter.
1995  */
1996  static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts,std::vector<TObject2D> &remainder);
1997  /**
1998  * Static method to retrieve all the segments in a vector of TObject2D, returning the remainder objects in another parameter.
1999  */
2000  static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms,std::vector<TObject2D> &remainder);
2001  /**
2002  * Static method to retrieve all the lines in a vector of TObject2D, returning the remainder objects in another parameter.
2003  */
2004  static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins,std::vector<TObject2D> &remainder);
2005  /**
2006  * Static method to retrieve all the polygons in a vector of TObject2D, returning the remainder objects in another parameter.
2007  */
2008  static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys,std::vector<TObject2D> &remainder);
2009  };
2010  /**
2011  * Standard object for storing any 3D lightweight object. Do not inherit from this class.
2012  * \sa TPoint3D,TSegment3D,TLine3D,TPlane,TPolygon3D
2013  */
2015  private:
2016  /**
2017  * Object type identifier.
2018  */
2019  unsigned char type;
2020  /**
2021  * Union containing pointer to actual data.
2022  */
2023  struct {
2029  } data;
2030  /**
2031  * Destroys the object and releases the pointer, if any.
2032  */
2033  void destroy() {
2034  if (type==GEOMETRIC_TYPE_POLYGON) delete data.polygon;
2036  }
2037  public:
2038  /**
2039  * Constructor from point.
2040  */
2042  data.point=p;
2043  }
2044  /**
2045  * Constructor from segment.
2046  */
2048  data.segment=s;
2049  }
2050  /**
2051  * Constructor from line.
2052  */
2054  data.line=r;
2055  }
2056  /**
2057  * Constructor from polygon.
2058  */
2060  data.polygon=new TPolygon3D(p);
2061  }
2062  /**
2063  * Constructor from plane.
2064  */
2066  data.plane=p;
2067  }
2068  /**
2069  * Empty constructor.
2070  */
2072  /**
2073  * Destructor.
2074  */
2076  destroy();
2077  }
2078  /**
2079  * Checks whether content is a point.
2080  */
2081  inline bool isPoint() const {
2082  return type==GEOMETRIC_TYPE_POINT;
2083  }
2084  /**
2085  * Checks whether content is a segment.
2086  */
2087  inline bool isSegment() const {
2088  return type==GEOMETRIC_TYPE_SEGMENT;
2089  }
2090  /**
2091  * Checks whether content is a line.
2092  */
2093  inline bool isLine() const {
2094  return type==GEOMETRIC_TYPE_LINE;
2095  }
2096  /**
2097  * Checks whether content is a polygon.
2098  */
2099  inline bool isPolygon() const {
2100  return type==GEOMETRIC_TYPE_POLYGON;
2101  }
2102  /**
2103  * Checks whether content is a plane.
2104  */
2105  inline bool isPlane() const {
2106  return type==GEOMETRIC_TYPE_PLANE;
2107  }
2108  /**
2109  * Gets object type.
2110  */
2111  inline unsigned char getType() const {
2112  return type;
2113  }
2114  /**
2115  * Gets the content as a point, returning false if the type is not adequate.
2116  */
2117  inline bool getPoint(TPoint3D &p) const {
2118  if (isPoint()) {
2119  p=data.point;
2120  return true;
2121  } else return false;
2122  }
2123  /**
2124  * Gets the content as a segment, returning false if the type is not adequate.
2125  */
2126  inline bool getSegment(TSegment3D &s) const {
2127  if (isSegment()) {
2128  s=data.segment;
2129  return true;
2130  } else return false;
2131  }
2132  /**
2133  * Gets the content as a line, returning false if the type is not adequate.
2134  */
2135  inline bool getLine(TLine3D &r) const {
2136  if (isLine()) {
2137  r=data.line;
2138  return true;
2139  } else return false;
2140  }
2141  /**
2142  * Gets the content as a polygon, returning false if the type is not adequate.
2143  */
2144  inline bool getPolygon(TPolygon3D &p) const {
2145  if (isPolygon()) {
2146  p=*(data.polygon);
2147  return true;
2148  } else return false;
2149  }
2150  /**
2151  * Gets the content as a plane, returning false if the type is not adequate.
2152  */
2153  inline bool getPlane(TPlane &p) const {
2154  if (isPlane()) {
2155  p=data.plane;
2156  return true;
2157  } else return false;
2158  }
2159  /**
2160  * Assigns another object, creating a new pointer if needed.
2161  */
2162  void operator=(const TObject3D &obj) {
2163  if (this==&obj) return;
2164  destroy();
2165  switch (type=obj.type) {
2166  case GEOMETRIC_TYPE_POINT:
2167  data.point=obj.data.point;
2168  break;
2170  data.segment=obj.data.segment;
2171  break;
2172  case GEOMETRIC_TYPE_LINE:
2173  data.line=obj.data.line;
2174  break;
2176  data.polygon=new TPolygon3D(*(obj.data.polygon));
2177  break;
2178  case GEOMETRIC_TYPE_PLANE:
2179  data.plane=obj.data.plane;
2180  break;
2182  break;
2183  default:
2184  THROW_EXCEPTION("Invalid TObject3D object");
2185  }
2186  }
2187  /**
2188  * Assigns a point to this object.
2189  */
2190  inline void operator=(const TPoint3D &p) {
2191  destroy();
2192  type=GEOMETRIC_TYPE_POINT;
2193  data.point=p;
2194  }
2195  /**
2196  * Assigns a segment to this object.
2197  */
2198  inline void operator=(const TSegment3D &s) {
2199  destroy();
2201  data.segment=s;
2202  }
2203  /**
2204  * Assigns a line to this object.
2205  */
2206  inline void operator=(const TLine3D &l) {
2207  destroy();
2208  type=GEOMETRIC_TYPE_LINE;
2209  data.line=l;
2210  }
2211  /**
2212  * Assigns a polygon to this object.
2213  */
2214  inline void operator=(const TPolygon3D &p) {
2215  destroy();
2217  data.polygon=new TPolygon3D(p);
2218  }
2219  /**
2220  * Assigns a plane to this object.
2221  */
2222  inline void operator=(const TPlane &p) {
2223  destroy();
2224  type=GEOMETRIC_TYPE_PLANE;
2225  data.plane=p;
2226  }
2227  /**
2228  * Projects into 2D space.
2229  * \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).
2230  */
2231  inline void generate2DObject(TObject2D &obj) const {
2232  switch (type) {
2233  case GEOMETRIC_TYPE_POINT:
2234  obj=TPoint2D(data.point);
2235  break;
2237  obj=TSegment2D(data.segment);
2238  break;
2239  case GEOMETRIC_TYPE_LINE:
2240  obj=TLine2D(data.line);
2241  break;
2243  obj=TPolygon2D(*(data.polygon));
2244  break;
2245  case GEOMETRIC_TYPE_PLANE:
2246  throw std::logic_error("Too many dimensions");
2247  default:
2248  obj=TObject2D();
2249  break;
2250  }
2251  }
2252  /**
2253  * Constructs from another object.
2254  */
2256  operator=(obj);
2257  }
2258  /**
2259  * Static method to retrieve every point included in a vector of objects.
2260  */
2261  static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts);
2262  /**
2263  * Static method to retrieve every segment included in a vector of objects.
2264  */
2265  static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms);
2266  /**
2267  * Static method to retrieve every line included in a vector of objects.
2268  */
2269  static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins);
2270  /**
2271  * Static method to retrieve every plane included in a vector of objects.
2272  */
2273  static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns);
2274  /**
2275  * Static method to retrieve every polygon included in a vector of objects.
2276  */
2277  static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys);
2278  /**
2279  * Static method to retrieve every point included in a vector of objects, returning the remaining objects in another argument.
2280  */
2281  static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts,std::vector<TObject3D> &remainder);
2282  /**
2283  * Static method to retrieve every segment included in a vector of objects, returning the remaining objects in another argument.
2284  */
2285  static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms,std::vector<TObject3D> &remainder);
2286  /**
2287  * Static method to retrieve every line included in a vector of objects, returning the remaining objects in another argument.
2288  */
2289  static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins,std::vector<TObject3D> &remainder);
2290  /**
2291  * Static method to retrieve every plane included in a vector of objects, returning the remaining objects in another argument.
2292  */
2293  static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns,std::vector<TObject3D> &remainder);
2294  /**
2295  * Static method to retrieve every polygon included in a vector of objects, returning the remaining objects in another argument.
2296  */
2297  static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder);
2298  };
2299 
2300 #endif
2301 
2302 
2303  //Streaming functions
2304  /**
2305  * TPoint2D binary input.
2306  */
2308  /**
2309  * TPoint2D binary output.
2310  */
2312 
2313  /**
2314  * TPoint3D binary input.
2315  */
2317  /**
2318  * TPoint3D binary output.
2319  */
2321 
2322  /**
2323  * TPose2D binary input.
2324  */
2326  /**
2327  * TPose2D binary output.
2328  */
2330 
2331  /**
2332  * TPose3D binary input.
2333  */
2335  /**
2336  * TPose3D binary output.
2337  */
2339 
2340  /**
2341  * TSegment2D binary input.
2342  */
2344  return in>>s.point1>>s.point2;
2345  }
2346  /**
2347  * TSegment2D binary output.
2348  */
2350  return out<<s.point1<<s.point2;
2351  }
2352 
2353  /**
2354  * TLine2D binary input.
2355  */
2357  return in>>l.coefs[0]>>l.coefs[1]>>l.coefs[2];
2358  }
2359  /**
2360  * TLine2D binary output.
2361  */
2363  return out<<l.coefs[0]<<l.coefs[1]<<l.coefs[2];
2364  }
2365 
2366  /**
2367  * TObject2D binary input.
2368  */
2370  /**
2371  * TObject2D binary input.
2372  */
2374 
2375  /**
2376  * TSegment3D binary input.
2377  */
2379  return in>>s.point1>>s.point2;
2380  }
2381  /**
2382  * TSegment3D binary output.
2383  */
2385  return out<<s.point1<<s.point2;
2386  }
2387 
2388  /**
2389  * TLine3D binary input.
2390  */
2392  return in>>l.pBase>>l.director[0]>>l.director[1]>>l.director[2];
2393  }
2394  /**
2395  * TLine3D binary output.
2396  */
2398  return out<<l.pBase<<l.director[0]<<l.director[1]<<l.director[2];
2399  }
2400 
2401  /**
2402  * TPlane binary input.
2403  */
2405  return in>>p.coefs[0]>>p.coefs[1]>>p.coefs[2]>>p.coefs[3];
2406  }
2407  /**
2408  * TPlane binary output.
2409  */
2411  return out<<p.coefs[0]<<p.coefs[1]<<p.coefs[2]<<p.coefs[3];
2412  }
2413 
2414  /**
2415  * TObject3D binary input.
2416  */
2418  /**
2419  * TObject3D binary output.
2420  */
2422 
2423 
2424  /** @} */ // end of grouping
2425 
2426  } //end of namespace math
2427 
2428  namespace utils
2429  {
2430  using namespace ::mrpt::math;
2431 
2432  // Specialization must occur in the same namespace
2433  MRPT_DECLARE_TTYPENAME(TPoint2D)
2434  MRPT_DECLARE_TTYPENAME(TPoint3D)
2435  MRPT_DECLARE_TTYPENAME(TPose2D)
2436  MRPT_DECLARE_TTYPENAME(TPose3D)
2437  MRPT_DECLARE_TTYPENAME(TSegment2D)
2438  MRPT_DECLARE_TTYPENAME(TLine2D)
2439  MRPT_DECLARE_TTYPENAME(TPolygon2D)
2440  MRPT_DECLARE_TTYPENAME(TObject2D)
2443  MRPT_DECLARE_TTYPENAME(TPlane)
2446 
2447  } // end of namespace utils
2448 
2449 } //end of namespace
2450 #endif
bool operator!=(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:280
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)
void getAsVector(vector_double &v) const
Transformation into vector.
double & operator[](size_t i)
Unsafe coordinate access using operator[].
double qx
Quaternion part, x.
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.
double y
Y coordinate.
TPolygon3D(const std::vector< TPoint3D > &v)
Implicit constructor from a 3D points vector.
TPoint2D operator/(double d) const
bool getPolygon(TPolygon2D &p) const
Gets the content as a polygon, returning false if the type is inadequate.
TPoint2D & operator*=(double d)
double y
Y coordinate.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
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.
double x
X coordinate.
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.
double x
X coordinate.
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 x
X coordinate.
double norm() const
Point norm.
void operator=(const TSegment2D &s)
Assign a segment to this object.
std::string asString() const
class BASE_IMPEXP CStream
Definition: math_frwds.h:28
TPoint3D()
Default fast constructor.
TPoint3D operator+(const TPoint3D &p) const
Points addition.
Standard type for storing any lightweight 2D type.
struct mrpt::math::TObject3D::@4 data
Union containing pointer to actual data.
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
Z coordinate.
TPoint3D & operator+=(const TPoint3D &p)
Translation.
struct BASE_IMPEXP TObject3D
double yaw
Yaw coordinate (rotation angle over Z axis).
void operator=(const TSegment3D &s)
Assigns a segment to this object.
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.
struct BASE_IMPEXP TSegment3D
double norm() const
Pose&#39;s spatial coordinates norm.
void getUnitaryDirectorVector(double(&vector)[2])
Unitarize line and then get director vector.
double & operator[](size_t i)
Unsafe coordinate access using operator[].
double qy
Quaternion part, y.
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.
void getAsVector(vector_double &v) const
Transformation into vector.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:36
void getAsVector(vector_double &v) const
Gets the pose as a vector of doubles.
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:19
2D segment, consisting of two points.
bool isPoint() const
Checks whether content is a point.
The base class of MRPT vectors, actually, Eigen column matrices of dynamic size with specialized cons...
TPlane(const double(&vec)[4])
Constructor from an array of coefficients.
3D segment, consisting of two points.
TObject2D(const TLine2D &r)
Implicit constructor from line.
Lightweight 3D point (float version).
TObject3D(const TPoint3D &p)
Constructor from point.
TPoint2D(double xx, double yy)
Constructor from coordinates.
const double & operator[](size_t i) const
Unsafe coordinate access using operator[].
TPoint3D point2
Destiny point.
void getAsVector(vector_double &v) const
Transformation into vector.
std::string asString() const
double distanceTo(const TPoint3D &p) const
Point-to-point distance.
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
Definition: ops_vectors.h:64
TLine2D(double A, double B, double C)
Constructor from line&#39;s coefficients.
double z
Translation in z.
const double & operator[](size_t i) const
Unsafe coordinate access using operator[].
double x
Translation in x.
const unsigned char GEOMETRIC_TYPE_SEGMENT
Object type identifier for TSegment2D or TSegment3D.
const TPoint3D & operator[](size_t i) const
Unsafe point access using [] operator, intended for loops.
bool operator==(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:272
TPoint3D operator/(double d) const
T wrapTo2Pi(T a)
Modifies the given angle to translate it into the [0,2pi[ range.
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.
double coefs[3]
Line coefficients, stored as an array: .
class BASE_IMPEXP TPolygon3D
TPoint2D operator*(double d) const
TPolygon2D()
Default constructor.
double x
X coordinate.
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:44
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.
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)
Unsafe point access using [] operator, intended for loops.
double qz
Quaternion part, z.
bool isPlane() const
Checks whether content is a plane.
double qr
Quaternion part, r.
A class used to store a 2D point.
Definition: CPoint2D.h:35
A class used to store a 3D point.
Definition: CPoint3D.h:33
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.
void getAsVector(vector_double &v) const
Gets the pose as a vector of doubles.
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 ).
double director[3]
Director vector.
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:276
double y
Y coordinate.
void resize(const size_t N, const T default_val)
Overloaded resize method that mimics std::vector::resize(SIZE,DEFAULT_VALUE) instead of resize(nrows...
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:158
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
Unsafe 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]" )
double y
Translation in y.
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:35
TPoint2D operator-(const TPoint2D &p) const
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:71
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.
double coefs[4]
Plane coefficients, stored as an array: .
TSegment2D()
Fast default constructor.
unsigned char type
Object type identifier.
~TObject2D()
Object destruction.
const TPoint2D & operator[](size_t i) const
Unsafe point access using [] operator, intended for loops.
TPoint3D & operator[](size_t i)
Unsafe point access using [] operator, intended for loops.
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.
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
Unsafe 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.
struct mrpt::math::TObject2D::@3 data
Union type storing pointers to every allowed type.
void getAsPose3D(mrpt::poses::CPose3D &outPose) const
Gets a pose whose XY plane corresponds to this plane.
double y
Y coordinate.
TObject3D(const TObject3D &obj)
Constructs from another object.
TPose2D()
Default fast constructor.
unsigned char getType() const
Gets object type.
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees) ...
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)
Unsafe 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 operator=(const TObject3D &obj)
Assigns another object, creating a new pointer if needed.
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.
TPlane(double A, double B, double C, double D)
Constructor from plane coefficients.
double z
Z coordinate.
TObject3D(const TPolygon3D &p)
Constructor from polygon.
double phi
Phi coordinate.
double & operator[](size_t i)
Unsafe 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>
double & operator[](size_t i)
Unsafe 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:53
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.
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]"...
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
Unsafe coordinate access using operator[].
TPoint2D & operator/=(double d)
void operator=(const TObject2D &obj)
Assign another TObject2D.
#define MRPT_DECLARE_TTYPENAME(_TYPE)
Definition: TTypeName.h:59
3D line, represented by a base point and a director vector.
TPolygon2D(const std::vector< TPoint2D > &v)
Implicit constructor from a vector of 2D points.
2D line without bounds, represented by its equation .



Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo