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