Main MRPT website > C++ reference for MRPT 1.4.0
lightweight_geom_data.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6 | See: http://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See details in http://www.mrpt.org/License |
8 +---------------------------------------------------------------------------+ */
9#ifndef LIGHTWEIGHT_GEOM_DATA_H
10#define LIGHTWEIGHT_GEOM_DATA_H
11
13#include <mrpt/config.h>
17#include <mrpt/math/math_frwds.h> // forward declarations
18#include <vector>
19#include <stdexcept>
20
21namespace mrpt {
22namespace math {
23 using mrpt::utils::square; //!< Allow square() to be also available under mrpt::math, which makes sense
24
25 /** \addtogroup geometry_grp
26 * @{ */
27
28 //Set of typedefs for lightweight geometric items.
29 /**
30 * Lightweight 2D point. Allows coordinate access using [] operator.
31 * \sa mrpt::poses::CPoint2D
32 */
34 enum { static_size = 2 };
35 double x, y; //!< X,Y coordinates
36 /** Constructor from TPose2D, discarding phi.
37 * \sa TPose2D
38 */
39 explicit TPoint2D(const TPose2D &p);
40 /**
41 * Constructor from TPoint3D, discarding z.
42 * \sa TPoint3D
43 */
44 explicit TPoint2D(const TPoint3D &p);
45 /**
46 * Constructor from TPose3D, discarding z and the angular coordinates.
47 * \sa TPose3D
48 */
49 explicit TPoint2D(const TPose3D &p);
50 /**
51 * Constructor from CPoseOrPoint, perhaps losing 3D information
52 * \sa CPoseOrPoint mrpt::poses::CPoint3D,CPose2D,CPose3D
53 */
54 template <class DERIVEDCLASS>
55 explicit TPoint2D(const mrpt::poses::CPoseOrPoint<DERIVEDCLASS> &p) :x(p.x()),y(p.y()) {}
56
57 /** Implicit transformation constructor from TPixelCoordf */
58 inline TPoint2D(const mrpt::utils::TPixelCoordf &p) :x(p.x),y(p.y) {}
59
60 /** Implicit constructor from mrpt::poses::CPoint2D */
62 /**
63 * Constructor from coordinates.
64 */
65 inline TPoint2D(double xx,double yy):x(xx),y(yy) {}
66 /**
67 * Default fast constructor. Initializes to garbage.
68 */
69 inline TPoint2D() {}
70 /** Coordinate access using operator[]. Order: x,y */
71 inline double &operator[](size_t i) { switch (i) { case 0: return x; case 1: return y; default: throw std::out_of_range("index out of range"); } }
72 /** Coordinate access using operator[]. Order: x,y */
73 inline const double &operator[](size_t i) const { switch (i) { case 0: return x; case 1: return y; default: throw std::out_of_range("index out of range"); } }
74 /**
75 * Transformation into vector.
76 */
77 inline void getAsVector(std::vector<double> &v) const {
78 v.resize(2);
79 v[0]=x; v[1]=y;
80 }
81
82 bool operator<(const TPoint2D &p) const;
83
84 inline TPoint2D &operator+=(const TPoint2D &p) {
85 x+=p.x;
86 y+=p.y;
87 return *this;
88 }
89
90 inline TPoint2D &operator-=(const TPoint2D &p) {
91 x-=p.x;
92 y-=p.y;
93 return *this;
94 }
95
96 inline TPoint2D &operator*=(double d) {
97 x*=d;
98 y*=d;
99 return *this;
100 }
101
102 inline TPoint2D &operator/=(double d) {
103 x/=d;
104 y/=d;
105 return *this;
106 }
107
108 inline TPoint2D operator+(const TPoint2D &p) const {
109 TPoint2D r(*this);
110 return r+=p;
111 }
112
113 inline TPoint2D operator-(const TPoint2D &p) const {
114 TPoint2D r(*this);
115 return r-=p;
116 }
117
118 inline TPoint2D operator*(double d) const {
119 TPoint2D r(*this);
120 return r*=d;
121 }
122
123 inline TPoint2D operator/(double d) const {
124 TPoint2D r(*this);
125 return r/=d;
126 }
127 /** Returns a human-readable textual representation of the object (eg: "[0.02 1.04]" )
128 * \sa fromString
129 */
130 void asString(std::string &s) const { s = mrpt::format("[%f %f]",x,y); }
131 inline std::string asString() const { std::string s; asString(s); return s; }
132
133 /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04]" )
134 * \sa asString
135 * \exception std::exception On invalid format
136 */
137 void fromString(const std::string &s);
138 static size_t size() { return 2; }
139
140 /** Point norm. */
141 inline double norm() const { return sqrt(square(x)+square(y)); }
142 };
143
144 /**
145 * Lightweight 2D pose. Allows coordinate access using [] operator.
146 * \sa mrpt::poses::CPose2D
147 */
149 enum { static_size = 3 };
150 double x,y; //!< X,Y coordinates
151 double phi; //!< Orientation (rads)
152 /** Implicit constructor from TPoint2D. Zeroes the phi coordinate.
153 * \sa TPoint2D
154 */
155 TPose2D(const TPoint2D &p);
156 /**
157 * Constructor from TPoint3D, losing information. Zeroes the phi coordinate.
158 * \sa TPoint3D
159 */
160 explicit TPose2D(const TPoint3D &p);
161 /**
162 * Constructor from TPose3D, losing information. The phi corresponds to the original pose's yaw.
163 * \sa TPose3D
164 */
165 explicit TPose2D(const TPose3D &p);
166 /**
167 * Implicit constructor from heavyweight type.
168 * \sa mrpt::poses::CPose2D
169 */
171 /**
172 * Constructor from coordinates.
173 */
174 inline TPose2D(double xx,double yy,double pphi):x(xx),y(yy),phi(pphi) {}
175 /**
176 * Default fast constructor. Initializes to garbage.
177 */
178 inline TPose2D() {}
179 /** Coordinate access using operator[]. Order: x,y,phi */
180 inline double &operator[](size_t i) { switch (i) { case 0: return x; case 1: return y; case 2: return phi; default: throw std::out_of_range("index out of range"); } }
181 /** Coordinate access using operator[]. Order: x,y,phi */
182 inline const double &operator[](size_t i) const { switch (i) { case 0: return x; case 1: return y; case 2: return phi; default: throw std::out_of_range("index out of range"); } }
183 /**
184 * Transformation into vector.
185 */
186 inline void getAsVector(std::vector<double> &v) const {
187 v.resize(3);
188 v[0]=x; v[1]=y; v[2]=phi;
189 }
190 /** Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees)
191 * \sa fromString
192 */
193 void asString(std::string &s) const;
194 inline std::string asString() const { std::string s; asString(s); return s; }
195
196 /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -45.0]" )
197 * \sa asString
198 * \exception std::exception On invalid format
199 */
200 void fromString(const std::string &s);
201 static size_t size() { return 3; }
202 };
203
204 /** Lightweight 3D point (float version).
205 * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3D
206 */
208 {
209 enum { static_size = 3 };
210 float x;
211 float y;
212 float z;
213
214 inline TPoint3Df() { }
215 inline TPoint3Df(const float xx,const float yy,const float zz) : x(xx), y(yy),z(zz) { }
216 inline TPoint3Df & operator +=(const TPoint3Df &p) { x+=p.x; y+=p.y; z+=p.z; return *this; }
217 inline TPoint3Df operator *(const float s) { return TPoint3Df(x*s,y*s,z*s); }
218 /** Coordinate access using operator[]. Order: x,y,z */
219 inline float &operator[](size_t i) { switch (i) { case 0: return x; case 1: return y; case 2: return z; default: throw std::out_of_range("index out of range"); } }
220
221 /** Coordinate access using operator[]. Order: x,y,z */
222 inline const float &operator[](size_t i) const { switch (i) { case 0: return x; case 1: return y; case 2: return z; default: throw std::out_of_range("index out of range"); } }
223 };
224
225 /**
226 * Lightweight 3D point. Allows coordinate access using [] operator.
227 * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3Df
228 */
230 enum { static_size = 3 };
231 double x,y,z; //!< X,Y,Z coordinates
232
233 /** Constructor from coordinates. */
234 inline TPoint3D(double xx,double yy,double zz):x(xx),y(yy),z(zz) {}
235 /** Default fast constructor. Initializes to garbage. */
236 inline TPoint3D() {}
237 /** Explicit constructor from coordinates. */
238 explicit inline TPoint3D(const TPoint3Df &p):x(p.x),y(p.y),z(p.z) {}
239
240 /** Implicit constructor from TPoint2D. Zeroes the z.
241 * \sa TPoint2D
242 */
244 /**
245 * Constructor from TPose2D, losing information. Zeroes the z.
246 * \sa TPose2D
247 */
248 explicit TPoint3D(const TPose2D &p);
249 /**
250 * Constructor from TPose3D, losing information.
251 * \sa TPose3D
252 */
253 explicit TPoint3D(const TPose3D &p);
254 /**
255 * Implicit constructor from heavyweight type.
256 * \sa mrpt::poses::CPoint3D
257 */
259 /**
260 * Constructor from heavyweight 3D pose.
261 * \sa mrpt::poses::CPose3D.
262 */
263 explicit TPoint3D(const mrpt::poses::CPose3D &p);
264 /** Coordinate access using operator[]. Order: x,y,z */
265 inline double &operator[](size_t i) { switch (i) { case 0: return x; case 1: return y; case 2: return z; default: throw std::out_of_range("index out of range"); } }
266 /** Coordinate access using operator[]. Order: x,y,z */
267 inline const double &operator[](size_t i) const { switch (i) { case 0: return x; case 1: return y; case 2: return z; default: throw std::out_of_range("index out of range"); } }
268 /**
269 * Point-to-point distance.
270 */
271 inline double distanceTo(const TPoint3D &p) const {
272 return sqrt(square(p.x-x)+square(p.y-y)+square(p.z-z));
273 }
274 /**
275 * Point-to-point distance, squared.
276 */
277 inline double sqrDistanceTo(const TPoint3D &p) const {
278 return square(p.x-x)+square(p.y-y)+square(p.z-z);
279 }
280 /**
281 * Point norm.
282 */
283 inline double norm() const {
284 return sqrt(square(x)+square(y)+square(z));
285 }
286 /**
287 * Point scale.
288 */
289 inline TPoint3D &operator*=(const double f) {
290 x*=f;y*=f;z*=f;
291 return *this;
292 }
293 /**
294 * Transformation into vector.
295 */
296 template <class VECTORLIKE>
297 void getAsVector(VECTORLIKE &v) const {
298 v.resize(3);
299 v[0]=x; v[1]=y; v[2]=z;
300 }
301 /**
302 * Translation.
303 */
304 inline TPoint3D &operator+=(const TPoint3D &p) {
305 x+=p.x;
306 y+=p.y;
307 z+=p.z;
308 return *this;
309 }
310 /**
311 * Difference between points.
312 */
313 inline TPoint3D &operator-=(const TPoint3D &p) {
314 x-=p.x;
315 y-=p.y;
316 z-=p.z;
317 return *this;
318 }
319 /**
320 * Points addition.
321 */
322 inline TPoint3D operator+(const TPoint3D &p) const {
323 return TPoint3D(x+p.x,y+p.y,z+p.z);
324 }
325 /**
326 * Points substraction.
327 */
328 inline TPoint3D operator-(const TPoint3D &p) const {
329 return TPoint3D(x-p.x,y-p.y,z-p.z);
330 }
331
332 inline TPoint3D operator*(double d) const {
333 return TPoint3D(x*d,y*d,z*d);
334 }
335
336 inline TPoint3D operator/(double d) const {
337 return TPoint3D(x/d,y/d,z/d);
338 }
339
340 bool operator<(const TPoint3D &p) const;
341
342 /** Returns a human-readable textual representation of the object (eg: "[0.02 1.04 -0.8]" )
343 * \sa fromString
344 */
345 void asString(std::string &s) const { s = mrpt::format("[%f %f %f]",x,y,z); }
346 inline std::string asString() const { std::string s; asString(s); return s; }
347
348 /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" )
349 * \sa asString
350 * \exception std::exception On invalid format
351 */
352 void fromString(const std::string &s);
353 static size_t size() { return 3; }
354 };
355
356 /** XYZ point (double) + Intensity(u8) \sa mrpt::math::TPoint3D */
359 uint8_t intensity;
360 inline TPointXYZIu8() : pt(), intensity(0) {}
361 inline TPointXYZIu8(double x,double y,double z, uint8_t intensity_val) : pt(x,y,z),intensity(intensity_val) {}
362 };
363 /** XYZ point (double) + RGB(u8) \sa mrpt::math::TPoint3D */
366 uint8_t R,G,B;
367 inline TPointXYZRGBu8() : pt(), R(0),G(0),B(0) {}
368 inline TPointXYZRGBu8(double x,double y,double z, uint8_t R_val, uint8_t G_val, uint8_t B_val) : pt(x,y,z),R(R_val),G(G_val),B(B_val) {}
369 };
370 /** XYZ point (float) + Intensity(u8) \sa mrpt::math::TPoint3D */
373 uint8_t intensity;
374 inline TPointXYZfIu8() : pt(), intensity(0) {}
375 inline TPointXYZfIu8(float x,float y,float z, uint8_t intensity_val) : pt(x,y,z),intensity(intensity_val) {}
376 };
377 /** XYZ point (float) + RGB(u8) \sa mrpt::math::TPoint3D */
380 uint8_t R,G,B;
381 inline TPointXYZfRGBu8() : pt(), R(0),G(0),B(0) {}
382 inline TPointXYZfRGBu8(float x,float y,float z, uint8_t R_val, uint8_t G_val, uint8_t B_val) : pt(x,y,z),R(R_val),G(G_val),B(B_val) {}
383 };
384
385 /**
386 * Lightweight 3D pose (three spatial coordinates, plus three angular coordinates). Allows coordinate access using [] operator.
387 * \sa mrpt::poses::CPose3D
388 */
390 enum { static_size = 6 };
391 double x,y,z; //!< X,Y,Z, coords
392 double yaw; //!< Yaw coordinate (rotation angle over Z axis).
393 double pitch; //!< Pitch coordinate (rotation angle over Y axis).
394 double roll; //!< Roll coordinate (rotation angle over X coordinate).
395 /** Implicit constructor from TPoint2D. Zeroes all the unprovided information.
396 * \sa TPoint2D
397 */
398 TPose3D(const TPoint2D &p);
399 /**
400 * Implicit constructor from TPose2D. Gets the yaw from the 2D pose's phi, zeroing all the unprovided information.
401 * \sa TPose2D
402 */
403 TPose3D(const TPose2D &p);
404 /**
405 * Implicit constructor from TPoint3D. Zeroes angular information.
406 * \sa TPoint3D
407 */
408 TPose3D(const TPoint3D &p);
409 /**
410 * Implicit constructor from heavyweight type.
411 * \sa mrpt::poses::CPose3D
412 */
414 /**
415 * Constructor from coordinates.
416 */
417 TPose3D(double _x,double _y,double _z,double _yaw,double _pitch,double _roll):x(_x),y(_y),z(_z),yaw(_yaw),pitch(_pitch),roll(_roll) {}
418 /**
419 * Default fast constructor. Initializes to garbage.
420 */
421 inline TPose3D() {}
422 /** Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll */
423 inline double &operator[](size_t i) { switch (i) { case 0: return x; case 1: return y; case 2: return z; case 3: return yaw; case 4: return pitch; case 5: return roll; default: throw std::out_of_range("index out of range"); } }
424 /** Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll */
425 inline const double &operator[](size_t i) const { switch (i) { case 0: return x; case 1: return y; case 2: return z; case 3: return yaw; case 4: return pitch; case 5: return roll; default: throw std::out_of_range("index out of range"); } }
426 /**
427 * Pose's spatial coordinates norm.
428 */
429 double norm() const {
430 return sqrt(square(x)+square(y)+square(z));
431 }
432 /**
433 * Gets the pose as a vector of doubles.
434 */
435 void getAsVector(std::vector<double> &v) const {
436 v.resize(6);
437 v[0]=x; v[1]=y; v[2]=z; v[3]=yaw; v[4]=pitch; v[5]=roll;
438 }
439 /** Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]", angles in degrees.)
440 * \sa fromString
441 */
442 void asString(std::string &s) const;
443 inline std::string asString() const { std::string s; asString(s); return s; }
444
445 /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" )
446 * \sa asString
447 * \exception std::exception On invalid format
448 */
449 void fromString(const std::string &s);
450 static size_t size() { return 6; }
451 };
452
453 /** Lightweight 3D pose (three spatial coordinates, plus a quaternion ). Allows coordinate access using [] operator.
454 * \sa mrpt::poses::CPose3DQuat
455 */
457 enum { static_size = 7 };
458 double x,y,z; //!< Translation in x,y,z
459 double qr,qx,qy,qz; //!< Unit quaternion part, qr,qx,qy,qz
460
461 /** Constructor from coordinates. */
462 inline TPose3DQuat(double _x,double _y,double _z,double _qr,double _qx, double _qy, double _qz):x(_x),y(_y),z(_z),qr(_qr),qx(_qx),qy(_qy),qz(_qz) { }
463 /** Default fast constructor. Initializes to garbage. */
464 inline TPose3DQuat() {}
465 /** Constructor from a CPose3DQuat */
467
468 /** Coordinate access using operator[]. Order: x,y,z,qr,qx,qy,qz */
469 inline double &operator[](size_t i) { switch (i) { case 0: return x; case 1: return y; case 2: return z; case 3: return qr; case 4: return qx; case 5: return qy; case 6: return qz; default: throw std::out_of_range("index out of range"); } }
470 /** Coordinate access using operator[]. Order: x,y,z,qr,qx,qy,qz */
471 inline const double &operator[](size_t i) const { switch (i) { case 0: return x; case 1: return y; case 2: return z; case 3: return qr; case 4: return qx; case 5: return qy; case 6: return qz; default: throw std::out_of_range("index out of range"); } }
472 /** Pose's spatial coordinates norm. */
473 double norm() const {
474 return sqrt(square(x)+square(y)+square(z));
475 }
476 /** Gets the pose as a vector of doubles. */
477 void getAsVector(std::vector<double> &v) const {
478 v.resize(7);
479 for (size_t i=0;i<7;i++) v[i]=(*this)[i];
480 }
481 /** Returns a human-readable textual representation of the object as "[x y z qr qx qy qz]"
482 * \sa fromString
483 */
484 void asString(std::string &s) const { s = mrpt::format("[%f %f %f %f %f %f %f]",x,y,z,qr,qx,qy,qz); }
485 inline std::string asString() const { std::string s; asString(s); return s; }
486
487 /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8 1.0 0.0 0.0 0.0]" )
488 * \sa asString
489 * \exception std::exception On invalid format
490 */
491 void fromString(const std::string &s);
492 static size_t size() { return 7; }
493 };
494
495 // Text streaming functions:
496 std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPoint2D & p);
497 std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPoint3D & p);
498 std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose2D & p);
499 std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose3D & p);
500 std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose3DQuat & p);
501
502
503 /**
504 * Unary minus operator for 3D points.
505 */
506 inline TPoint3D operator-(const TPoint3D &p1) {
507 return TPoint3D(-p1.x,-p1.y,-p1.z);
508 }
509 /**
510 * Exact comparison between 2D points.
511 */
512 inline bool operator==(const TPoint2D &p1,const TPoint2D &p2) {
513 return (p1.x==p2.x)&&(p1.y==p2.y); //-V550
514 }
515 /**
516 * Exact comparison between 2D points.
517 */
518 inline bool operator!=(const TPoint2D &p1,const TPoint2D &p2) {
519 return (p1.x!=p2.x)||(p1.y!=p2.y); //-V550
520 }
521 /**
522 * Exact comparison between 3D points.
523 */
524 inline bool operator==(const TPoint3D &p1,const TPoint3D &p2) {
525 return (p1.x==p2.x)&&(p1.y==p2.y)&&(p1.z==p2.z); //-V550
526 }
527 /**
528 * Exact comparison between 3D points.
529 */
530 inline bool operator!=(const TPoint3D &p1,const TPoint3D &p2) {
531 return (p1.x!=p2.x)||(p1.y!=p2.y)||(p1.z!=p2.z); //-V550
532 }
533 /**
534 * Exact comparison between 2D poses, taking possible cycles into account.
535 */
536 inline bool operator==(const TPose2D &p1,const TPose2D &p2) {
537 return (p1.x==p2.x)&&(p1.y==p2.y)&&(mrpt::math::wrapTo2Pi(p1.phi)==mrpt::math::wrapTo2Pi(p2.phi)); //-V550
538 }
539 /**
540 * Exact comparison between 2D poses, taking possible cycles into account.
541 */
542 inline bool operator!=(const TPose2D &p1,const TPose2D &p2) {
543 return (p1.x!=p2.x)||(p1.y!=p2.y)||(mrpt::math::wrapTo2Pi(p1.phi)!=mrpt::math::wrapTo2Pi(p2.phi)); //-V550
544 }
545 /**
546 * Exact comparison between 3D poses, taking possible cycles into account.
547 */
548 inline bool operator==(const TPose3D &p1,const TPose3D &p2) {
550 }
551 /**
552 * Exact comparison between 3D poses, taking possible cycles into account.
553 */
554 inline bool operator!=(const TPose3D &p1,const TPose3D &p2) {
556 }
557 //Forward declarations
562
563 //Pragma defined to ensure no structure packing
564 /**
565 * 2D segment, consisting of two points.
566 * \sa TSegment3D,TLine2D,TPolygon2D,TPoint2D
567 */
569 public:
570 /**
571 * Origin point.
572 */
574 /**
575 * Destiny point.
576 */
578 /**
579 * Segment length.
580 */
581 double length() const;
582 /**
583 * Distance to point.
584 */
585 double distance(const TPoint2D &point) const;
586 /**
587 * Distance with sign to point (sign indicates which side the point is).
588 */
589 double signedDistance(const TPoint2D &point) const;
590 /**
591 * Check whether a point is inside a segment.
592 */
593 bool contains(const TPoint2D &point) const;
594 /** Access to points using operator[0-1] */
595 inline TPoint2D &operator[](size_t i) { switch (i) { case 0: return point1; case 1: return point2; default: throw std::out_of_range("index out of range"); } }
596 /** Access to points using operator[0-1] */
597 inline const TPoint2D &operator[](size_t i) const { switch (i) { case 0: return point1; case 1: return point2; default: throw std::out_of_range("index out of range"); } }
598 /**
599 * Project into 3D space, setting the z to 0.
600 */
602 /**
603 * Segment's central point.
604 */
605 inline void getCenter(TPoint2D &p) const {
606 p.x=(point1.x+point2.x)/2;
607 p.y=(point1.y+point2.y)/2;
608 }
609 /**
610 * Constructor from both points.
611 */
612 TSegment2D(const TPoint2D &p1,const TPoint2D &p2):point1(p1),point2(p2) {}
613 /**
614 * Fast default constructor. Initializes to garbage.
615 */
617 /**
618 * Explicit constructor from 3D object, discarding the z.
619 */
620 explicit TSegment2D(const TSegment3D &s);
621
622 bool operator<(const TSegment2D &s) const;
623 };
624 /**
625 * 3D segment, consisting of two points.
626 * \sa TSegment2D,TLine3D,TPlane,TPolygon3D,TPoint3D
627 */
629 public:
630 /**
631 * Origin point.
632 */
634 /**
635 * Destiny point.
636 */
638 /**
639 * Segment length.
640 */
641 double length() const;
642 /**
643 * Distance to point.
644 */
645 double distance(const TPoint3D &point) const;
646 /**
647 * Distance to another segment.
648 */
649 double distance(const TSegment3D &segment) const;
650 /**
651 * Check whether a point is inside the segment.
652 */
653 bool contains(const TPoint3D &point) const;
654 /** Access to points using operator[0-1] */
655 inline TPoint3D &operator[](size_t i) { switch (i) { case 0: return point1; case 1: return point2; default: throw std::out_of_range("index out of range"); } }
656 /** Access to points using operator[0-1] */
657 inline const TPoint3D &operator[](size_t i) const { switch (i) { case 0: return point1; case 1: return point2; default: throw std::out_of_range("index out of range"); } }
658 /**
659 * Projection into 2D space, discarding the z.
660 */
661 inline void generate2DObject(TSegment2D &s) const {
662 s=TSegment2D(*this);
663 }
664 /**
665 * Segment's central point.
666 */
667 inline void getCenter(TPoint3D &p) const {
668 p.x=(point1.x+point2.x)/2;
669 p.y=(point1.y+point2.y)/2;
670 p.z=(point1.z+point2.z)/2;
671 }
672 /**
673 * Constructor from both points.
674 */
675 TSegment3D(const TPoint3D &p1,const TPoint3D &p2):point1(p1),point2(p2) {}
676 /**
677 * Fast default constructor. Initializes to garbage.
678 */
680 /**
681 * Constructor from 2D object. Sets the z to zero.
682 */
683 TSegment3D(const TSegment2D &s):point1(s.point1),point2(s.point2) {}
684
685 bool operator<(const TSegment3D &s) const;
686 };
687
688 inline bool operator==(const TSegment2D &s1,const TSegment2D &s2) {
689 return (s1.point1==s2.point1)&&(s1.point2==s2.point2);
690 }
691
692 inline bool operator!=(const TSegment2D &s1,const TSegment2D &s2) {
693 return (s1.point1!=s2.point1)||(s1.point2!=s2.point2);
694 }
695
696 inline bool operator==(const TSegment3D &s1,const TSegment3D &s2) {
697 return (s1.point1==s2.point1)&&(s1.point2==s2.point2);
698 }
699
700 inline bool operator!=(const TSegment3D &s1,const TSegment3D &s2) {
701 return (s1.point1!=s2.point1)||(s1.point2!=s2.point2);
702 }
703
704 /**
705 * 2D line without bounds, represented by its equation \f$Ax+By+C=0\f$.
706 * \sa TLine3D,TSegment2D,TPolygon2D,TPoint2D
707 */
709 public:
710 /**
711 * Line coefficients, stored as an array: \f$\left[A,B,C\right]\f$.
712 */
713 double coefs[3];
714 /**
715 * Evaluate point in the line's equation.
716 */
717 double evaluatePoint(const TPoint2D &point) const;
718 /**
719 * Check whether a point is inside the line.
720 */
721 bool contains(const TPoint2D &point) const;
722 /**
723 * Distance from a given point.
724 */
725 double distance(const TPoint2D &point) const;
726 /**
727 * Distance with sign from a given point (sign indicates side).
728 */
729 double signedDistance(const TPoint2D &point) const;
730 /**
731 * Get line's normal vector.
732 */
733 void getNormalVector(double (&vector)[2]) const;
734 /**
735 * Unitarize line's normal vector.
736 */
737 void unitarize();
738 /**
739 * Get line's normal vector after unitarizing line.
740 */
741 inline void getUnitaryNormalVector(double (&vector)[2]) {
742 unitarize();
743 getNormalVector(vector);
744 }
745 /**
746 * Get line's director vector.
747 */
748 void getDirectorVector(double (&vector)[2]) const;
749 /**
750 * Unitarize line and then get director vector.
751 */
752 inline void getUnitaryDirectorVector(double (&vector)[2]) {
753 unitarize();
754 getDirectorVector(vector);
755 }
756 /**
757 * Project into 3D space, setting the z to 0.
758 */
759 void generate3DObject(TLine3D &l) const;
760 /**
761 * Get a pose2D whose X axis corresponds to the line.
762 * \sa mrpt::poses::CPose2D.
763 */
764 void getAsPose2D(mrpt::poses::CPose2D &outPose) const;
765 /**
766 * Get a pose2D whose X axis corresponds to the line, forcing the base point to one given.
767 * \throw logic_error if the point is not inside the line.
768 * \sa mrpt::poses::CPose2D.
769 */
770 void getAsPose2DForcingOrigin(const TPoint2D &origin,mrpt::poses::CPose2D &outPose) const;
771 /**
772 * Constructor from two points, through which the line will pass.
773 * \throw logic_error if both points are the same
774 */
775 TLine2D(const TPoint2D &p1,const TPoint2D &p2) throw(std::logic_error);
776 /**
777 * Constructor from a segment.
778 */
779 explicit TLine2D(const TSegment2D &s);
780 /**
781 * Fast default constructor. Initializes to garbage.
782 */
784 /**
785 * Constructor from line's coefficients.
786 */
787 inline TLine2D(double A,double B,double C) {
788 coefs[0]=A;
789 coefs[1]=B;
790 coefs[2]=C;
791 }
792 /**
793 * Construction from 3D object, discarding the Z.
794 * \throw std::logic_error if the line is normal to the XY plane.
795 */
796 explicit TLine2D(const TLine3D &l);
797 };
798
799 /**
800 * 3D line, represented by a base point and a director vector.
801 * \sa TLine2D,TSegment3D,TPlane,TPolygon3D,TPoint3D
802 */
804 public:
805 /**
806 * Base point.
807 */
809 /**
810 * Director vector.
811 */
812 double director[3];
813 /**
814 * Check whether a point is inside the line.
815 */
816 bool contains(const TPoint3D &point) const;
817 /**
818 * Distance between the line and a point.
819 */
820 double distance(const TPoint3D &point) const;
821 /**
822 * Unitarize director vector.
823 */
824 void unitarize();
825 /**
826 * Get director vector.
827 */
828 inline void getDirectorVector(double (&vector)[3]) const {
829 for (size_t i=0;i<3;i++) vector[i]=director[i];
830 }
831 /**
832 * Unitarize and then get director vector.
833 */
834 inline void getUnitaryDirectorVector(double (&vector)[3]) {
835 unitarize();
836 getDirectorVector(vector);
837 }
838 /**
839 * Project into 2D space, discarding the Z coordinate.
840 * \throw std::logic_error if the line's director vector is orthogonal to the XY plane.
841 */
842 inline void generate2DObject(TLine2D &l) const {
843 l=TLine2D(*this);
844 }
845 /**
846 * Constructor from two points, through which the line will pass.
847 * \throw std::logic_error if both points are the same.
848 */
849 TLine3D(const TPoint3D &p1,const TPoint3D &p2) throw(std::logic_error);
850 /**
851 * Constructor from 3D segment.
852 */
853 explicit TLine3D(const TSegment3D &s);
854 /**
855 * Fast default constructor. Initializes to garbage.
856 */
858 /**
859 * Implicit constructor from 2D object. Zeroes the z.
860 */
861 TLine3D(const TLine2D &l);
862 };
863
864 /**
865 * 3D Plane, represented by its equation \f$Ax+By+Cz+D=0\f$
866 * \sa TSegment3D,TLine3D,TPolygon3D,TPoint3D
867 */
869 public:
870 /**
871 * Plane coefficients, stored as an array: \f$\left[A,B,C,D\right]\f$
872 */
873 double coefs[4];
874 /**
875 * Evaluate a point in the plane's equation.
876 */
877 double evaluatePoint(const TPoint3D &point) const;
878 /**
879 * Check whether a point is contained into the plane.
880 */
881 bool contains(const TPoint3D &point) const;
882 /**
883 * Check whether a segment is fully contained into the plane.
884 */
885 inline bool contains(const TSegment3D &segment) const {
886 return contains(segment.point1)&&contains(segment.point2);
887 }
888 /**
889 * Check whether a line is fully contained into the plane.
890 */
891 bool contains(const TLine3D &line) const;
892 /**
893 * Distance to 3D point.
894 */
895 double distance(const TPoint3D &point) const;
896 /**
897 * Distance to 3D line. Will be zero if the line is not parallel to the plane.
898 */
899 double distance(const TLine3D &line) const;
900 /**
901 * Get plane's normal vector.
902 */
903 void getNormalVector(double (&vec)[3]) const;
904 /**
905 * Unitarize normal vector.
906 */
907 void unitarize();
908 /**
909 * Unitarize, then get normal vector.
910 */
911 inline void getUnitaryNormalVector(double (&vec)[3]) {
912 unitarize();
913 getNormalVector(vec);
914 }
915 /**
916 * Gets a pose whose XY plane corresponds to this plane.
917 */
919 /**
920 * Gets a pose whose XY plane corresponds to this plane.
921 */
922 inline void getAsPose3D(mrpt::poses::CPose3D &outPose) const {
923 TPlane p=*this;
924 p.getAsPose3D(outPose);
925 }
926 /**
927 * Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates.
928 * \throw std::logic_error if the point is not inside the plane.
929 */
931 /**
932 * Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates.
933 * \throw std::logic_error if the point is not inside the plane.
934 */
935 inline void getAsPose3DForcingOrigin(const TPoint3D &newOrigin,mrpt::poses::CPose3D &pose) const {
936 TPlane p=*this;
937 p.getAsPose3DForcingOrigin(newOrigin,pose);
938 }
939 /**
940 * Gets a plane which contains these three points.
941 * \throw std::logic_error if the points are linearly dependants.
942 */
943 TPlane(const TPoint3D &p1,const TPoint3D &p2,const TPoint3D &p3) throw(std::logic_error);
944 /**
945 * Gets a plane which contains this point and this line.
946 * \throw std::logic_error if the point is inside the line.
947 */
948 TPlane(const TPoint3D &p1,const TLine3D &r2) throw(std::logic_error);
949 /**
950 * Gets a plane which contains the two lines.
951 * \throw std::logic_error if the lines do not cross.
952 */
953 TPlane(const TLine3D &r1,const TLine3D &r2) throw(std::logic_error);
954 /**
955 * Fast default constructor. Initializes to garbage.
956 */
958 /**
959 * Constructor from plane coefficients.
960 */
961 inline TPlane(double A,double B,double C,double D) {
962 coefs[0]=A;
963 coefs[1]=B;
964 coefs[2]=C;
965 coefs[3]=D;
966 }
967 /**
968 * Constructor from an array of coefficients.
969 */
970 inline TPlane(const double (&vec)[4]) {
971 for (size_t i=0;i<4;i++) coefs[i]=vec[i];
972 }
973 };
974
976
977 /**
978 * 2D polygon, inheriting from std::vector<TPoint2D>.
979 * \sa TPolygon3D,TSegment2D,TLine2D,TPoint2D, CPolygon
980 */
981 class BASE_IMPEXP TPolygon2D:public std::vector<TPoint2D> {
982 public:
983 /**
984 * Distance to a point.
985 */
986 double distance(const TPoint2D &point) const;
987 /**
988 * Check whether a point is inside the polygon.
989 */
990 bool contains(const TPoint2D &point) const;
991 /**
992 * Gets as set of segments, instead of points.
993 */
994 void getAsSegmentList(std::vector<TSegment2D> &v) const;
995 /**
996 * Projects into 3D space, zeroing the z.
997 */
999 /**
1000 * Polygon's central point.
1001 */
1002 void getCenter(TPoint2D &p) const;
1003 /**
1004 * Checks whether is convex.
1005 */
1006 bool isConvex() const;
1007 /**
1008 * Erase repeated vertices.
1009 * \sa removeRedundantVertices
1010 */
1012 /**
1013 * Erase every redundant vertex from the polygon, saving space.
1014 * \sa removeRepeatedVertices
1015 */
1017 /**
1018 * Gets plot data, ready to use on a 2D plot.
1019 * \sa mrpt::gui::CDisplayWindowPlots
1020 */
1021 void getPlotData(std::vector<double> &x,std::vector<double> &y) const;
1022 /**
1023 * Default constructor.
1024 */
1025 TPolygon2D():std::vector<TPoint2D>() {}
1026 /**
1027 * Constructor for a given number of vertices, intializing them as garbage.
1028 */
1029 explicit TPolygon2D(size_t N):std::vector<TPoint2D>(N) {}
1030 /**
1031 * Implicit constructor from a vector of 2D points.
1032 */
1033 TPolygon2D(const std::vector<TPoint2D> &v):std::vector<TPoint2D>(v) {}
1034 /**
1035 * Constructor from a 3D object.
1036 */
1037 explicit TPolygon2D(const TPolygon3D &p);
1038 /**
1039 * Static method to create a regular polygon, given its size and radius.
1040 * \throw std::logic_error if radius is near zero or the number of edges is less than three.
1041 */
1042 static void createRegularPolygon(size_t numEdges,double radius,TPolygon2D &poly);
1043 /**
1044 * Static method to create a regular polygon from its size and radius. The center will correspond to the given pose.
1045 * \throw std::logic_error if radius is near zero or the number of edges is less than three.
1046 */
1047 static inline void createRegularPolygon(size_t numEdges,double radius,TPolygon2D &poly,const mrpt::poses::CPose2D &pose);
1048 };
1049
1050 /**
1051 * 3D polygon, inheriting from std::vector<TPoint3D>
1052 * \sa TPolygon2D,TSegment3D,TLine3D,TPlane,TPoint3D
1053 */
1054 class BASE_IMPEXP TPolygon3D:public std::vector<TPoint3D> {
1055 public:
1056 /**
1057 * Distance to point.
1058 */
1059 double distance(const TPoint3D &point) const;
1060 /**
1061 * Check whether a point is inside the polygon.
1062 */
1063 bool contains(const TPoint3D &point) const;
1064 /**
1065 * Gets as set of segments, instead of set of points.
1066 */
1067 void getAsSegmentList(std::vector<TSegment3D> &v) const;
1068 /**
1069 * Gets a plane which contains the polygon. Returns false if the polygon is skew and cannot be fit inside a plane.
1070 */
1071 bool getPlane(TPlane &p) const;
1072 /**
1073 * Gets the best fitting plane, disregarding whether the polygon actually fits inside or not.
1074 * \sa getBestFittingPlane
1075 */
1077 /**
1078 * Projects into a 2D space, discarding the z.
1079 * \get getPlane,isSkew
1080 */
1081 inline void generate2DObject(TPolygon2D &p) const {
1082 p=TPolygon2D(*this);
1083 }
1084 /**
1085 * Get polygon's central point.
1086 */
1087 void getCenter(TPoint3D &p) const;
1088 /**
1089 * Check whether the polygon is skew. Returns true if there doesn't exist a plane in which the polygon can fit.
1090 * \sa getBestFittingPlane
1091 */
1092 bool isSkew() const;
1093 /**
1094 * Remove polygon's repeated vertices.
1095 */
1097 /**
1098 * Erase every redundant vertex, thus saving space.
1099 */
1101 /**
1102 * Default constructor. Creates a polygon with no vertices.
1103 */
1104 TPolygon3D():std::vector<TPoint3D>() {}
1105 /**
1106 * Constructor for a given size. Creates a polygon with a fixed number of vertices, which are initialized to garbage.
1107 */
1108 explicit TPolygon3D(size_t N):std::vector<TPoint3D>(N) {}
1109 /**
1110 * Implicit constructor from a 3D points vector.
1111 */
1112 TPolygon3D(const std::vector<TPoint3D> &v):std::vector<TPoint3D>(v) {}
1113 /**
1114 * Constructor from a 2D object. Zeroes the z.
1115 */
1117 /**
1118 * Static method to create a regular polygon, given its size and radius.
1119 * \throw std::logic_error if number of edges is less than three, or radius is near zero.
1120 */
1121 static void createRegularPolygon(size_t numEdges,double radius,TPolygon3D &poly);
1122 /**
1123 * Static method to create a regular polygon, given its size and radius. The center will be located on the given pose.
1124 * \throw std::logic_error if number of edges is less than three, or radius is near zero.
1125 */
1126 static inline void createRegularPolygon(size_t numEdges,double radius,TPolygon3D &poly,const mrpt::poses::CPose3D &pose);
1127 };
1128
1129 /**
1130 * Object type identifier for TPoint2D or TPoint3D.
1131 * \sa TObject2D,TObject3D
1132 */
1133 const unsigned char GEOMETRIC_TYPE_POINT=0;
1134 /**
1135 * Object type identifier for TSegment2D or TSegment3D.
1136 * \sa TObject2D,TObject3D
1137 */
1138 const unsigned char GEOMETRIC_TYPE_SEGMENT=1;
1139 /**
1140 * Object type identifier for TLine2D or TLine3D.
1141 * \sa TObject2D,TObject3D
1142 */
1143 const unsigned char GEOMETRIC_TYPE_LINE=2;
1144 /**
1145 * Object type identifier for TPolygon2D or TPolygon3D.
1146 * \sa TObject2D,TObject3D
1147 */
1148 const unsigned char GEOMETRIC_TYPE_POLYGON=3;
1149 /**
1150 * Object type identifier for TPlane.
1151 * \sa TObject3D
1152 */
1153 const unsigned char GEOMETRIC_TYPE_PLANE=4;
1154 /**
1155 * Object type identifier for empty TObject2D or TObject3D.
1156 * \sa TObject2D,TObject3D
1157 */
1158 const unsigned char GEOMETRIC_TYPE_UNDEFINED=255;
1159
1160 /**
1161 * Standard type for storing any lightweight 2D type. Do not inherit from this class.
1162 * \sa TPoint2D,TSegment2D,TLine2D,TPolygon2D
1163 */
1165 private:
1166 /**
1167 * Object type identifier.
1168 */
1169 unsigned char type;
1170 /**
1171 * Union type storing pointers to every allowed type.
1172 */
1178
1179 tobject2d_data_t() : polygon(NULL) { }
1180 } data;
1181 /**
1182 * Destroys the object, releasing the pointer to the content (if any).
1183 */
1184 inline void destroy() {
1185 if (type==GEOMETRIC_TYPE_POLYGON) delete data.polygon;
1187 }
1188 public:
1189 /**
1190 * Implicit constructor from point.
1191 */
1192 inline TObject2D(const TPoint2D &p):type(GEOMETRIC_TYPE_POINT) {
1193 data.point=p;
1194 }
1195 /**
1196 * Implicit constructor from segment.
1197 */
1199 data.segment=s;
1200 }
1201 /**
1202 * Implicit constructor from line.
1203 */
1204 inline TObject2D(const TLine2D &r):type(GEOMETRIC_TYPE_LINE) {
1205 data.line=r;
1206 }
1207 /**
1208 * Implicit constructor from polygon.
1209 */
1211 data.polygon=new TPolygon2D(p);
1212 }
1213 /**
1214 * Implicit constructor from polygon.
1215 */
1217 /**
1218 * Object destruction.
1219 */
1221 destroy();
1222 }
1223 /**
1224 * Checks whether content is a point.
1225 */
1226 inline bool isPoint() const {
1227 return type==GEOMETRIC_TYPE_POINT;
1228 }
1229 /**
1230 * Checks whether content is a segment.
1231 */
1232 inline bool isSegment() const {
1233 return type==GEOMETRIC_TYPE_SEGMENT;
1234 }
1235 /**
1236 * Checks whether content is a line.
1237 */
1238 inline bool isLine() const {
1239 return type==GEOMETRIC_TYPE_LINE;
1240 }
1241 /**
1242 * Checks whether content is a polygon.
1243 */
1244 inline bool isPolygon() const {
1245 return type==GEOMETRIC_TYPE_POLYGON;
1246 }
1247 /**
1248 * Gets content type.
1249 */
1250 inline unsigned char getType() const {
1251 return type;
1252 }
1253 /**
1254 * Gets the content as a point, returning false if the type is inadequate.
1255 */
1256 inline bool getPoint(TPoint2D &p) const {
1257 if (isPoint()) {
1258 p=data.point;
1259 return true;
1260 } else return false;
1261 }
1262 /**
1263 * Gets the content as a segment, returning false if the type is inadequate.
1264 */
1265 inline bool getSegment(TSegment2D &s) const {
1266 if (isSegment()) {
1267 s=data.segment;
1268 return true;
1269 } else return false;
1270 }
1271 /**
1272 * Gets the content as a line, returning false if the type is inadequate.
1273 */
1274 inline bool getLine(TLine2D &r) const {
1275 if (isLine()) {
1276 r=data.line;
1277 return true;
1278 } else return false;
1279 }
1280 /**
1281 * Gets the content as a polygon, returning false if the type is inadequate.
1282 */
1283 inline bool getPolygon(TPolygon2D &p) const {
1284 if (isPolygon()) {
1285 p=*(data.polygon);
1286 return true;
1287 } else return false;
1288 }
1289 /**
1290 * Assign another TObject2D. Pointers are not shared.
1291 */
1293 if (this==&obj) return *this;
1294 destroy();
1295 switch (type=obj.type) {
1297 data.point=obj.data.point;
1298 break;
1300 data.segment=obj.data.segment;
1301 break;
1303 data.line=obj.data.line;
1304 break;
1306 data.polygon=new TPolygon2D(*(obj.data.polygon));
1307 break;
1308 }
1309 return *this;
1310 }
1311 /**
1312 * Assign a point to this object.
1313 */
1314 inline void operator=(const TPoint2D &p) {
1315 destroy();
1317 data.point=p;
1318 }
1319 /**
1320 * Assign a segment to this object.
1321 */
1322 inline void operator=(const TSegment2D &s) {
1323 destroy();
1325 data.segment=s;
1326 }
1327 /**
1328 * Assign a line to this object.
1329 */
1330 inline void operator=(const TLine2D &l) {
1331 destroy();
1333 data.line=l;
1334 }
1335 /**
1336 * Assign a polygon to this object.
1337 */
1338 inline void operator=(const TPolygon2D &p) {
1339 destroy();
1341 data.polygon=new TPolygon2D(p);
1342 }
1343 /**
1344 * Project into 3D space.
1345 */
1346 void generate3DObject(TObject3D &obj) const;
1347 /**
1348 * Constructor from another TObject2D.
1349 */
1351 operator=(obj);
1352 }
1353 /**
1354 * Static method to retrieve all the points in a vector of TObject2D.
1355 */
1356 static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts);
1357 /**
1358 * Static method to retrieve all the segments in a vector of TObject2D.
1359 */
1360 static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms);
1361 /**
1362 * Static method to retrieve all the lines in a vector of TObject2D.
1363 */
1364 static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins);
1365 /**
1366 * Static method to retrieve all the polygons in a vector of TObject2D.
1367 */
1368 static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys);
1369 /**
1370 * Static method to retrieve all the points in a vector of TObject2D, returning the remainder objects in another parameter.
1371 */
1372 static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts,std::vector<TObject2D> &remainder);
1373 /**
1374 * Static method to retrieve all the segments in a vector of TObject2D, returning the remainder objects in another parameter.
1375 */
1376 static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms,std::vector<TObject2D> &remainder);
1377 /**
1378 * Static method to retrieve all the lines in a vector of TObject2D, returning the remainder objects in another parameter.
1379 */
1380 static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins,std::vector<TObject2D> &remainder);
1381 /**
1382 * Static method to retrieve all the polygons in a vector of TObject2D, returning the remainder objects in another parameter.
1383 */
1384 static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys,std::vector<TObject2D> &remainder);
1385 };
1386 /**
1387 * Standard object for storing any 3D lightweight object. Do not inherit from this class.
1388 * \sa TPoint3D,TSegment3D,TLine3D,TPlane,TPolygon3D
1389 */
1391 private:
1392 /**
1393 * Object type identifier.
1394 */
1395 unsigned char type;
1396 /**
1397 * Union containing pointer to actual data.
1398 */
1405
1406 tobject3d_data_t() : polygon(NULL) { }
1407 } data;
1408 /**
1409 * Destroys the object and releases the pointer, if any.
1410 */
1411 void destroy() {
1412 if (type==GEOMETRIC_TYPE_POLYGON) delete data.polygon;
1414 }
1415 public:
1416 /**
1417 * Constructor from point.
1418 */
1420 data.point=p;
1421 }
1422 /**
1423 * Constructor from segment.
1424 */
1426 data.segment=s;
1427 }
1428 /**
1429 * Constructor from line.
1430 */
1432 data.line=r;
1433 }
1434 /**
1435 * Constructor from polygon.
1436 */
1438 data.polygon=new TPolygon3D(p);
1439 }
1440 /**
1441 * Constructor from plane.
1442 */
1444 data.plane=p;
1445 }
1446 /**
1447 * Empty constructor.
1448 */
1450 /**
1451 * Destructor.
1452 */
1454 destroy();
1455 }
1456 /**
1457 * Checks whether content is a point.
1458 */
1459 inline bool isPoint() const {
1460 return type==GEOMETRIC_TYPE_POINT;
1461 }
1462 /**
1463 * Checks whether content is a segment.
1464 */
1465 inline bool isSegment() const {
1466 return type==GEOMETRIC_TYPE_SEGMENT;
1467 }
1468 /**
1469 * Checks whether content is a line.
1470 */
1471 inline bool isLine() const {
1472 return type==GEOMETRIC_TYPE_LINE;
1473 }
1474 /**
1475 * Checks whether content is a polygon.
1476 */
1477 inline bool isPolygon() const {
1478 return type==GEOMETRIC_TYPE_POLYGON;
1479 }
1480 /**
1481 * Checks whether content is a plane.
1482 */
1483 inline bool isPlane() const {
1484 return type==GEOMETRIC_TYPE_PLANE;
1485 }
1486 /**
1487 * Gets object type.
1488 */
1489 inline unsigned char getType() const {
1490 return type;
1491 }
1492 /**
1493 * Gets the content as a point, returning false if the type is not adequate.
1494 */
1495 inline bool getPoint(TPoint3D &p) const {
1496 if (isPoint()) {
1497 p=data.point;
1498 return true;
1499 } else return false;
1500 }
1501 /**
1502 * Gets the content as a segment, returning false if the type is not adequate.
1503 */
1504 inline bool getSegment(TSegment3D &s) const {
1505 if (isSegment()) {
1506 s=data.segment;
1507 return true;
1508 } else return false;
1509 }
1510 /**
1511 * Gets the content as a line, returning false if the type is not adequate.
1512 */
1513 inline bool getLine(TLine3D &r) const {
1514 if (isLine()) {
1515 r=data.line;
1516 return true;
1517 } else return false;
1518 }
1519 /**
1520 * Gets the content as a polygon, returning false if the type is not adequate.
1521 */
1522 inline bool getPolygon(TPolygon3D &p) const {
1523 if (isPolygon()) {
1524 p=*(data.polygon);
1525 return true;
1526 } else return false;
1527 }
1528 /**
1529 * Gets the content as a plane, returning false if the type is not adequate.
1530 */
1531 inline bool getPlane(TPlane &p) const {
1532 if (isPlane()) {
1533 p=data.plane;
1534 return true;
1535 } else return false;
1536 }
1537 /**
1538 * Assigns another object, creating a new pointer if needed.
1539 */
1541 if (this==&obj) return *this;
1542 destroy();
1543 switch (type=obj.type) {
1545 data.point=obj.data.point;
1546 break;
1548 data.segment=obj.data.segment;
1549 break;
1551 data.line=obj.data.line;
1552 break;
1554 data.polygon=new TPolygon3D(*(obj.data.polygon));
1555 break;
1557 data.plane=obj.data.plane;
1558 break;
1560 break;
1561 default:
1562 THROW_EXCEPTION("Invalid TObject3D object");
1563 }
1564 return *this;
1565 }
1566 /**
1567 * Assigns a point to this object.
1568 */
1569 inline void operator=(const TPoint3D &p) {
1570 destroy();
1572 data.point=p;
1573 }
1574 /**
1575 * Assigns a segment to this object.
1576 */
1577 inline void operator=(const TSegment3D &s) {
1578 destroy();
1580 data.segment=s;
1581 }
1582 /**
1583 * Assigns a line to this object.
1584 */
1585 inline void operator=(const TLine3D &l) {
1586 destroy();
1588 data.line=l;
1589 }
1590 /**
1591 * Assigns a polygon to this object.
1592 */
1593 inline void operator=(const TPolygon3D &p) {
1594 destroy();
1596 data.polygon=new TPolygon3D(p);
1597 }
1598 /**
1599 * Assigns a plane to this object.
1600 */
1601 inline void operator=(const TPlane &p) {
1602 destroy();
1604 data.plane=p;
1605 }
1606 /**
1607 * Projects into 2D space.
1608 * \throw std::logic_error if the 3D object loses its properties when projecting into 2D space (for example, it's a plane or a vertical line).
1609 */
1610 inline void generate2DObject(TObject2D &obj) const {
1611 switch (type) {
1613 obj=TPoint2D(data.point);
1614 break;
1616 obj=TSegment2D(data.segment);
1617 break;
1619 obj=TLine2D(data.line);
1620 break;
1622 obj=TPolygon2D(*(data.polygon));
1623 break;
1625 throw std::logic_error("Too many dimensions");
1626 default:
1627 obj=TObject2D();
1628 break;
1629 }
1630 }
1631 /**
1632 * Constructs from another object.
1633 */
1635 operator=(obj);
1636 }
1637 /**
1638 * Static method to retrieve every point included in a vector of objects.
1639 */
1640 static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts);
1641 /**
1642 * Static method to retrieve every segment included in a vector of objects.
1643 */
1644 static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms);
1645 /**
1646 * Static method to retrieve every line included in a vector of objects.
1647 */
1648 static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins);
1649 /**
1650 * Static method to retrieve every plane included in a vector of objects.
1651 */
1652 static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns);
1653 /**
1654 * Static method to retrieve every polygon included in a vector of objects.
1655 */
1656 static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys);
1657 /**
1658 * Static method to retrieve every point included in a vector of objects, returning the remaining objects in another argument.
1659 */
1660 static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts,std::vector<TObject3D> &remainder);
1661 /**
1662 * Static method to retrieve every segment included in a vector of objects, returning the remaining objects in another argument.
1663 */
1664 static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms,std::vector<TObject3D> &remainder);
1665 /**
1666 * Static method to retrieve every line included in a vector of objects, returning the remaining objects in another argument.
1667 */
1668 static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins,std::vector<TObject3D> &remainder);
1669 /**
1670 * Static method to retrieve every plane included in a vector of objects, returning the remaining objects in another argument.
1671 */
1672 static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns,std::vector<TObject3D> &remainder);
1673 /**
1674 * Static method to retrieve every polygon included in a vector of objects, returning the remaining objects in another argument.
1675 */
1676 static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder);
1677 };
1678
1679
1680
1681 //Streaming functions
1682 /** TPoint2D binary input. */
1684 /** TPoint2D binary output. */
1686 /** TPoint3D binary input. */
1688 /** TPoint3D binary output. */
1690 /** TPose2D binary input. */
1692 /**TPose2D binary output. */
1694 /** TPose3D binary input. */
1696 /** TPose3D binary output. */
1698 /**TSegment2D binary input. */
1700 /** TSegment2D binary output. */
1702 /**TLine2D binary input. */
1704 /** TLine2D binary output. */
1706 /** TObject2D binary input. */
1708 /** TObject2D binary input. */
1710 /** TSegment3D binary input. */
1712 /**TSegment3D binary output. */
1714 /** TLine3D binary input. */
1716 /** TLine3D binary output. */
1718 /** TPlane binary input. */
1720 /** TPlane binary output. */
1722 /** TObject3D binary input. */
1724 /** TObject3D binary output. */
1726
1727 /** @} */ // end of grouping
1728
1729 } //end of namespace math
1730
1731 namespace utils
1732 {
1733 // Specialization must occur in the same namespace
1747
1748 } // end of namespace utils
1749
1750} //end of namespace
1751#endif
#define MRPT_DECLARE_TTYPENAME_NAMESPACE(_TYPE, __NS)
Definition: TTypeName.h:64
2D polygon, inheriting from std::vector<TPoint2D>.
TPolygon2D(const std::vector< TPoint2D > &v)
Implicit constructor from a vector of 2D points.
TPolygon2D()
Default constructor.
double distance(const TPoint2D &point) const
Distance to a point.
void getAsSegmentList(std::vector< TSegment2D > &v) const
Gets as set of segments, instead of points.
void getCenter(TPoint2D &p) const
Polygon's central point.
static void createRegularPolygon(size_t numEdges, double radius, TPolygon2D &poly, const mrpt::poses::CPose2D &pose)
Static method to create a regular polygon from its size and radius.
void generate3DObject(TPolygon3D &p) const
Projects into 3D space, zeroing the z.
bool isConvex() const
Checks whether is convex.
void removeRedundantVertices()
Erase every redundant vertex from the polygon, saving space.
TPolygon2D(const TPolygon3D &p)
Constructor from a 3D object.
static void createRegularPolygon(size_t numEdges, double radius, TPolygon2D &poly)
Static method to create a regular polygon, given its size and radius.
void removeRepeatedVertices()
Erase repeated vertices.
TPolygon2D(size_t N)
Constructor for a given number of vertices, intializing them as garbage.
bool contains(const TPoint2D &point) const
Check whether a point is inside the polygon.
void getPlotData(std::vector< double > &x, std::vector< double > &y) const
Gets plot data, ready to use on a 2D plot.
3D polygon, inheriting from std::vector<TPoint3D>
TPolygon3D()
Default constructor.
void getBestFittingPlane(TPlane &p) const
Gets the best fitting plane, disregarding whether the polygon actually fits inside or not.
void getAsSegmentList(std::vector< TSegment3D > &v) const
Gets as set of segments, instead of set of points.
bool isSkew() const
Check whether the polygon is skew.
bool contains(const TPoint3D &point) const
Check whether a point is inside the polygon.
void generate2DObject(TPolygon2D &p) const
Projects into a 2D space, discarding the z.
void removeRedundantVertices()
Erase every redundant vertex, thus saving space.
TPolygon3D(const std::vector< TPoint3D > &v)
Implicit constructor from a 3D points vector.
double distance(const TPoint3D &point) const
Distance to point.
void removeRepeatedVertices()
Remove polygon's repeated vertices.
TPolygon3D(size_t N)
Constructor for a given size.
bool getPlane(TPlane &p) const
Gets a plane which contains the polygon.
void getCenter(TPoint3D &p) const
Get polygon's central point.
static void createRegularPolygon(size_t numEdges, double radius, TPolygon3D &poly, const mrpt::poses::CPose3D &pose)
Static method to create a regular polygon, given its size and radius.
static void createRegularPolygon(size_t numEdges, double radius, TPolygon3D &poly)
Static method to create a regular polygon, given its size and radius.
TPolygon3D(const TPolygon2D &p)
Constructor from a 2D object.
A class used to store a 2D point.
Definition: CPoint2D.h:37
A class used to store a 3D point.
Definition: CPoint3D.h:33
A class used to store a 2D pose.
Definition: CPose2D.h:37
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
Definition: CPose3DQuat.h:42
The base template class for 2D & 3D points and poses.
Definition: CPoseOrPoint.h:108
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:39
@ static_size
Definition: eigen_plugins.h:17
std::vector< T1 > operator*(const std::vector< T1 > &a, const std::vector< T2 > &b)
a*b (element-wise multiplication)
Definition: ops_vectors.h:59
T wrapTo2Pi(T a)
Modifies the given angle to translate it into the [0,2pi[ range.
Definition: wrap2pi.h:40
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
Definition: ops_vectors.h:70
const unsigned char GEOMETRIC_TYPE_POINT
Object type identifier for TPoint2D or TPoint3D.
const unsigned char GEOMETRIC_TYPE_LINE
Object type identifier for TLine2D or TLine3D.
TPoint3D operator-(const TPoint3D &p1)
Unary minus operator for 3D points.
struct BASE_IMPEXP TSegment3D
struct BASE_IMPEXP TObject3D
const unsigned char GEOMETRIC_TYPE_UNDEFINED
Object type identifier for empty TObject2D or TObject3D.
const unsigned char GEOMETRIC_TYPE_POLYGON
Object type identifier for TPolygon2D or TPolygon3D.
std::ostream BASE_IMPEXP & operator<<(std::ostream &o, const TPoint2D &p)
const unsigned char GEOMETRIC_TYPE_SEGMENT
Object type identifier for TSegment2D or TSegment3D.
struct BASE_IMPEXP TLine3D
class BASE_IMPEXP TPolygon3D
const unsigned char GEOMETRIC_TYPE_PLANE
Object type identifier for TPlane.
#define THROW_EXCEPTION(msg)
Definition: mrpt_macros.h:110
This base provides a set of functions for maths stuff.
Definition: CArray.h:19
BASE_IMPEXP::mrpt::utils::CStream & operator>>(mrpt::utils::CStream &in, CMatrixPtr &pObj)
bool operator!=(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:287
bool operator==(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:279
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:113
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
STL namespace.
2D line without bounds, represented by its equation .
TLine2D(const TLine3D &l)
Construction from 3D object, discarding the Z.
TLine2D()
Fast default constructor.
double evaluatePoint(const TPoint2D &point) const
Evaluate point in the line's equation.
TLine2D(const TSegment2D &s)
Constructor from a segment.
void generate3DObject(TLine3D &l) const
Project into 3D space, setting the z to 0.
void getNormalVector(double(&vector)[2]) const
Get line's normal vector.
void getUnitaryDirectorVector(double(&vector)[2])
Unitarize line and then get director vector.
double signedDistance(const TPoint2D &point) const
Distance with sign from a given point (sign indicates side).
TLine2D(const TPoint2D &p1, const TPoint2D &p2)
Constructor from two points, through which the line will pass.
void unitarize()
Unitarize line's normal vector.
double distance(const TPoint2D &point) const
Distance from a given point.
void getDirectorVector(double(&vector)[2]) const
Get line's director vector.
void getAsPose2D(mrpt::poses::CPose2D &outPose) const
Get a pose2D whose X axis corresponds to the line.
bool contains(const TPoint2D &point) const
Check whether a point is inside the line.
void getAsPose2DForcingOrigin(const TPoint2D &origin, mrpt::poses::CPose2D &outPose) const
Get a pose2D whose X axis corresponds to the line, forcing the base point to one given.
TLine2D(double A, double B, double C)
Constructor from line's coefficients.
void getUnitaryNormalVector(double(&vector)[2])
Get line's normal vector after unitarizing line.
3D line, represented by a base point and a director vector.
TLine3D()
Fast default constructor.
void getDirectorVector(double(&vector)[3]) const
Get director vector.
double distance(const TPoint3D &point) const
Distance between the line and a point.
void unitarize()
Unitarize director vector.
bool contains(const TPoint3D &point) const
Check whether a point is inside the line.
void generate2DObject(TLine2D &l) const
Project into 2D space, discarding the Z coordinate.
TLine3D(const TPoint3D &p1, const TPoint3D &p2)
Constructor from two points, through which the line will pass.
void getUnitaryDirectorVector(double(&vector)[3])
Unitarize and then get director vector.
TPoint3D pBase
Base point.
TLine3D(const TSegment3D &s)
Constructor from 3D segment.
TLine3D(const TLine2D &l)
Implicit constructor from 2D object.
Union type storing pointers to every allowed type.
Standard type for storing any lightweight 2D type.
void generate3DObject(TObject3D &obj) const
Project into 3D space.
void operator=(const TPoint2D &p)
Assign a point to this object.
bool isPoint() const
Checks whether content is a point.
static void getSegments(const std::vector< TObject2D > &objs, std::vector< TSegment2D > &sgms, std::vector< TObject2D > &remainder)
Static method to retrieve all the segments in a vector of TObject2D, returning the remainder objects ...
static void getLines(const std::vector< TObject2D > &objs, std::vector< TLine2D > &lins)
Static method to retrieve all the lines in a vector of TObject2D.
bool isSegment() const
Checks whether content is a segment.
static void getPoints(const std::vector< TObject2D > &objs, std::vector< TPoint2D > &pnts, std::vector< TObject2D > &remainder)
Static method to retrieve all the points in a vector of TObject2D, returning the remainder objects in...
TObject2D(const TPoint2D &p)
Implicit constructor from point.
bool isPolygon() const
Checks whether content is a polygon.
TObject2D & operator=(const TObject2D &obj)
Assign another TObject2D.
static void getPoints(const std::vector< TObject2D > &objs, std::vector< TPoint2D > &pnts)
Static method to retrieve all the points in a vector of TObject2D.
bool getLine(TLine2D &r) const
Gets the content as a line, returning false if the type is inadequate.
~TObject2D()
Object destruction.
static void getSegments(const std::vector< TObject2D > &objs, std::vector< TSegment2D > &sgms)
Static method to retrieve all the segments in a vector of TObject2D.
unsigned char type
Object type identifier.
TObject2D(const TLine2D &r)
Implicit constructor from line.
unsigned char getType() const
Gets content type.
TObject2D(const TObject2D &obj)
Constructor from another TObject2D.
void operator=(const TPolygon2D &p)
Assign a polygon to this object.
bool getSegment(TSegment2D &s) const
Gets the content as a segment, returning false if the type is inadequate.
static void getPolygons(const std::vector< TObject2D > &objs, std::vector< TPolygon2D > &polys)
Static method to retrieve all the polygons in a vector of TObject2D.
bool getPolygon(TPolygon2D &p) const
Gets the content as a polygon, returning false if the type is inadequate.
void destroy()
Destroys the object, releasing the pointer to the content (if any).
void operator=(const TLine2D &l)
Assign a line to this object.
bool getPoint(TPoint2D &p) const
Gets the content as a point, returning false if the type is inadequate.
static void getLines(const std::vector< TObject2D > &objs, std::vector< TLine2D > &lins, std::vector< TObject2D > &remainder)
Static method to retrieve all the lines in a vector of TObject2D, returning the remainder objects in ...
TObject2D(const TPolygon2D &p)
Implicit constructor from polygon.
TObject2D(const TSegment2D &s)
Implicit constructor from segment.
bool isLine() const
Checks whether content is a line.
void operator=(const TSegment2D &s)
Assign a segment to this object.
TObject2D()
Implicit constructor from polygon.
struct mrpt::math::TObject2D::tobject2d_data_t data
static void getPolygons(const std::vector< TObject2D > &objs, std::vector< TPolygon2D > &polys, std::vector< TObject2D > &remainder)
Static method to retrieve all the polygons in a vector of TObject2D, returning the remainder objects ...
Union containing pointer to actual data.
Standard object for storing any 3D lightweight object.
void operator=(const TPolygon3D &p)
Assigns a polygon to this object.
static void getLines(const std::vector< TObject3D > &objs, std::vector< TLine3D > &lins, std::vector< TObject3D > &remainder)
Static method to retrieve every line included in a vector of objects, returning the remaining objects...
void operator=(const TPoint3D &p)
Assigns a point to this object.
static void getPoints(const std::vector< TObject3D > &objs, std::vector< TPoint3D > &pnts, std::vector< TObject3D > &remainder)
Static method to retrieve every point included in a vector of objects, returning the remaining object...
static void getSegments(const std::vector< TObject3D > &objs, std::vector< TSegment3D > &sgms)
Static method to retrieve every segment included in a vector of objects.
bool isPolygon() const
Checks whether content is a polygon.
TObject3D & operator=(const TObject3D &obj)
Assigns another object, creating a new pointer if needed.
void operator=(const TSegment3D &s)
Assigns a segment to this object.
bool getPlane(TPlane &p) const
Gets the content as a plane, returning false if the type is not adequate.
TObject3D(const TLine3D &r)
Constructor from line.
void destroy()
Destroys the object and releases the pointer, if any.
void operator=(const TPlane &p)
Assigns a plane to this object.
unsigned char getType() const
Gets object type.
TObject3D(const TPlane &p)
Constructor from plane.
bool isLine() const
Checks whether content is a line.
static void getPoints(const std::vector< TObject3D > &objs, std::vector< TPoint3D > &pnts)
Static method to retrieve every point included in a vector of objects.
bool isSegment() const
Checks whether content is a segment.
TObject3D(const TPoint3D &p)
Constructor from point.
static void getPolygons(const std::vector< TObject3D > &objs, std::vector< TPolygon3D > &polys)
Static method to retrieve every polygon included in a vector of objects.
bool getSegment(TSegment3D &s) const
Gets the content as a segment, returning false if the type is not adequate.
struct mrpt::math::TObject3D::tobject3d_data_t data
bool getLine(TLine3D &r) const
Gets the content as a line, returning false if the type is not adequate.
void operator=(const TLine3D &l)
Assigns a line to this object.
static void getSegments(const std::vector< TObject3D > &objs, std::vector< TSegment3D > &sgms, std::vector< TObject3D > &remainder)
Static method to retrieve every segment included in a vector of objects, returning the remaining obje...
static void getPlanes(const std::vector< TObject3D > &objs, std::vector< TPlane > &plns)
Static method to retrieve every plane included in a vector of objects.
TObject3D(const TSegment3D &s)
Constructor from segment.
static void getLines(const std::vector< TObject3D > &objs, std::vector< TLine3D > &lins)
Static method to retrieve every line included in a vector of objects.
bool getPoint(TPoint3D &p) const
Gets the content as a point, returning false if the type is not adequate.
unsigned char type
Object type identifier.
TObject3D(const TPolygon3D &p)
Constructor from polygon.
bool isPlane() const
Checks whether content is a plane.
TObject3D()
Empty constructor.
static void getPlanes(const std::vector< TObject3D > &objs, std::vector< TPlane > &plns, std::vector< TObject3D > &remainder)
Static method to retrieve every plane included in a vector of objects, returning the remaining object...
bool isPoint() const
Checks whether content is a point.
TObject3D(const TObject3D &obj)
Constructs from another object.
void generate2DObject(TObject2D &obj) const
Projects into 2D space.
static void getPolygons(const std::vector< TObject3D > &objs, std::vector< TPolygon3D > &polys, std::vector< TObject3D > &remainder)
Static method to retrieve every polygon included in a vector of objects, returning the remaining obje...
bool getPolygon(TPolygon3D &p) const
Gets the content as a polygon, returning false if the type is not adequate.
3D Plane, represented by its equation
TPlane(const TLine3D &r1, const TLine3D &r2)
Gets a plane which contains the two lines.
double distance(const TLine3D &line) const
Distance to 3D line.
TPlane()
Fast default constructor.
void getAsPose3D(mrpt::poses::CPose3D &outPose) const
Gets a pose whose XY plane corresponds to this plane.
double distance(const TPoint3D &point) const
Distance to 3D point.
double evaluatePoint(const TPoint3D &point) const
Evaluate a point in the plane's equation.
bool contains(const TSegment3D &segment) const
Check whether a segment is fully contained into the plane.
void getNormalVector(double(&vec)[3]) const
Get plane's normal vector.
bool contains(const TLine3D &line) const
Check whether a line is fully contained into the plane.
void getUnitaryNormalVector(double(&vec)[3])
Unitarize, then get normal vector.
void getAsPose3D(mrpt::poses::CPose3D &outPose)
Gets a pose whose XY plane corresponds to this plane.
TPlane(const TPoint3D &p1, const TPoint3D &p2, const TPoint3D &p3)
Gets a plane which contains these three points.
bool contains(const TPoint3D &point) const
Check whether a point is contained into the plane.
void getAsPose3DForcingOrigin(const TPoint3D &newOrigin, mrpt::poses::CPose3D &pose)
Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates.
TPlane(const double(&vec)[4])
Constructor from an array of coefficients.
void getAsPose3DForcingOrigin(const TPoint3D &newOrigin, mrpt::poses::CPose3D &pose) const
Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates.
TPlane(const TPoint3D &p1, const TLine3D &r2)
Gets a plane which contains this point and this line.
TPlane(double A, double B, double C, double D)
Constructor from plane coefficients.
void unitarize()
Unitarize normal vector.
Lightweight 2D point.
double y
X,Y coordinates.
double & operator[](size_t i)
Coordinate access using operator[].
TPoint2D operator*(double d) const
TPoint2D(const mrpt::utils::TPixelCoordf &p)
Implicit transformation constructor from TPixelCoordf.
const double & operator[](size_t i) const
Coordinate access using operator[].
TPoint2D(const mrpt::poses::CPoseOrPoint< DERIVEDCLASS > &p)
Constructor from CPoseOrPoint, perhaps losing 3D information.
TPoint2D(const TPose3D &p)
Constructor from TPose3D, discarding z and the angular coordinates.
TPoint2D(double xx, double yy)
Constructor from coordinates.
TPoint2D & operator*=(double d)
TPoint2D(const TPoint3D &p)
Constructor from TPoint3D, discarding z.
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04]" )
bool operator<(const TPoint2D &p) const
TPoint2D & operator/=(double d)
TPoint2D operator+(const TPoint2D &p) const
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[0.02 1.04]" )
void getAsVector(std::vector< double > &v) const
Transformation into vector.
TPoint2D(const TPose2D &p)
Constructor from TPose2D, discarding phi.
TPoint2D(const mrpt::poses::CPoint2D &p)
Implicit constructor from mrpt::poses::CPoint2D
std::string asString() const
TPoint2D operator-(const TPoint2D &p) const
TPoint2D & operator-=(const TPoint2D &p)
double norm() const
Point norm.
TPoint2D & operator+=(const TPoint2D &p)
TPoint2D operator/(double d) const
TPoint2D()
Default fast constructor.
Lightweight 3D point.
TPoint3D & operator-=(const TPoint3D &p)
Difference between points.
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[0.02 1.04 -0....
double distanceTo(const TPoint3D &p) const
Point-to-point distance.
TPoint3D(const TPoint2D &p)
Implicit constructor from TPoint2D.
std::string asString() const
TPoint3D(double xx, double yy, double zz)
Constructor from coordinates.
TPoint3D operator+(const TPoint3D &p) const
Points addition.
double z
X,Y,Z coordinates.
TPoint3D & operator*=(const double f)
Point scale.
double & operator[](size_t i)
Coordinate access using operator[].
TPoint3D(const TPose2D &p)
Constructor from TPose2D, losing information.
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0....
TPoint3D(const TPoint3Df &p)
Explicit constructor from coordinates.
TPoint3D(const mrpt::poses::CPose3D &p)
Constructor from heavyweight 3D pose.
TPoint3D operator/(double d) const
const double & operator[](size_t i) const
Coordinate access using operator[].
double norm() const
Point norm.
TPoint3D(const TPose3D &p)
Constructor from TPose3D, losing information.
TPoint3D operator*(double d) const
TPoint3D()
Default fast constructor.
void getAsVector(VECTORLIKE &v) const
Transformation into vector.
double sqrDistanceTo(const TPoint3D &p) const
Point-to-point distance, squared.
TPoint3D operator-(const TPoint3D &p) const
Points substraction.
TPoint3D & operator+=(const TPoint3D &p)
Translation.
TPoint3D(const mrpt::poses::CPoint3D &p)
Implicit constructor from heavyweight type.
bool operator<(const TPoint3D &p) const
Lightweight 3D point (float version).
const float & operator[](size_t i) const
Coordinate access using operator[].
float & operator[](size_t i)
Coordinate access using operator[].
TPoint3Df(const float xx, const float yy, const float zz)
XYZ point (double) + Intensity(u8)
TPointXYZIu8(double x, double y, double z, uint8_t intensity_val)
XYZ point (double) + RGB(u8)
TPointXYZRGBu8(double x, double y, double z, uint8_t R_val, uint8_t G_val, uint8_t B_val)
XYZ point (float) + Intensity(u8)
TPointXYZfIu8(float x, float y, float z, uint8_t intensity_val)
XYZ point (float) + RGB(u8)
TPointXYZfRGBu8(float x, float y, float z, uint8_t R_val, uint8_t G_val, uint8_t B_val)
Lightweight 2D pose.
TPose2D(const TPose3D &p)
Constructor from TPose3D, losing information.
double phi
Orientation (rads)
const double & operator[](size_t i) const
Coordinate access using operator[].
double y
X,Y coordinates.
std::string asString() const
TPose2D(double xx, double yy, double pphi)
Constructor from coordinates.
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees)
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -45....
TPose2D()
Default fast constructor.
double & operator[](size_t i)
Coordinate access using operator[].
TPose2D(const TPoint3D &p)
Constructor from TPoint3D, losing information.
void getAsVector(std::vector< double > &v) const
Transformation into vector.
TPose2D(const mrpt::poses::CPose2D &p)
Implicit constructor from heavyweight type.
TPose2D(const TPoint2D &p)
Implicit constructor from TPoint2D.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
TPose3D(const TPose2D &p)
Implicit constructor from TPose2D.
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]",...
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0....
TPose3D(double _x, double _y, double _z, double _yaw, double _pitch, double _roll)
Constructor from coordinates.
TPose3D(const TPoint3D &p)
Implicit constructor from TPoint3D.
TPose3D(const mrpt::poses::CPose3D &p)
Implicit constructor from heavyweight type.
void getAsVector(std::vector< double > &v) const
Gets the pose as a vector of doubles.
double z
X,Y,Z, coords.
double roll
Roll coordinate (rotation angle over X coordinate).
std::string asString() const
double norm() const
Pose's spatial coordinates norm.
TPose3D()
Default fast constructor.
double pitch
Pitch coordinate (rotation angle over Y axis).
const double & operator[](size_t i) const
Coordinate access using operator[].
double & operator[](size_t i)
Coordinate access using operator[].
double yaw
Yaw coordinate (rotation angle over Z axis).
TPose3D(const TPoint2D &p)
Implicit constructor from TPoint2D.
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
std::string asString() const
void getAsVector(std::vector< double > &v) const
Gets the pose as a vector of doubles.
double & operator[](size_t i)
Coordinate access using operator[].
TPose3DQuat(double _x, double _y, double _z, double _qr, double _qx, double _qy, double _qz)
Constructor from coordinates.
TPose3DQuat(const mrpt::poses::CPose3DQuat &p)
Constructor from a CPose3DQuat.
const double & operator[](size_t i) const
Coordinate access using operator[].
void asString(std::string &s) const
Returns a human-readable textual representation of the object as "[x y z qr qx qy qz]".
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0....
double norm() const
Pose's spatial coordinates norm.
TPose3DQuat()
Default fast constructor.
2D segment, consisting of two points.
bool contains(const TPoint2D &point) const
Check whether a point is inside a segment.
TPoint2D point2
Destiny point.
TSegment2D(const TPoint2D &p1, const TPoint2D &p2)
Constructor from both points.
double signedDistance(const TPoint2D &point) const
Distance with sign to point (sign indicates which side the point is).
const TPoint2D & operator[](size_t i) const
Access to points using operator[0-1].
bool operator<(const TSegment2D &s) const
TSegment2D()
Fast default constructor.
TSegment2D(const TSegment3D &s)
Explicit constructor from 3D object, discarding the z.
TPoint2D point1
Origin point.
TPoint2D & operator[](size_t i)
Access to points using operator[0-1].
void getCenter(TPoint2D &p) const
Segment's central point.
void generate3DObject(TSegment3D &s) const
Project into 3D space, setting the z to 0.
double length() const
Segment length.
double distance(const TPoint2D &point) const
Distance to point.
3D segment, consisting of two points.
TPoint3D & operator[](size_t i)
Access to points using operator[0-1].
const TPoint3D & operator[](size_t i) const
Access to points using operator[0-1].
double distance(const TPoint3D &point) const
Distance to point.
TPoint3D point1
Origin point.
void getCenter(TPoint3D &p) const
Segment's central point.
double length() const
Segment length.
TPoint3D point2
Destiny point.
double distance(const TSegment3D &segment) const
Distance to another segment.
bool operator<(const TSegment3D &s) const
TSegment3D()
Fast default constructor.
bool contains(const TPoint3D &point) const
Check whether a point is inside the segment.
TSegment3D(const TPoint3D &p1, const TPoint3D &p2)
Constructor from both points.
TSegment3D(const TSegment2D &s)
Constructor from 2D object.
void generate2DObject(TSegment2D &s) const
Projection into 2D space, discarding the z.
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:22



Page generated by Doxygen 1.9.2 for MRPT 1.4.0 SVN: at Mon Sep 20 00:21:40 UTC 2021