MRPT  2.0.5
TPoint3D.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-2020, 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/format.h>
13 #include <mrpt/math/TPoseOrPoint.h>
14 #include <mrpt/math/math_frwds.h> // CMatrixFixed
15 #include <cmath> // sqrt
16 
17 namespace mrpt::math
18 {
19 // Ensure 1-byte memory alignment, no additional stride bytes.
20 #pragma pack(push, 1)
21 
22 /** Trivially copiable underlying data for TPoint3D
23  * 1-byte memory packed, no padding]
24  */
25 template <typename T>
27 {
28  /** X,Y,Z coordinates */
29  T x, y, z;
30 };
31 
32 /** Base template for TPoint3D and TPoint3Df.
33  * [1-byte memory packed, no padding]
34  * \ingroup geometry_grp
35  */
36 template <typename T>
37 struct TPoint3D_ : public TPoseOrPoint,
38  public TPoint3D_data<T>,
39  public internal::ProvideStaticResize<TPoint3D_<T>>
40 {
41  enum
42  {
44  };
45 
46  /** Default constructor. Initializes to zeros. */
47  constexpr TPoint3D_() : TPoint3D_data<T>{0, 0, 0} {}
48  /** Constructor from coordinates. */
49  constexpr TPoint3D_(T xx, T yy, T zz) : TPoint3D_data<T>{xx, yy, zz} {}
50 
51  /** Constructor from coordinates. */
52  template <typename U>
54  {
55  TPoint3D_data<T>::x = static_cast<T>(p.x);
56  TPoint3D_data<T>::y = static_cast<T>(p.y);
57  TPoint3D_data<T>::z = static_cast<T>(p.z);
58  }
59 
60  /** Constructor from column vector. */
61  template <typename U>
63  {
64  TPoint3D_data<T>::x = static_cast<T>(m[0]);
65  TPoint3D_data<T>::y = static_cast<T>(m[1]);
66  TPoint3D_data<T>::z = static_cast<T>(m[2]);
67  }
68 
69  /** Implicit constructor from TPoint2D. Zeroes the z.
70  * \sa TPoint2D
71  */
72  TPoint3D_(const TPoint2D_<T>& p);
73  /**
74  * Constructor from TPose2D, losing information. Zeroes the z.
75  * \sa TPose2D
76  */
77  explicit TPoint3D_(const TPose2D& p);
78  /**
79  * Constructor from TPose3D, losing information.
80  * \sa TPose3D
81  */
82  explicit TPoint3D_(const TPose3D& p);
83 
84  /** Return a copy of this object using type U for coordinates */
85  template <typename U>
87  {
88  return TPoint3D_<U>(
89  static_cast<U>(this->x), static_cast<U>(this->y),
90  static_cast<U>(this->z));
91  }
92 
93  /** Coordinate access using operator[]. Order: x,y,z */
94  T& operator[](size_t i)
95  {
96  switch (i)
97  {
98  case 0:
99  return TPoint3D_data<T>::x;
100  case 1:
101  return TPoint3D_data<T>::y;
102  case 2:
103  return TPoint3D_data<T>::z;
104  default:
105  throw std::out_of_range("index out of range");
106  }
107  }
108  /** Coordinate access using operator[]. Order: x,y,z */
109  constexpr T operator[](size_t i) const
110  {
111  switch (i)
112  {
113  case 0:
114  return TPoint3D_data<T>::x;
115  case 1:
116  return TPoint3D_data<T>::y;
117  case 2:
118  return TPoint3D_data<T>::z;
119  default:
120  throw std::out_of_range("index out of range");
121  }
122  }
123  /**
124  * Point-to-point distance.
125  */
126  T distanceTo(const TPoint3D_<T>& p) const
127  {
128  return std::sqrt(
132  }
133  /**
134  * Point-to-point distance, squared.
135  */
136  T sqrDistanceTo(const TPoint3D_<T>& p) const
137  {
138  return square(p.x - TPoint3D_data<T>::x) +
141  }
142  /** Squared norm: |v|^2 = x^2+y^2+z^2 */
143  T sqrNorm() const
144  {
147  }
148 
149  /** Point norm: |v| = sqrt(x^2+y^2+z^2) */
150  T norm() const { return std::sqrt(sqrNorm()); }
151 
152  /** Returns this vector with unit length: v/norm(v) */
154  {
155  const T n = norm();
156  ASSERT_ABOVE_(n, 0);
157  const T f = 1 / n;
158  return {TPoint3D_data<T>::x * f, TPoint3D_data<T>::y * f,
159  TPoint3D_data<T>::z * f};
160  }
161 
162  /** Scale point/vector */
164  {
165  TPoint3D_data<T>::x *= f;
166  TPoint3D_data<T>::y *= f;
167  TPoint3D_data<T>::z *= f;
168  return *this;
169  }
170  /**
171  * Transformation into vector.
172  */
173  template <class VECTORLIKE>
174  void asVector(VECTORLIKE& v) const
175  {
176  v.resize(3);
177  v[0] = TPoint3D_data<T>::x;
178  v[1] = TPoint3D_data<T>::y;
179  v[2] = TPoint3D_data<T>::z;
180  }
181  /**
182  * Translation.
183  */
185  {
186  TPoint3D_data<T>::x += p.x;
187  TPoint3D_data<T>::y += p.y;
188  TPoint3D_data<T>::z += p.z;
189  return *this;
190  }
191  /**
192  * Difference between points.
193  */
195  {
196  TPoint3D_data<T>::x -= p.x;
197  TPoint3D_data<T>::y -= p.y;
198  TPoint3D_data<T>::z -= p.z;
199  return *this;
200  }
201  /**
202  * Points addition.
203  */
204  constexpr TPoint3D_<T> operator+(const TPoint3D_<T>& p) const
205  {
206  return {TPoint3D_data<T>::x + p.x, TPoint3D_data<T>::y + p.y,
207  TPoint3D_data<T>::z + p.z};
208  }
209  /**
210  * Points substraction.
211  */
212  constexpr TPoint3D_<T> operator-(const TPoint3D_<T>& p) const
213  {
214  return {TPoint3D_data<T>::x - p.x, TPoint3D_data<T>::y - p.y,
215  TPoint3D_data<T>::z - p.z};
216  }
217 
218  constexpr TPoint3D_<T> operator*(T d) const
219  {
220  return {TPoint3D_data<T>::x * d, TPoint3D_data<T>::y * d,
221  TPoint3D_data<T>::z * d};
222  }
223 
224  constexpr TPoint3D_<T> operator/(T d) const
225  {
226  return {TPoint3D_data<T>::x / d, TPoint3D_data<T>::y / d,
227  TPoint3D_data<T>::z / d};
228  }
229 
230  bool operator<(const TPoint3D_<T>& p) const;
231 
232  /** Returns a human-readable textual representation of the object (eg:
233  * "[0.02 1.04 -0.8]" )
234  * \sa fromString
235  */
236  void asString(std::string& s) const
237  {
238  s = mrpt::format(
241  }
242  std::string asString() const
243  {
244  std::string s;
245  asString(s);
246  return s;
247  }
248 
249  /** Set the current object value from a string generated by 'asString' (eg:
250  * "[0.02 1.04 -0.8]" )
251  * \sa asString
252  * \exception std::exception On invalid format
253  */
254  void fromString(const std::string& s);
255 
256  static TPoint3D_<T> FromString(const std::string& s)
257  {
258  TPoint3D_<T> o;
259  o.fromString(s);
260  return o;
261  }
262 };
263 
264 /** Lightweight 3D point. Allows coordinate access using [] operator.
265  * (1-byte memory packed, no padding).
266  * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3Df
267  */
270 
271 /** Useful type alias for 3-vectors.
272  * (1-byte memory packed, no padding) */
275 
276 /** XYZ point (double) + Intensity(u8) \sa mrpt::math::TPoint3D */
278 {
280  uint8_t intensity{0};
281  TPointXYZIu8() : pt() {}
282  constexpr TPointXYZIu8(double x, double y, double z, uint8_t intensity_val)
283  : pt(x, y, z), intensity(intensity_val)
284  {
285  }
286 };
287 /** XYZ point (double) + RGB(u8) \sa mrpt::math::TPoint3D */
289 {
291  uint8_t r{0}, g{0}, b{0};
292  TPointXYZRGBu8() = default;
293  constexpr TPointXYZRGBu8(
294  double x, double y, double z, uint8_t R_val, uint8_t G_val,
295  uint8_t B_val)
296  : pt(x, y, z), r(R_val), g(G_val), b(B_val)
297  {
298  }
299 };
300 /** XYZ point (float) + Intensity(u8) \sa mrpt::math::TPoint3D */
302 {
304  uint8_t intensity{0};
305  TPointXYZfIu8() = default;
306  constexpr TPointXYZfIu8(float x, float y, float z, uint8_t intensity_val)
307  : pt(x, y, z), intensity(intensity_val)
308  {
309  }
310 };
311 /** XYZ point (float) + RGB(u8) \sa mrpt::math::TPoint3D */
313 {
315  uint8_t r{0}, g{0}, b{0};
317  constexpr TPointXYZfRGBu8(
318  float x, float y, float z, uint8_t R_val, uint8_t G_val, uint8_t B_val)
319  : pt(x, y, z), r(R_val), g(G_val), b(B_val)
320  {
321  }
322 };
323 
328 
329 /** XYZ point (float) + RGBA(u8) \sa mrpt::math::TPoint3D */
331 {
333  uint8_t r{0}, g{0}, b{0}, a{0xff};
335  constexpr TPointXYZfRGBAu8(
336  float x, float y, float z, uint8_t R_val, uint8_t G_val, uint8_t B_val,
337  uint8_t A_val = 0xff)
338  : pt(x, y, z), r(R_val), g(G_val), b(B_val), a(A_val)
339  {
340  }
341 };
342 
347 
348 /** XYZ point (float) + RGBA(float) [1-byte memory packed, no padding]
349  * \sa mrpt::math::TPoint3D */
351 {
353  float R{0}, G{0}, B{0}, A{0};
354  TPointXYZRGBAf() = default;
355 
356  constexpr TPointXYZRGBAf(
357  float x, float y, float z, float R_val, float G_val, float B_val,
358  float A_val)
359  : pt(x, y, z), R(R_val), G(G_val), B(B_val), A(A_val)
360  {
361  }
362 };
363 #pragma pack(pop)
364 
365 /** Unary minus operator for 3D points. */
366 template <typename T>
367 constexpr TPoint3D_<T> operator-(const TPoint3D_<T>& p1)
368 {
369  return {-p1.x, -p1.y, -p1.z};
370 }
371 
372 /** Exact comparison between 3D points */
373 template <typename T>
374 constexpr bool operator==(const TPoint3D_<T>& p1, const TPoint3D_<T>& p2)
375 {
376  return (p1.x == p2.x) && (p1.y == p2.y) && (p1.z == p2.z); //-V550
377 }
378 /** Exact comparison between 3D points */
379 template <typename T>
380 constexpr bool operator!=(const TPoint3D_<T>& p1, const TPoint3D_<T>& p2)
381 {
382  return (p1.x != p2.x) || (p1.y != p2.y) || (p1.z != p2.z); //-V550
383 }
384 
385 } // namespace mrpt::math
386 
387 namespace mrpt::typemeta
388 {
389 // Specialization must occur in the same namespace
392 } // namespace mrpt::typemeta
T sqrNorm() const
Squared norm: |v|^2 = x^2+y^2+z^2.
Definition: TPoint3D.h:143
std::vector< T1 > operator-(const std::vector< T1 > &v1, const std::vector< T2 > &v2)
Definition: ops_vectors.h:92
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
constexpr TPointXYZIu8(double x, double y, double z, uint8_t intensity_val)
Definition: TPoint3D.h:282
TPoint3D_(const TPoint3D_data< U > &p)
Constructor from coordinates.
Definition: TPoint3D.h:53
Trivially copiable underlying data for TPoint3D 1-byte memory packed, no padding].
Definition: TPoint3D.h:26
TPoint3D_(const mrpt::math::CMatrixFixed< U, 3, 1 > &m)
Constructor from column vector.
Definition: TPoint3D.h:62
mrpt::math::TPoint3D pt
Definition: TPoint3D.h:290
constexpr TPointXYZfIu8(float x, float y, float z, uint8_t intensity_val)
Definition: TPoint3D.h:306
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::math::TPoint3Df pt
Definition: TPoint3D.h:332
Base type of all TPoseXX and TPointXX classes in mrpt::math.
Definition: TPoseOrPoint.h:24
TPoint3D_< T > & operator-=(const TPoint3D_< T > &p)
Difference between points.
Definition: TPoint3D.h:194
mrpt::math::TPoint3Df pt
Definition: TPoint3D.h:352
constexpr TPoint3D_< T > operator*(T d) const
Definition: TPoint3D.h:218
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...
Definition: TPoint3D.cpp:59
constexpr T operator[](size_t i) const
Coordinate access using operator[].
Definition: TPoint3D.h:109
void asVector(VECTORLIKE &v) const
Transformation into vector.
Definition: TPoint3D.h:174
mrpt::math::TPoint3Df pt
Definition: TPoint3D.h:314
constexpr TPointXYZRGBu8(double x, double y, double z, uint8_t R_val, uint8_t G_val, uint8_t B_val)
Definition: TPoint3D.h:293
constexpr TPoint3D_< T > operator+(const TPoint3D_< T > &p) const
Points addition.
Definition: TPoint3D.h:204
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, CMatrixD::Ptr &pObj)
constexpr TPoint3D_()
Default constructor.
Definition: TPoint3D.h:47
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[0.02 1.04 -0.8]" )
Definition: TPoint3D.h:236
This base provides a set of functions for maths stuff.
mrpt::math::TPoint3D pt
Definition: TPoint3D.h:279
TPoint3D_< U > cast() const
Return a copy of this object using type U for coordinates.
Definition: TPoint3D.h:86
mrpt::math::TPoint3Df pt
Definition: TPoint3D.h:303
XYZ point (float) + RGB(u8)
Definition: TPoint3D.h:312
XYZ point (float) + RGBA(float) [1-byte memory packed, no padding].
Definition: TPoint3D.h:350
static TPoint3D_< T > FromString(const std::string &s)
Definition: TPoint3D.h:256
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
#define MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(_TYPE, __NS)
Declares a typename to be "type" (without the NS prefix)
Definition: TTypeName.h:128
XYZ point (float) + Intensity(u8)
Definition: TPoint3D.h:301
T sqrDistanceTo(const TPoint3D_< T > &p) const
Point-to-point distance, squared.
Definition: TPoint3D.h:136
XYZ point (float) + RGBA(u8)
Definition: TPoint3D.h:330
TPoint3D_< T > & operator+=(const TPoint3D_< T > &p)
Translation.
Definition: TPoint3D.h:184
constexpr bool operator==(const TPoint2D_< T > &p1, const TPoint2D_< T > &p2)
Exact comparison between 2D points.
Definition: TPoint2D.h:223
Base template for TPoint2D and TPoint2Df.
Definition: TPoint2D.h:32
return_t square(const num_t x)
Inline function for the square of a number.
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
TPoint3D_< T > unitarize() const
Returns this vector with unit length: v/norm(v)
Definition: TPoint3D.h:153
constexpr bool operator!=(const TPoint2D_< T > &p1, const TPoint2D_< T > &p2)
Exact comparison between 2D points.
Definition: TPoint2D.h:230
TPoint3D_< T > & operator*=(const T f)
Scale point/vector.
Definition: TPoint3D.h:163
constexpr TPointXYZRGBAf(float x, float y, float z, float R_val, float G_val, float B_val, float A_val)
Definition: TPoint3D.h:356
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
Provided for STL and matrices/vectors compatibility.
Definition: TPoseOrPoint.h:63
XYZ point (double) + RGB(u8)
Definition: TPoint3D.h:288
mrpt::vision::TStereoCalibResults out
mrpt::serialization::CArchive & operator<<(mrpt::serialization::CArchive &s, const CVectorFloat &a)
Definition: math.cpp:626
Base template for TPoint3D and TPoint3Df.
Definition: TPoint3D.h:37
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
Lightweight 2D pose.
Definition: TPose2D.h:22
constexpr TPoint3D_(T xx, T yy, T zz)
Constructor from coordinates.
Definition: TPoint3D.h:49
T & operator[](size_t i)
Coordinate access using operator[].
Definition: TPoint3D.h:94
T distanceTo(const TPoint3D_< T > &p) const
Point-to-point distance.
Definition: TPoint3D.h:126
constexpr TPointXYZfRGBAu8(float x, float y, float z, uint8_t R_val, uint8_t G_val, uint8_t B_val, uint8_t A_val=0xff)
Definition: TPoint3D.h:335
T norm() const
Point norm: |v| = sqrt(x^2+y^2+z^2)
Definition: TPoint3D.h:150
TPoint3D_< float > TPoint3Df
Definition: TPoint3D.h:269
constexpr TPoint3D_< T > operator-(const TPoint3D_< T > &p) const
Points substraction.
Definition: TPoint3D.h:212
constexpr TPoint3D_< T > operator/(T d) const
Definition: TPoint3D.h:224
constexpr TPointXYZfRGBu8(float x, float y, float z, uint8_t R_val, uint8_t G_val, uint8_t B_val)
Definition: TPoint3D.h:317
XYZ point (double) + Intensity(u8)
Definition: TPoint3D.h:277
std::string asString() const
Definition: TPoint3D.h:242



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: fc2d69e97 Fri Jul 10 18:20:57 2020 +0200 at vie jul 10 18:30:13 CEST 2020