MRPT  1.9.9
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-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/format.h>
13 #include <mrpt/math/TPoseOrPoint.h>
14 #include <cmath> // sqrt
15 
16 namespace mrpt::math
17 {
18 /** Trivially copiable underlying data for TPoint3D */
19 template <typename T>
21 {
22  /** X,Y,Z coordinates */
23  T x, y, z;
24 };
25 
26 /** Base template for TPoint3D and TPoint3Df
27  * \ingroup geometry_grp
28  */
29 template <typename T>
30 struct TPoint3D_ : public TPoseOrPoint,
31  public TPoint3D_data<T>,
32  public internal::ProvideStaticResize<TPoint3D_<T>>
33 {
34  enum
35  {
37  };
38 
39  /** Default constructor. Initializes to zeros. */
40  constexpr TPoint3D_() : TPoint3D_data<T>{0, 0, 0} {}
41  /** Constructor from coordinates. */
42  constexpr TPoint3D_(T xx, T yy, T zz) : TPoint3D_data<T>{xx, yy, zz} {}
43 
44  /** Constructor from coordinates. */
45  template <typename U>
47  {
48  TPoint3D_data<T>::x = static_cast<T>(p.x);
49  TPoint3D_data<T>::y = static_cast<T>(p.y);
50  TPoint3D_data<T>::z = static_cast<T>(p.z);
51  }
52 
53  /** Implicit constructor from TPoint2D. Zeroes the z.
54  * \sa TPoint2D
55  */
56  TPoint3D_(const TPoint2D_<T>& p);
57  /**
58  * Constructor from TPose2D, losing information. Zeroes the z.
59  * \sa TPose2D
60  */
61  explicit TPoint3D_(const TPose2D& p);
62  /**
63  * Constructor from TPose3D, losing information.
64  * \sa TPose3D
65  */
66  explicit TPoint3D_(const TPose3D& p);
67 
68  /** Return a copy of this object using type U for coordinates */
69  template <typename U>
71  {
72  return TPoint3D_<U>(
73  static_cast<U>(this->x), static_cast<U>(this->y),
74  static_cast<U>(this->z));
75  }
76 
77  /** Coordinate access using operator[]. Order: x,y,z */
78  T& operator[](size_t i)
79  {
80  switch (i)
81  {
82  case 0:
83  return TPoint3D_data<T>::x;
84  case 1:
85  return TPoint3D_data<T>::y;
86  case 2:
87  return TPoint3D_data<T>::z;
88  default:
89  throw std::out_of_range("index out of range");
90  }
91  }
92  /** Coordinate access using operator[]. Order: x,y,z */
93  constexpr T operator[](size_t i) const
94  {
95  switch (i)
96  {
97  case 0:
98  return TPoint3D_data<T>::x;
99  case 1:
100  return TPoint3D_data<T>::y;
101  case 2:
102  return TPoint3D_data<T>::z;
103  default:
104  throw std::out_of_range("index out of range");
105  }
106  }
107  /**
108  * Point-to-point distance.
109  */
110  T distanceTo(const TPoint3D_<T>& p) const
111  {
112  return std::sqrt(
116  }
117  /**
118  * Point-to-point distance, squared.
119  */
120  T sqrDistanceTo(const TPoint3D_<T>& p) const
121  {
122  return square(p.x - TPoint3D_data<T>::x) +
125  }
126  /** Squared norm: |v|^2 = x^2+y^2+z^2 */
127  T sqrNorm() const
128  {
131  }
132 
133  /** Point norm: |v| = sqrt(x^2+y^2+z^2) */
134  T norm() const { return std::sqrt(sqrNorm()); }
135 
136  /** Scale point/vector */
138  {
139  TPoint3D_data<T>::x *= f;
140  TPoint3D_data<T>::y *= f;
141  TPoint3D_data<T>::z *= f;
142  return *this;
143  }
144  /**
145  * Transformation into vector.
146  */
147  template <class VECTORLIKE>
148  void asVector(VECTORLIKE& v) const
149  {
150  v.resize(3);
151  v[0] = TPoint3D_data<T>::x;
152  v[1] = TPoint3D_data<T>::y;
153  v[2] = TPoint3D_data<T>::z;
154  }
155  /**
156  * Translation.
157  */
159  {
160  TPoint3D_data<T>::x += p.x;
161  TPoint3D_data<T>::y += p.y;
162  TPoint3D_data<T>::z += p.z;
163  return *this;
164  }
165  /**
166  * Difference between points.
167  */
169  {
170  TPoint3D_data<T>::x -= p.x;
171  TPoint3D_data<T>::y -= p.y;
172  TPoint3D_data<T>::z -= p.z;
173  return *this;
174  }
175  /**
176  * Points addition.
177  */
178  constexpr TPoint3D_<T> operator+(const TPoint3D_<T>& p) const
179  {
180  return {TPoint3D_data<T>::x + p.x, TPoint3D_data<T>::y + p.y,
181  TPoint3D_data<T>::z + p.z};
182  }
183  /**
184  * Points substraction.
185  */
186  constexpr TPoint3D_<T> operator-(const TPoint3D_<T>& p) const
187  {
188  return {TPoint3D_data<T>::x - p.x, TPoint3D_data<T>::y - p.y,
189  TPoint3D_data<T>::z - p.z};
190  }
191 
192  constexpr TPoint3D_<T> operator*(T d) const
193  {
194  return {TPoint3D_data<T>::x * d, TPoint3D_data<T>::y * d,
195  TPoint3D_data<T>::z * d};
196  }
197 
198  constexpr TPoint3D_<T> operator/(T d) const
199  {
200  return {TPoint3D_data<T>::x / d, TPoint3D_data<T>::y / d,
201  TPoint3D_data<T>::z / d};
202  }
203 
204  bool operator<(const TPoint3D_<T>& p) const;
205 
206  /** Returns a human-readable textual representation of the object (eg:
207  * "[0.02 1.04 -0.8]" )
208  * \sa fromString
209  */
210  void asString(std::string& s) const
211  {
212  s = mrpt::format(
215  }
216  std::string asString() const
217  {
218  std::string s;
219  asString(s);
220  return s;
221  }
222 
223  /** Set the current object value from a string generated by 'asString' (eg:
224  * "[0.02 1.04 -0.8]" )
225  * \sa asString
226  * \exception std::exception On invalid format
227  */
228  void fromString(const std::string& s);
229 
230  static TPoint3D_<T> FromString(const std::string& s)
231  {
232  TPoint3D_<T> o;
233  o.fromString(s);
234  return o;
235  }
236 };
237 
238 /**
239  * Lightweight 3D point. Allows coordinate access using [] operator.
240  * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3Df
241  */
244 
245 /** Useful type alias for 3-vectors */
248 
249 /** Useful type alias for 2-vectors */
251 
252 /** XYZ point (double) + Intensity(u8) \sa mrpt::math::TPoint3D */
254 {
256  uint8_t intensity{0};
257  TPointXYZIu8() : pt() {}
258  constexpr TPointXYZIu8(double x, double y, double z, uint8_t intensity_val)
259  : pt(x, y, z), intensity(intensity_val)
260  {
261  }
262 };
263 /** XYZ point (double) + RGB(u8) \sa mrpt::math::TPoint3D */
265 {
267  uint8_t R{0}, G{0}, B{0};
268  TPointXYZRGBu8() = default;
269  constexpr TPointXYZRGBu8(
270  double x, double y, double z, uint8_t R_val, uint8_t G_val,
271  uint8_t B_val)
272  : pt(x, y, z), R(R_val), G(G_val), B(B_val)
273  {
274  }
275 };
276 /** XYZ point (float) + Intensity(u8) \sa mrpt::math::TPoint3D */
278 {
280  uint8_t intensity{0};
281  TPointXYZfIu8() = default;
282  constexpr TPointXYZfIu8(float x, float y, float z, uint8_t intensity_val)
283  : pt(x, y, z), intensity(intensity_val)
284  {
285  }
286 };
287 /** XYZ point (float) + RGB(u8) \sa mrpt::math::TPoint3D */
289 {
291  uint8_t R{0}, G{0}, B{0};
293  constexpr TPointXYZfRGBu8(
294  float x, float y, float z, uint8_t R_val, uint8_t G_val, uint8_t B_val)
295  : pt(x, y, z), R(R_val), G(G_val), B(B_val)
296  {
297  }
298 };
299 
300 /** Unary minus operator for 3D points. */
301 template <typename T>
302 constexpr TPoint3D_<T> operator-(const TPoint3D_<T>& p1)
303 {
304  return {-p1.x, -p1.y, -p1.z};
305 }
306 
307 /** Exact comparison between 3D points */
308 template <typename T>
309 constexpr bool operator==(const TPoint3D_<T>& p1, const TPoint3D_<T>& p2)
310 {
311  return (p1.x == p2.x) && (p1.y == p2.y) && (p1.z == p2.z); //-V550
312 }
313 /** Exact comparison between 3D points */
314 template <typename T>
315 constexpr bool operator!=(const TPoint3D_<T>& p1, const TPoint3D_<T>& p2)
316 {
317  return (p1.x != p2.x) || (p1.y != p2.y) || (p1.z != p2.z); //-V550
318 }
319 
320 } // namespace mrpt::math
321 
322 namespace mrpt::typemeta
323 {
324 // Specialization must occur in the same namespace
327 } // namespace mrpt::typemeta
T sqrNorm() const
Squared norm: |v|^2 = x^2+y^2+z^2.
Definition: TPoint3D.h:127
std::vector< T1 > operator-(const std::vector< T1 > &v1, const std::vector< T2 > &v2)
Definition: ops_vectors.h:92
constexpr TPointXYZIu8(double x, double y, double z, uint8_t intensity_val)
Definition: TPoint3D.h:258
TPoint3D_(const TPoint3D_data< U > &p)
Constructor from coordinates.
Definition: TPoint3D.h:46
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:204
Trivially copiable underlying data for TPoint3D.
Definition: TPoint3D.h:20
mrpt::math::TPoint3D pt
Definition: TPoint3D.h:266
constexpr TPointXYZfIu8(float x, float y, float z, uint8_t intensity_val)
Definition: TPoint3D.h:282
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
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:168
constexpr TPoint3D_< T > operator*(T d) const
Definition: TPoint3D.h:192
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:93
void asVector(VECTORLIKE &v) const
Transformation into vector.
Definition: TPoint3D.h:148
mrpt::math::TPoint3Df pt
Definition: TPoint3D.h:290
constexpr TPointXYZRGBu8(double x, double y, double z, uint8_t R_val, uint8_t G_val, uint8_t B_val)
Definition: TPoint3D.h:269
constexpr TPoint3D_< T > operator+(const TPoint3D_< T > &p) const
Points addition.
Definition: TPoint3D.h:178
constexpr TPoint3D_()
Default constructor.
Definition: TPoint3D.h:40
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:210
This base provides a set of functions for maths stuff.
mrpt::math::TPoint3D pt
Definition: TPoint3D.h:255
T square(const T x)
Inline function for the square of a number.
TPoint3D_< U > cast() const
Return a copy of this object using type U for coordinates.
Definition: TPoint3D.h:70
mrpt::math::TPoint3Df pt
Definition: TPoint3D.h:279
XYZ point (float) + RGB(u8)
Definition: TPoint3D.h:288
static TPoint3D_< T > FromString(const std::string &s)
Definition: TPoint3D.h:230
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:242
#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:277
T sqrDistanceTo(const TPoint3D_< T > &p) const
Point-to-point distance, squared.
Definition: TPoint3D.h:120
TPoint3D_< T > & operator+=(const TPoint3D_< T > &p)
Translation.
Definition: TPoint3D.h:158
constexpr bool operator==(const TPoint2D_< T > &p1, const TPoint2D_< T > &p2)
Exact comparison between 2D points.
Definition: TPoint2D.h:209
Base template for TPoint2D and TPoint2Df.
Definition: TPoint2D.h:31
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:23
constexpr bool operator!=(const TPoint2D_< T > &p1, const TPoint2D_< T > &p2)
Exact comparison between 2D points.
Definition: TPoint2D.h:216
TPoint3D_< T > & operator*=(const T f)
Scale point/vector.
Definition: TPoint3D.h:137
Provided for STL and matrices/vectors compatibility.
Definition: TPoseOrPoint.h:63
XYZ point (double) + RGB(u8)
Definition: TPoint3D.h:264
Base template for TPoint3D and TPoint3Df.
Definition: TPoint3D.h:30
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:42
T & operator[](size_t i)
Coordinate access using operator[].
Definition: TPoint3D.h:78
T distanceTo(const TPoint3D_< T > &p) const
Point-to-point distance.
Definition: TPoint3D.h:110
T norm() const
Point norm: |v| = sqrt(x^2+y^2+z^2)
Definition: TPoint3D.h:134
TPoint3D_< float > TPoint3Df
Definition: TPoint3D.h:243
constexpr TPoint3D_< T > operator-(const TPoint3D_< T > &p) const
Points substraction.
Definition: TPoint3D.h:186
constexpr TPoint3D_< T > operator/(T d) const
Definition: TPoint3D.h:198
constexpr TPointXYZfRGBu8(float x, float y, float z, uint8_t R_val, uint8_t G_val, uint8_t B_val)
Definition: TPoint3D.h:293
XYZ point (double) + Intensity(u8)
Definition: TPoint3D.h:253
std::string asString() const
Definition: TPoint3D.h:216



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: d1962bc6a Wed Jan 15 17:38:30 2020 +0100 at miƩ ene 15 17:45:11 CET 2020