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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce444d842 Fri Dec 6 19:35:10 2019 +0100 at vie dic 6 19:45:12 CET 2019