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