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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020