MRPT  1.9.9
geometry.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 
12 #include <mrpt/math/CMatrixFixed.h>
14 #include <mrpt/math/TLine2D.h>
15 #include <mrpt/math/TPlane.h>
16 #include <mrpt/math/TPolygon2D.h>
17 #include <mrpt/math/TPolygon3D.h>
18 #include <mrpt/math/TPose3D.h>
19 #include <mrpt/math/epsilon.h>
20 #include <mrpt/math/math_frwds.h> // forward declarations
21 #include <mrpt/math/wrap2pi.h>
22 
23 namespace mrpt::math
24 {
25 /** \addtogroup geometry_grp Geometry: lines, planes, intersections, SLERP,
26  * "lightweight" point & pose classes
27  * \ingroup mrpt_math_grp
28  * @{ */
29 
30 /** Slightly heavyweight type to speed-up calculations with polygons in 3D
31  * \sa TPolygon3D,TPlane
32  */
34 {
35  public:
36  /** Actual polygon. */
38  /** Plane containing the polygon. */
40  /** Plane's pose. \sa inversePose */
42  /** Plane's inverse pose. \sa pose */
44  /** Polygon, after being projected to the plane using inversePose. \sa
45  * inversePose */
47  /** Constructor. Takes a polygon and computes each parameter. */
48  TPolygonWithPlane(const TPolygon3D& p);
49  /** Basic constructor. Needed to create containers \sa
50  * TPolygonWithPlane(const TPolygon3D &) */
51  TPolygonWithPlane() = default;
52  /** Static method for vectors. Takes a set of polygons and creates every
53  * TPolygonWithPlane */
54  static void getPlanes(
55  const std::vector<TPolygon3D>& oldPolys,
56  std::vector<TPolygonWithPlane>& newPolys);
57 };
58 
59 /** @name Simple intersection operations, relying basically on geometrical
60  operations.
61  @{
62  */
63 /** Gets the intersection between two 3D segments. Possible outcomes:
64  * - Segments intersect: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT
65  * - Segments don't intersect & are parallel: Return=true,
66  *obj.getType()=GEOMETRIC_TYPE_SEGMENT, obj is the segment "in between" both
67  *segments.
68  * - Segments don't intersect & aren't parallel: Return=false.
69  * \sa TObject3D
70  */
71 bool intersect(const TSegment3D& s1, const TSegment3D& s2, TObject3D& obj);
72 
73 /** Gets the intersection between a 3D segment and a plane. Possible outcomes:
74  * - Don't intersect: Return=false
75  * - s1 is within the plane: Return=true,
76  *obj.getType()=GEOMETRIC_TYPE_SEGMENT
77  * - s1 intersects the plane at one point: Return=true,
78  *obj.getType()=GEOMETRIC_TYPE_POINT
79  * \sa TObject3D
80  */
81 bool intersect(const TSegment3D& s1, const TPlane& p2, TObject3D& obj);
82 
83 /** Gets the intersection between a 3D segment and a 3D line. Possible outcomes:
84  * - They don't intersect : Return=false
85  * - s1 lies within the line: Return=true,
86  *obj.getType()=GEOMETRIC_TYPE_SEGMENT
87  * - s1 intersects the line at a point: Return=true,
88  *obj.getType()=GEOMETRIC_TYPE_POINT
89  * \sa TObject3D
90  */
91 bool intersect(const TSegment3D& s1, const TLine3D& r2, TObject3D& obj);
92 
93 /** Gets the intersection between a plane and a 3D segment. Possible outcomes:
94  * - Don't intersect: Return=false
95  * - s2 is within the plane: Return=true,
96  *obj.getType()=GEOMETRIC_TYPE_SEGMENT
97  * - s2 intersects the plane at one point: Return=true,
98  *obj.getType()=GEOMETRIC_TYPE_POINT
99  * \sa TObject3D
100  */
101 inline bool intersect(const TPlane& p1, const TSegment3D& s2, TObject3D& obj)
102 {
103  return intersect(s2, p1, obj);
104 }
105 
106 /** Gets the intersection between two planes. Possible outcomes:
107  * - Planes are parallel: Return=false
108  * - Planes intersect into a line: Return=true,
109  *obj.getType()=GEOMETRIC_TYPE_LINE
110  * \sa TObject3D
111  */
112 bool intersect(const TPlane& p1, const TPlane& p2, TObject3D& obj);
113 
114 /** Gets the intersection between a plane and a 3D line. Possible outcomes:
115  * - Line is parallel to plane but not within it: Return=false
116  * - Line is contained in the plane: Return=true,
117  *obj.getType()=GEOMETRIC_TYPE_LINE
118  * - Line intersects the plane at one point: Return=true,
119  *obj.getType()=GEOMETRIC_TYPE_POINT
120  * \sa TObject3D
121  */
122 bool intersect(const TPlane& p1, const TLine3D& p2, TObject3D& obj);
123 
124 /** Gets the intersection between a 3D line and a 3D segment. Possible outcomes:
125  * - They don't intersect : Return=false
126  * - s2 lies within the line: Return=true,
127  *obj.getType()=GEOMETRIC_TYPE_SEGMENT
128  * - s2 intersects the line at a point: Return=true,
129  *obj.getType()=GEOMETRIC_TYPE_POINT
130  * \sa TObject3D
131  */
132 inline bool intersect(const TLine3D& r1, const TSegment3D& s2, TObject3D& obj)
133 {
134  return intersect(s2, r1, obj);
135 }
136 
137 /** Gets the intersection between a 3D line and a plane. Possible outcomes:
138  * - Line is parallel to plane but not within it: Return=false
139  * - Line is contained in the plane: Return=true,
140  *obj.getType()=GEOMETRIC_TYPE_LINE
141  * - Line intersects the plane at one point: Return=true,
142  *obj.getType()=GEOMETRIC_TYPE_POINT
143  * \sa TObject3D
144  */
145 inline bool intersect(const TLine3D& r1, const TPlane& p2, TObject3D& obj)
146 {
147  return intersect(p2, r1, obj);
148 }
149 
150 /** Gets the intersection between two 3D lines. Possible outcomes:
151  * - Lines do not intersect: Return=false
152  * - Lines are parallel and do not coincide: Return=false
153  * - Lines coincide (are the same): Return=true,
154  *obj.getType()=GEOMETRIC_TYPE_LINE
155  * - Lines intesect in a point: Return=true,
156  *obj.getType()=GEOMETRIC_TYPE_POINT
157  * \sa TObject3D
158  */
159 bool intersect(const TLine3D& r1, const TLine3D& r2, TObject3D& obj);
160 
161 /** Gets the intersection between two 2D lines. Possible outcomes:
162  * - Lines do not intersect: Return=false
163  * - Lines are parallel and do not coincide: Return=false
164  * - Lines coincide (are the same): Return=true,
165  *obj.getType()=GEOMETRIC_TYPE_LINE
166  * - Lines intesect in a point: Return=true,
167  *obj.getType()=GEOMETRIC_TYPE_POINT
168  * \sa TObject2D
169  */
170 bool intersect(const TLine2D& r1, const TLine2D& r2, TObject2D& obj);
171 
172 /** Gets the intersection between a 2D line and a 2D segment. Possible outcomes:
173  * - They don't intersect: Return=false
174  * - s2 lies within the line: Return=true,
175  *obj.getType()=GEOMETRIC_TYPE_SEGMENT
176  * - Both intersects in one point: Return=true,
177  *obj.getType()=GEOMETRIC_TYPE_POINT
178  * \sa TObject2D
179  */
180 bool intersect(const TLine2D& r1, const TSegment2D& s2, TObject2D& obj);
181 
182 /** Gets the intersection between a 2D line and a 2D segment. Possible outcomes:
183  * - They don't intersect: Return=false
184  * - s1 lies within the line: Return=true,
185  *obj.getType()=GEOMETRIC_TYPE_SEGMENT
186  * - Both intersects in one point: Return=true,
187  *obj.getType()=GEOMETRIC_TYPE_POINT
188  * \sa TObject2D
189  */
190 inline bool intersect(const TSegment2D& s1, const TLine2D& r2, TObject2D& obj)
191 {
192  return intersect(r2, s1, obj);
193 }
194 
195 /** Gets the intersection between two 2D segments. Possible outcomes:
196  * - Segments intersect: Return=true, obj.getType()=GEOMETRIC_TYPE_POINT
197  * - Segments don't intersect & are parallel: Return=true,
198  *obj.getType()=GEOMETRIC_TYPE_SEGMENT, obj is the segment "in between" both
199  *segments.
200  * - Segments don't intersect & aren't parallel: Return=false.
201  * \sa TObject2D
202  */
203 bool intersect(const TSegment2D& s1, const TSegment2D& s2, TObject2D& obj);
204 
205 /** @}
206  */
207 
208 /** @name Angle retrieval methods. Methods which use TSegments will
209  automatically use TLines' implicit constructors.
210  @{
211  */
212 /**
213  * Computes the angle between two planes.
214  */
215 double getAngle(const TPlane& p1, const TPlane& p2);
216 /**
217  * Computes the angle between a plane and a 3D line or segment (implicit
218  * constructor will be used if passing a segment instead of a line).
219  */
220 double getAngle(const TPlane& p1, const TLine3D& r2);
221 /**
222  * Computes the angle between a 3D line or segment and a plane (implicit
223  * constructor will be used if passing a segment instead of a line).
224  */
225 inline double getAngle(const TLine3D& r1, const TPlane& p2)
226 {
227  return getAngle(p2, r1);
228 }
229 /**
230  * Computes the accute relative angle (range: [-PI/2,PI/2]) between two lines.
231  * \note Implicit constructor allows passing a segment as argument too.
232  */
233 double getAngle(const TLine3D& r1, const TLine3D& r2);
234 /**
235  * Computes the relative angle (range: [-PI,PI]) of line 2 wrt line 1.
236  * \note Implicit constructor allows passing a segment as argument too.
237  */
238 double getAngle(const TLine2D& r1, const TLine2D& r2);
239 /** @}
240  */
241 
242 /** @name Creation of lines from poses.
243  @{
244  */
245 /**
246  * Gets a 3D line corresponding to the X axis in a given pose. An implicit
247  * constructor is used if a TPose3D is given.
248  * \sa createFromPoseY,createFromPoseZ,createFromPoseAndVector
249  */
250 void createFromPoseX(const mrpt::math::TPose3D& p, TLine3D& r);
251 /**
252  * Gets a 3D line corresponding to the Y axis in a given pose. An implicit
253  * constructor is used if a TPose3D is given.
254  * \sa createFromPoseX,createFromPoseZ,createFromPoseAndVector
255  */
256 void createFromPoseY(const mrpt::math::TPose3D& p, TLine3D& r);
257 /**
258  * Gets a 3D line corresponding to the Z axis in a given pose. An implicit
259  * constructor is used if a TPose3D is given.
260  * \sa createFromPoseX,createFromPoseY,createFromPoseAndVector
261  */
262 void createFromPoseZ(const mrpt::math::TPose3D& p, TLine3D& r);
263 /**
264  * Gets a 3D line corresponding to any arbitrary vector, in the base given by
265  * the pose. An implicit constructor is used if a TPose3D is given.
266  * \sa createFromPoseX,createFromPoseY,createFromPoseZ
267  */
269  const mrpt::math::TPose3D& p, const double (&vector)[3], TLine3D& r);
270 /**
271  * Gets a 2D line corresponding to the X axis in a given pose. An implicit
272  * constructor is used if a CPose2D is given.
273  * \sa createFromPoseY,createFromPoseAndVector
274  */
275 void createFromPoseX(const TPose2D& p, TLine2D& r);
276 /**
277  * Gets a 2D line corresponding to the Y axis in a given pose. An implicit
278  * constructor is used if a CPose2D is given.
279  * \sa createFromPoseX,createFromPoseAndVector
280  */
281 void createFromPoseY(const TPose2D& p, TLine2D& r);
282 /**
283  * Gets a 2D line corresponding to any arbitrary vector, in the base given the
284  * given pose. An implicit constructor is used if a CPose2D is given.
285  * \sa createFromPoseY,createFromPoseAndVector
286  */
288  const TPose2D& p, const double (&vector)[2], TLine2D& r);
289 /** @}
290  */
291 
292 /** @name Other line or plane related methods.
293  @{
294  */
295 /**
296  * Checks whether this polygon or set of points acceptably fits a plane.
297  * \sa TPolygon3D,getEpsilon
298  */
299 bool conformAPlane(const std::vector<TPoint3D>& points);
300 /**
301  * Checks whether this polygon or set of points acceptably fits a plane, and if
302  * it's the case returns it in the second argument.
303  * \sa TPolygon3D,getEpsilon
304  */
305 bool conformAPlane(const std::vector<TPoint3D>& points, TPlane& p);
306 /**
307  * Checks whether this set of points acceptably fits a 2D line.
308  * \sa getEpsilon
309  */
310 bool areAligned(const std::vector<TPoint2D>& points);
311 /**
312  * Checks whether this set of points acceptably fits a 2D line, and if it's the
313  * case returns it in the second argument.
314  * \sa getEpsilon
315  */
316 bool areAligned(const std::vector<TPoint2D>& points, TLine2D& r);
317 /**
318  * Checks whether this set of points acceptably fits a 3D line.
319  * \sa getEpsilon
320  */
321 bool areAligned(const std::vector<TPoint3D>& points);
322 /**
323  * Checks whether this set of points acceptably fits a 3D line, and if it's the
324  * case returns it in the second argument.
325  */
326 bool areAligned(const std::vector<TPoint3D>& points, TLine3D& r);
327 /** @}
328  */
329 
330 /** @name Projections
331  @{
332  */
333 /** Uses the given pose 3D to project a point into a new base */
334 inline void project3D(
335  const TPoint3D& point, const mrpt::math::TPose3D& newXYpose,
336  TPoint3D& newPoint)
337 {
338  newXYpose.composePoint(point, newPoint);
339 }
340 /** Uses the given pose 3D to project a segment into a new base */
341 inline void project3D(
342  const TSegment3D& segment, const mrpt::math::TPose3D& newXYpose,
343  TSegment3D& newSegment)
344 {
345  project3D(segment.point1, newXYpose, newSegment.point1);
346  project3D(segment.point2, newXYpose, newSegment.point2);
347 }
348 
349 /** Uses the given pose 3D to project a line into a new base */
350 void project3D(
351  const TLine3D& line, const mrpt::math::TPose3D& newXYpose,
352  TLine3D& newLine);
353 /** Uses the given pose 3D to project a plane into a new base */
354 void project3D(
355  const TPlane& plane, const mrpt::math::TPose3D& newXYpose,
356  TPlane& newPlane);
357 /** Uses the given pose 3D to project a polygon into a new base */
358 void project3D(
359  const TPolygon3D& polygon, const mrpt::math::TPose3D& newXYpose,
360  TPolygon3D& newPolygon);
361 /** Uses the given pose 3D to project any 3D object into a new base. */
362 void project3D(
363  const TObject3D& object, const mrpt::math::TPose3D& newXYPose,
364  TObject3D& newObject);
365 
366 /** Projects any 3D object into the plane's base, using its inverse pose. If the
367  * object is exactly inside the plane, this projection will zero its Z
368  * coordinates */
369 template <class T>
370 void project3D(const T& obj, const TPlane& newXYPlane, T& newObj)
371 {
372  mrpt::math::TPose3D pose;
373  TPlane(newXYPlane).getAsPose3D(pose);
374  project3D(obj, -pose, newObj);
375 }
376 
377 /** Projects any 3D object into the plane's base, using its inverse pose and
378  * forcing the position of the new coordinates origin. If the object is exactly
379  * inside the plane, this projection will zero its Z coordinates */
380 template <class T>
381 void project3D(
382  const T& obj, const TPlane& newXYPlane, const TPoint3D& newOrigin,
383  T& newObj)
384 {
385  mrpt::math::TPose3D pose;
386  // TPlane(newXYPlane).getAsPose3DForcingOrigin(newOrigin,pose);
387  TPlane(newXYPlane).getAsPose3D(pose);
388  project3D(obj, -pose, newObj);
389 }
390 
391 /** Projects a set of 3D objects into the plane's base. */
392 template <class T>
393 void project3D(
394  const std::vector<T>& objs, const mrpt::math::TPose3D& newXYpose,
395  std::vector<T>& newObjs)
396 {
397  size_t N = objs.size();
398  newObjs.resize(N);
399  for (size_t i = 0; i < N; i++) project3D(objs[i], newXYpose, newObjs[i]);
400 }
401 
402 /** Uses the given pose 2D to project a point into a new base. */
404  const TPoint2D& point, const TPose2D& newXpose, TPoint2D& newPoint);
405 
406 /** Uses the given pose 2D to project a segment into a new base */
407 inline void project2D(
408  const TSegment2D& segment, const TPose2D& newXpose, TSegment2D& newSegment)
409 {
410  project2D(segment.point1, newXpose, newSegment.point1);
411  project2D(segment.point2, newXpose, newSegment.point2);
412 }
413 
414 /** Uses the given pose 2D to project a line into a new base */
415 void project2D(const TLine2D& line, const TPose2D& newXpose, TLine2D& newLine);
416 /** Uses the given pose 2D to project a polygon into a new base. */
417 void project2D(
418  const TPolygon2D& polygon, const TPose2D& newXpose, TPolygon2D& newPolygon);
419 /** Uses the given pose 2D to project any 2D object into a new base */
420 void project2D(
421  const TObject2D& object, const TPose2D& newXpose, TObject2D& newObject);
422 
423 /** Projects any 2D object into the line's base, using its inverse pose. If the
424  * object is exactly inside the line, this projection will zero its Y
425  * coordinate.
426  * \tparam CPOSE2D set to TPose2D
427  */
428 template <class T, class CPOSE2D>
429 void project2D(const T& obj, const TLine2D& newXLine, T& newObj)
430 {
431  CPOSE2D pose;
432  newXLine.getAsPose2D(pose);
433  project2D(obj, CPOSE2D(0, 0, 0) - pose, newObj);
434 }
435 
436 /** Projects any 2D object into the line's base, using its inverse pose and
437  * forcing the position of the new coordinate origin. If the object is exactly
438  * inside the line, this projection will zero its Y coordinate.
439  * \tparam CPOSE2D set to TPose2D
440  */
441 template <class T, class CPOSE2D>
442 void project2D(
443  const T& obj, const TLine2D& newXLine, const TPoint2D& newOrigin, T& newObj)
444 {
445  CPOSE2D pose;
446  newXLine.getAsPose2DForcingOrigin(newOrigin, pose);
447  project2D(obj, CPOSE2D(0, 0, 0) - pose, newObj);
448 }
449 
450 /** Projects a set of 2D objects into the line's base */
451 template <class T>
452 void project2D(
453  const std::vector<T>& objs, const TPose2D& newXpose,
454  std::vector<T>& newObjs)
455 {
456  size_t N = objs.size();
457  newObjs.resize(N);
458  for (size_t i = 0; i < N; i++) project2D(objs[i], newXpose, newObjs[i]);
459 }
460 /** @}
461  */
462 
463 /** @name Polygon intersections. These operations rely more on spatial reasoning
464  than in raw numerical operations.
465  @{
466  */
467 /** Gets the intersection between a 2D polygon and a 2D segment. \sa TObject2D
468  */
469 bool intersect(const TPolygon2D& p1, const TSegment2D& s2, TObject2D& obj);
470 /** Gets the intersection between a 2D polygon and a 2D line. \sa TObject2D */
471 bool intersect(const TPolygon2D& p1, const TLine2D& r2, TObject2D& obj);
472 /** Gets the intersection between two 2D polygons. \sa TObject2D */
473 bool intersect(const TPolygon2D& p1, const TPolygon2D& p2, TObject2D& obj);
474 /** Gets the intersection between a 2D segment and a 2D polygon. \sa TObject2D
475  */
476 inline bool intersect(
477  const TSegment2D& s1, const TPolygon2D& p2, TObject2D& obj)
478 {
479  return intersect(p2, s1, obj);
480 }
481 /** Gets the intersection between a 2D line and a 2D polygon.\sa TObject2D */
482 inline bool intersect(const TLine2D& r1, const TPolygon2D& p2, TObject2D& obj)
483 {
484  return intersect(p2, r1, obj);
485 }
486 /** Gets the intersection between a 3D polygon and a 3D segment. \sa TObject3D
487  */
488 bool intersect(const TPolygon3D& p1, const TSegment3D& s2, TObject3D& obj);
489 /** Gets the intersection between a 3D polygon and a 3D line. \sa TObject3D */
490 bool intersect(const TPolygon3D& p1, const TLine3D& r2, TObject3D& obj);
491 /** Gets the intersection between a 3D polygon and a plane. \sa TObject3D */
492 bool intersect(const TPolygon3D& p1, const TPlane& p2, TObject3D& obj);
493 /** Gets the intersection between two 3D polygons. \sa TObject3D */
494 bool intersect(const TPolygon3D& p1, const TPolygon3D& p2, TObject3D& obj);
495 /** Gets the intersection between a 3D segment and a 3D polygon. \sa TObject3D
496  */
497 inline bool intersect(
498  const TSegment3D& s1, const TPolygon3D& p2, TObject3D& obj)
499 {
500  return intersect(p2, s1, obj);
501 }
502 /** Gets the intersection between a 3D line and a 3D polygon.\sa TObject3D */
503 inline bool intersect(const TLine3D& r1, const TPolygon3D& p2, TObject3D& obj)
504 {
505  return intersect(p2, r1, obj);
506 }
507 /** Gets the intersection between a plane and a 3D polygon. \sa TObject3D */
508 inline bool intersect(const TPlane& p1, const TPolygon3D& p2, TObject3D& obj)
509 {
510  return intersect(p2, p1, obj);
511 }
512 
513 /** Gets the intersection between two sets of 3D polygons. The intersection is
514  * returned as an sparse matrix with each pair of polygons' intersections, and
515  * the return value is the amount of intersections found.
516  * \sa TObject3D,CSparseMatrixTemplate */
517 size_t intersect(
518  const std::vector<TPolygon3D>& v1, const std::vector<TPolygon3D>& v2,
519  CSparseMatrixTemplate<TObject3D>& objs);
520 /** Gets the intersection between two sets of 3D polygons. The intersection is
521  * returned as a vector with every intersection found, and the return value is
522  * the amount of intersections found.
523  * \sa TObject3D */
524 size_t intersect(
525  const std::vector<TPolygon3D>& v1, const std::vector<TPolygon3D>& v2,
526  std::vector<TObject3D>& objs);
527 /** @}
528  */
529 
530 /** @name Other intersections
531  @{
532  */
533 /** Gets the intersection between vectors of geometric objects and returns it in
534  * a sparse matrix of either TObject2D or TObject3D.
535  * \sa TObject2D,TObject3D,CSparseMatrix */
536 template <class T, class U, class O>
537 size_t intersect(
538  const std::vector<T>& v1, const std::vector<U>& v2,
540 {
541  size_t M = v1.size(), N = v2.size();
542  O obj;
543  objs.clear();
544  objs.resize(M, N);
545  for (size_t i = 0; i < M; i++)
546  for (size_t j = 0; j < M; j++)
547  if (intersect(v1[i], v2[j], obj)) objs(i, j) = obj;
548  return objs.getNonNullElements();
549 }
550 
551 /** Gets the intersection between vectors of geometric objects and returns it in
552  * a vector of either TObject2D or TObject3D.
553  * \sa TObject2D,TObject3D */
554 template <class T, class U, class O>
555 size_t intersect(
556  const std::vector<T>& v1, const std::vector<U>& v2, std::vector<O> objs)
557 {
558  objs.resize(0);
559  O obj;
560  for (typename std::vector<T>::const_iterator it1 = v1.begin();
561  it1 != v1.end(); ++it1)
562  {
563  const T& elem1 = *it1;
564  for (typename std::vector<U>::const_iterator it2 = v2.begin();
565  it2 != v2.end(); ++it2)
566  if (intersect(elem1, *it2, obj)) objs.push_back(obj);
567  }
568  return objs.size();
569 }
570 
571 /** Gets the intersection between any pair of 2D objects.*/
572 bool intersect(const TObject2D& o1, const TObject2D& o2, TObject2D& obj);
573 /** Gets the intersection between any pair of 3D objects.*/
574 bool intersect(const TObject3D& o1, const TObject3D& o2, TObject3D& obj);
575 /** @}
576  */
577 
578 /** @name Distances
579  @{
580  */
581 /** Gets the distance between two points in a 2D space. */
582 double distance(const TPoint2D& p1, const TPoint2D& p2);
583 /** Gets the distance between two points in a 3D space. */
584 double distance(const TPoint3D& p1, const TPoint3D& p2);
585 /** Gets the distance between two lines in a 2D space. */
586 double distance(const TLine2D& r1, const TLine2D& r2);
587 /** Gets the distance between two lines in a 3D space. */
588 double distance(const TLine3D& r1, const TLine3D& r2);
589 /** Gets the distance between two planes. It will be zero if the planes are not
590  * parallel. */
591 double distance(const TPlane& p1, const TPlane& p2);
592 /** Gets the distance between two polygons in a 2D space. */
593 double distance(const TPolygon2D& p1, const TPolygon2D& p2);
594 /** Gets the distance between a polygon and a segment in a 2D space. */
595 double distance(const TPolygon2D& p1, const TSegment2D& s2);
596 /** Gets the distance between a segment and a polygon in a 2D space. */
597 inline double distance(const TSegment2D& s1, const TPolygon2D& p2)
598 {
599  return distance(p2, s1);
600 }
601 /** Gets the distance between a polygon and a line in a 2D space. */
602 double distance(const TPolygon2D& p1, const TLine2D& l2);
603 inline double distance(const TLine2D& l1, const TPolygon2D& p2)
604 {
605  return distance(p2, l1);
606 }
607 /** Gets the distance between two polygons in a 3D space. */
608 double distance(const TPolygon3D& p1, const TPolygon3D& p2);
609 /** Gets the distance between a polygon and a segment in a 3D space. */
610 double distance(const TPolygon3D& p1, const TSegment3D& s2);
611 /** Gets the distance between a segment and a polygon in a 3D space.*/
612 inline double distance(const TSegment3D& s1, const TPolygon3D& p2)
613 {
614  return distance(p2, s1);
615 }
616 /** Gets the distance between a polygon and a line in a 3D space. */
617 double distance(const TPolygon3D& p1, const TLine3D& l2);
618 /** Gets the distance between a line and a polygon in a 3D space */
619 inline double distance(const TLine3D& l1, const TPolygon3D& p2)
620 {
621  return distance(p2, l1);
622 }
623 /** Gets the distance between a polygon and a plane. */
624 double distance(const TPolygon3D& po, const TPlane& pl);
625 /** Gets the distance between a plane and a polygon.*/
626 inline double distance(const TPlane& pl, const TPolygon3D& po)
627 {
628  return distance(po, pl);
629 }
630 /** @}
631  */
632 
633 /** @name Bound checkers
634  @{
635  */
636 /** Gets the rectangular bounds of a 2D polygon or set of 2D points */
637 void getRectangleBounds(
638  const std::vector<TPoint2D>& poly, TPoint2D& pMin, TPoint2D& pMax);
639 /** Gets the prism bounds of a 3D polygon or set of 3D points. */
640 void getPrismBounds(
641  const std::vector<TPoint3D>& poly, TPoint3D& pMin, TPoint3D& pMax);
642 /** @}
643  */
644 
645 /** @name Creation of planes from poses
646  @{
647  */
648 /**
649  * Given a pose, creates a plane orthogonal to its Z vector.
650  * \sa createPlaneFromPoseXZ,createPlaneFromPoseYZ,createPlaneFromPoseAndNormal
651  */
652 void createPlaneFromPoseXY(const mrpt::math::TPose3D& pose, TPlane& plane);
653 /**
654  * Given a pose, creates a plane orthogonal to its Y vector.
655  * \sa createPlaneFromPoseXY,createPlaneFromPoseYZ,createPlaneFromPoseAndNormal
656  */
657 void createPlaneFromPoseXZ(const mrpt::math::TPose3D& pose, TPlane& plane);
658 /**
659  * Given a pose, creates a plane orthogonal to its X vector.
660  * \sa createPlaneFromPoseXY,createPlaneFromPoseXZ,createPlaneFromPoseAndNormal
661  */
662 void createPlaneFromPoseYZ(const mrpt::math::TPose3D& pose, TPlane& plane);
663 /**
664  * Given a pose and any vector, creates a plane orthogonal to that vector in
665  * the pose's coordinates.
666  * \sa createPlaneFromPoseXY,createPlaneFromPoseXZ,createPlaneFromPoseYZ
667  */
669  const mrpt::math::TPose3D& pose, const double (&normal)[3], TPlane& plane);
670 /**
671  * Creates a homogeneus matrix (4x4) such that the coordinate given (0 for x, 1
672  * for y, 2 for z) corresponds to the provided vector.
673  * \param[in] vec must be a *unitary* vector
674  * \sa generateAxisBaseFromDirectionAndAxis()
675  */
677  const double (&vec)[3], uint8_t coord, CMatrixDouble44& matrix);
678 /** @}
679  */
680 
681 /** @name Linear regression methods
682  @{
683  */
684 /**
685  * Using eigenvalues, gets the best fitting line for a set of 2D points.
686  * Returns an estimation of the error.
687  * \sa spline, leastSquareLinearFit
688  */
689 double getRegressionLine(const std::vector<TPoint2D>& points, TLine2D& line);
690 /**
691  * Using eigenvalues, gets the best fitting line for a set of 3D points.
692  * Returns an estimation of the error.
693  * \sa spline, leastSquareLinearFit
694  */
695 double getRegressionLine(const std::vector<TPoint3D>& points, TLine3D& line);
696 /**
697  * Using eigenvalues, gets the best fitting plane for a set of 3D points.
698  * Returns an estimation of the error.
699  * \sa spline, leastSquareLinearFit
700  */
701 double getRegressionPlane(const std::vector<TPoint3D>& points, TPlane& plane);
702 /** @}
703  */
704 
705 /** @name Miscellaneous Geometry methods
706  @{
707  */
708 /**
709  * Tries to assemble a set of segments into a set of closed polygons.
710  */
711 void assemblePolygons(
712  const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys);
713 /**
714  * Tries to assemble a set of segments into a set of closed polygons, returning
715  * the unused segments as another out parameter.
716  */
717 void assemblePolygons(
718  const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys,
719  std::vector<TSegment3D>& remainder);
720 /**
721  * Extracts all the polygons, including those formed from segments, from the
722  * set of objects.
723  */
724 void assemblePolygons(
725  const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys);
726 /**
727  * Extracts all the polygons, including those formed from segments, from the
728  * set of objects.
729  */
730 void assemblePolygons(
731  const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
732  std::vector<TObject3D>& remainder);
733 /**
734  * Extracts all the polygons, including those formed from segments, from the
735  * set of objects.
736  */
737 void assemblePolygons(
738  const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
739  std::vector<TSegment3D>& remainder1, std::vector<TObject3D>& remainder2);
740 
741 /**
742  * Splits a 2D polygon into convex components.
743  */
745  const TPolygon2D& poly, std::vector<TPolygon2D>& components);
746 /**
747  * Splits a 3D polygon into convex components.
748  * \throw std::logic_error if the polygon can't be fit into a plane.
749  */
751  const TPolygon3D& poly, std::vector<TPolygon3D>& components);
752 
753 /**
754  * Gets the bisector of a 2D segment.
755  */
756 void getSegmentBisector(const TSegment2D& sgm, TLine2D& bis);
757 /**
758  * Gets the bisector of a 3D segment.
759  */
760 void getSegmentBisector(const TSegment3D& sgm, TPlane& bis);
761 /**
762  * Gets the bisector of two lines or segments (implicit constructor will be
763  * used if necessary)
764  */
765 void getAngleBisector(const TLine2D& l1, const TLine2D& l2, TLine2D& bis);
766 /**
767  * Gets the bisector of two lines or segments (implicit constructor will be
768  * used if necessary)
769  * \throw std::logic_error if the lines do not fit in a single plane.
770  */
771 void getAngleBisector(const TLine3D& l1, const TLine3D& l2, TLine3D& bis);
772 
773 /**
774  * Fast ray tracing method using polygons' properties.
775  * \sa CRenderizable::rayTrace
776  */
777 bool traceRay(
778  const std::vector<TPolygonWithPlane>& vec, const mrpt::math::TPose3D& pose,
779  double& dist);
780 /**
781  * Fast ray tracing method using polygons' properties.
782  * \sa CRenderizable::rayTrace
783  */
784 inline bool traceRay(
785  const std::vector<TPolygon3D>& vec, const mrpt::math::TPose3D& pose,
786  double& dist)
787 {
788  std::vector<TPolygonWithPlane> pwp;
790  return traceRay(pwp, pose, dist);
791 }
792 
793 /** Computes the cross product of two 3D vectors, returning a vector normal to
794  both.
795  * It uses the simple implementation:
796 
797  \f[ v_out = \left(
798  \begin{array}{c c c}
799  \hat{i} ~ \hat{j} ~ \hat{k} \\
800  x0 ~ y0 ~ z0 \\
801  x1 ~ y1 ~ z1 \\
802  \end{array} \right)
803  \f]
804  */
805 template <class T, class U, class V>
806 inline void crossProduct3D(const T& v0, const U& v1, V& vOut)
807 {
808  vOut[0] = v0[1] * v1[2] - v0[2] * v1[1];
809  vOut[1] = v0[2] * v1[0] - v0[0] * v1[2];
810  vOut[2] = v0[0] * v1[1] - v0[1] * v1[0];
811 }
812 
813 //! \overload
814 template <class T>
815 inline void crossProduct3D(
816  const std::vector<T>& v0, const std::vector<T>& v1, std::vector<T>& v_out)
817 {
818  ASSERT_(v0.size() == 3);
819  ASSERT_(v1.size() == 3);
820  v_out.resize(3);
821  crossProduct3D<std::vector<T>, std::vector<T>, std::vector<T>>(
822  v0, v1, v_out);
823 }
824 //! overload (returning a vector of size 3 by value).
825 template <class VEC1, class VEC2>
826 inline VEC1 crossProduct3D(const VEC1& v0, const VEC2& v1)
827 {
828  VEC1 vOut;
829  crossProduct3D<VEC1, VEC2, VEC1>(v0, v1, vOut);
830  return vOut;
831 }
833 /** Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array:
834  * \f[ M([x ~ y ~ z]^\top) = \left(
835  * \begin{array}{c c c}
836  * 0 & -z & y \\
837  * z & 0 & -x \\
838  * -y & x & 0
839  * \end{array} \right)
840  * \f]
841  */
842 template <class VECTOR, class MATRIX>
843 inline void skew_symmetric3(const VECTOR& v, MATRIX& M)
844 {
845  ASSERT_(v.size() == 3);
846  M.setSize(3, 3);
847  M(0, 0) = 0;
848  M(0, 1) = -v[2];
849  M(0, 2) = v[1];
850  M(1, 0) = v[2];
851  M(1, 1) = 0;
852  M(1, 2) = -v[0];
853  M(2, 0) = -v[1];
854  M(2, 1) = v[0];
855  M(2, 2) = 0;
856 }
857 //! \overload
858 template <class VECTOR>
859 inline mrpt::math::CMatrixDouble33 skew_symmetric3(const VECTOR& v)
860 {
862  skew_symmetric3(v, M);
863  return M;
864 }
865 
866 /** Computes the negative version of a 3x3 skew symmetric matrix from a 3-vector
867  * or 3-array:
868  * \f[ -M([x ~ y ~ z]^\top) = \left(
869  * \begin{array}{c c c}
870  * 0 & z & -y \\
871  * -z & 0 & x \\
872  * y & -x & 0
873  * \end{array} \right)
874  * \f]
875  */
876 template <class VECTOR, class MATRIX>
877 inline void skew_symmetric3_neg(const VECTOR& v, MATRIX& M)
878 {
879  ASSERT_(v.size() == 3);
880  ASSERT_(M.rows() == 3 && M.cols() == 3);
881  M(0, 0) = 0;
882  M(0, 1) = v[2];
883  M(0, 2) = -v[1];
884  M(1, 0) = -v[2];
885  M(1, 1) = 0;
886  M(1, 2) = v[0];
887  M(2, 0) = v[1];
888  M(2, 1) = -v[0];
889  M(2, 2) = 0;
890 }
891 //! \overload
892 template <class VECTOR>
893 inline mrpt::math::CMatrixDouble33 skew_symmetric3_neg(const VECTOR& v)
894 {
896  skew_symmetric3_neg(v, M);
897  return M;
898 }
899 
900 /**
901  * Returns true if two 2D vectors are parallel. The arguments may be points,
902  * arrays, etc.
903  */
904 template <class T, class U>
905 inline bool vectorsAreParallel2D(const T& v1, const U& v2)
906 {
907  return std::abs(v1[0] * v2[1] - v2[0] * v1[1]) < getEpsilon();
908 }
909 
910 /**
911  * Returns true if two 3D vectors are parallel. The arguments may be points,
912  * arrays, etc.
913  */
914 template <class T, class U>
915 inline bool vectorsAreParallel3D(const T& v1, const U& v2)
916 {
917  if (std::abs(v1[0] * v2[1] - v2[0] * v1[1]) >= getEpsilon()) return false;
918  if (std::abs(v1[1] * v2[2] - v2[1] * v1[2]) >= getEpsilon()) return false;
919  return std::abs(v1[2] * v2[0] - v2[2] * v1[0]) < getEpsilon();
920 }
921 
922 /** Computes the closest point from a given point to a segment.
923  * \sa closestFromPointToLine
924  */
926  double Px, double Py, double x1, double y1, double x2, double y2,
927  double& out_x, double& out_y);
928 
929 /** Computes the closest point from a given point to a (infinite) line.
930  * \sa closestFromPointToSegment
931  */
933  double Px, double Py, double x1, double y1, double x2, double y2,
934  double& out_x, double& out_y);
935 
936 /** Returns the square distance from a point to a line.
937  */
939  double Px, double Py, double x1, double y1, double x2, double y2);
940 
941 /** Returns the distance between 2 points in 2D. */
942 template <typename T>
943 T distanceBetweenPoints(const T x1, const T y1, const T x2, const T y2)
944 {
945  return std::sqrt(square(x1 - x2) + square(y1 - y2));
946 }
948 /** Returns the distance between 2 points in 3D. */
949 template <typename T>
951  const T x1, const T y1, const T z1, const T x2, const T y2, const T z2)
952 {
953  return std::sqrt(square(x1 - x2) + square(y1 - y2) + square(z1 - z2));
954 }
955 
956 /** Returns the square distance between 2 points in 2D. */
957 template <typename T>
958 T distanceSqrBetweenPoints(const T x1, const T y1, const T x2, const T y2)
959 {
960  return square(x1 - x2) + square(y1 - y2);
961 }
962 
963 /** Returns the square distance between 2 points in 3D. */
964 template <typename T>
966  const T x1, const T y1, const T z1, const T x2, const T y2, const T z2)
967 {
968  return square(x1 - x2) + square(y1 - y2) + square(z1 - z2);
969 }
970 
971 /** Computes the closest point from a given point to a segment, and returns that
972  * minimum distance.
973  */
974 template <typename T>
976  const double Px, const double Py, const double x1, const double y1,
977  const double x2, const double y2, T& out_x, T& out_y)
978 {
979  double ox, oy;
980  closestFromPointToSegment(Px, Py, x1, y1, x2, y2, ox, oy);
981  out_x = static_cast<T>(ox);
982  out_y = static_cast<T>(oy);
983  return distanceBetweenPoints(Px, Py, ox, oy);
984 }
985 
986 /** Returns the intersection point, and if it exists, between two segments.
987  */
989  const double x1, const double y1, const double x2, const double y2,
990  const double x3, const double y3, const double x4, const double y4,
991  double& ix, double& iy);
992 
993 /** Returns the intersection point, and if it exists, between two segments.
994  */
996  const double x1, const double y1, const double x2, const double y2,
997  const double x3, const double y3, const double x4, const double y4,
998  float& ix, float& iy);
999 
1000 /** Returns true if the 2D point (px,py) falls INTO the given polygon.
1001  * \sa pointIntoQuadrangle
1002  */
1003 bool pointIntoPolygon2D(
1004  double px, double py, unsigned int polyEdges, const double* poly_xs,
1005  const double* poly_ys);
1006 
1007 /** Specialized method to check whether a point (x,y) falls into a quadrangle.
1008  * \sa pointIntoPolygon2D
1009  */
1010 template <typename T>
1011 bool pointIntoQuadrangle(
1012  T x, T y, T v1x, T v1y, T v2x, T v2y, T v3x, T v3y, T v4x, T v4y)
1013 {
1014  using mrpt::sign;
1015 
1016  const T a1 = atan2(v1y - y, v1x - x);
1017  const T a2 = atan2(v2y - y, v2x - x);
1018  const T a3 = atan2(v3y - y, v3x - x);
1019  const T a4 = atan2(v4y - y, v4x - x);
1020 
1021  // The point is INSIDE iff all the signs of the angles between each vertex
1022  // and the next one are equal.
1023  const T da1 = mrpt::math::wrapToPi(a2 - a1);
1024  const T da2 = mrpt::math::wrapToPi(a3 - a2);
1025  if (sign(da1) != sign(da2)) return false;
1026 
1027  const T da3 = mrpt::math::wrapToPi(a4 - a3);
1028  if (sign(da2) != sign(da3)) return false;
1029 
1030  const T da4 = mrpt::math::wrapToPi(a1 - a4);
1031  return (sign(da3) == sign(da4) && (sign(da4) == sign(da1)));
1032 }
1033 
1034 /** Returns the closest distance of a given 2D point to a polygon, or "0" if the
1035  * point is INTO the polygon or its perimeter.
1036  */
1038  double px, double py, unsigned int polyEdges, const double* poly_xs,
1039  const double* poly_ys);
1040 
1041 /** Calculates the minimum distance between a pair of lines.
1042  The lines are given by:
1043  - Line 1 = P1 + f (P2-P1)
1044  - Line 2 = P3 + f (P4-P3)
1045  The Euclidean distance is returned in "dist", and the mid point between the
1046  lines in (x,y,z)
1047  \return It returns false if there is no solution, i.e. lines are (almost, up
1048  to EPS) parallel.
1049  */
1050 bool minDistBetweenLines(
1051  double p1_x, double p1_y, double p1_z, double p2_x, double p2_y,
1052  double p2_z, double p3_x, double p3_y, double p3_z, double p4_x,
1053  double p4_y, double p4_z, double& x, double& y, double& z, double& dist);
1054 
1055 /** Returns whether two rotated rectangles intersect.
1056  * The first rectangle is not rotated and given by
1057  * (R1_x_min,R1_x_max)-(R1_y_min,R1_y_max).
1058  * The second rectangle is given is a similar way, but it is internally rotated
1059  * according
1060  * to the given coordinates translation
1061  * (R2_pose_x,R2_pose_y,R2_pose_phi(radians)), relative
1062  * to the coordinates system of rectangle 1.
1063  */
1065  double R1_x_min, double R1_x_max, double R1_y_min, double R1_y_max,
1066  double R2_x_min, double R2_x_max, double R2_y_min, double R2_y_max,
1067  double R2_pose_x, double R2_pose_y, double R2_pose_phi);
1068 
1069 /** Computes an axis base (a set of three 3D normal vectors) with the given
1070  vector being the first of them ("X")
1071  * NOTE: Make sure of passing all floats or doubles and that the template of
1072  the receiving matrix is of the same type!
1073  *
1074  * If \f$ d = [ dx ~ dy ~ dz ] \f$ is the input vector, then this function
1075  returns a matrix \f$ M \f$ such as:
1076  *
1077  \f[ M = \left(
1078  \begin{array}{c c c}
1079  v^1_x ~ v^2_x ~ v^3_x \\
1080  v^1_y ~ v^2_y ~ v^3_y \\
1081  v^1_z ~ v^2_z ~ v^3_z
1082  \end{array} \right)
1083  \f]
1084  *
1085  * And the three normal vectors are computed as:
1086  *
1087  * \f[ v^1 = \frac{d}{|d|} \f]
1088  *
1089  * If (dx!=0 or dy!=0):
1090  * \f[ v^2 = \frac{[-dy ~ dx ~ 0 ]}{\sqrt{dx^2+dy^2}} \f]
1091  * otherwise (the direction vector is vertical):
1092  * \f[ v^2 = [1 ~ 0 ~ 0] \f]
1093  *
1094  * And finally, the third vector is the cross product of the others:
1095  *
1096  * \f[ v^3 = v^1 \times v^2 \f]
1097  *
1098  * \return The 3x3 matrix (CMatrixDynamic<T>), containing one vector
1099  per column.
1100  * \except Throws an std::exception on invalid input (i.e. null direction
1101  vector)
1102  * \sa generateAxisBaseFromDirectionAndAxis()
1103  *
1104  * (JLB @ 18-SEP-2007)
1105  */
1106 CMatrixDouble33 generateAxisBaseFromDirection(double dx, double dy, double dz);
1107 
1108 /** @} */ // end of misc. geom. methods
1109 
1110 /** @} */ // end of grouping
1111 
1112 } // namespace mrpt::math
bool RectanglesIntersection(double R1_x_min, double R1_x_max, double R1_y_min, double R1_y_max, double R2_x_min, double R2_x_max, double R2_y_min, double R2_y_max, double R2_pose_x, double R2_pose_y, double R2_pose_phi)
Returns whether two rotated rectangles intersect.
Definition: geometry.cpp:367
void project3D(const TPoint3D &point, const mrpt::math::TPose3D &newXYpose, TPoint3D &newPoint)
Uses the given pose 3D to project a point into a new base.
Definition: geometry.h:330
void closestFromPointToLine(double Px, double Py, double x1, double y1, double x2, double y2, double &out_x, double &out_y)
Computes the closest point from a given point to a (infinite) line.
Definition: geometry.cpp:77
void generateAxisBaseFromDirectionAndAxis(const double(&vec)[3], uint8_t coord, CMatrixDouble44 &matrix)
Creates a homogeneus matrix (4x4) such that the coordinate given (0 for x, 1 for y, 2 for z) corresponds to the provided vector.
Definition: geometry.cpp:2076
bool splitInConvexComponents(const TPolygon2D &poly, std::vector< TPolygon2D > &components)
Splits a 2D polygon into convex components.
Definition: geometry.cpp:2372
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:204
bool traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::math::TPose3D &pose, double &dist)
Fast ray tracing method using polygons&#39; properties.
Definition: geometry.cpp:2565
void skew_symmetric3(const VECTOR &v, MATRIX &M)
Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array: .
Definition: geometry.h:832
size_t getNonNullElements() const
Gets the amount of non-null elements inside the matrix.
void assemblePolygons(const std::vector< TSegment3D > &segms, std::vector< TPolygon3D > &polys)
Tries to assemble a set of segments into a set of closed polygons.
Definition: geometry.cpp:2171
void getSegmentBisector(const TSegment2D &sgm, TLine2D &bis)
Gets the bisector of a 2D segment.
Definition: geometry.cpp:2491
mrpt::math::TPose3D inversePose
Plane&#39;s inverse pose.
Definition: geometry.h:43
void createFromPoseAndVector(const mrpt::math::TPose3D &p, const double(&vector)[3], TLine3D &r)
Gets a 3D line corresponding to any arbitrary vector, in the base given by the pose.
Definition: geometry.cpp:948
Slightly heavyweight type to speed-up calculations with polygons in 3D.
Definition: geometry.h:33
bool vectorsAreParallel2D(const T &v1, const U &v2)
Returns true if two 2D vectors are parallel.
Definition: geometry.h:894
bool minDistBetweenLines(double p1_x, double p1_y, double p1_z, double p2_x, double p2_y, double p2_z, double p3_x, double p3_y, double p3_z, double p4_x, double p4_y, double p4_z, double &x, double &y, double &z, double &dist)
Calculates the minimum distance between a pair of lines.
Definition: geometry.cpp:298
void createPlaneFromPoseXY(const mrpt::math::TPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its Z vector.
Definition: geometry.cpp:2047
double closestSquareDistanceFromPointToLine(double Px, double Py, double x1, double y1, double x2, double y2)
Returns the square distance from a point to a line.
Definition: geometry.cpp:100
Standard type for storing any lightweight 2D type.
Definition: TObject2D.h:24
Standard object for storing any 3D lightweight object.
Definition: TObject3D.h:25
void createPlaneFromPoseAndNormal(const mrpt::math::TPose3D &pose, const double(&normal)[3], TPlane &plane)
Given a pose and any vector, creates a plane orthogonal to that vector in the pose&#39;s coordinates...
Definition: geometry.cpp:2062
bool vectorsAreParallel3D(const T &v1, const U &v2)
Returns true if two 3D vectors are parallel.
Definition: geometry.h:904
void getAngleBisector(const TLine2D &l1, const TLine2D &l2, TLine2D &bis)
Gets the bisector of two lines or segments (implicit constructor will be used if necessary) ...
Definition: geometry.cpp:2513
A sparse matrix container (with cells of any type), with iterators.
void createFromPoseY(const mrpt::math::TPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the Y axis in a given pose.
Definition: geometry.cpp:938
void crossProduct3D(const T &v0, const U &v1, V &vOut)
Computes the cross product of two 3D vectors, returning a vector normal to both.
Definition: geometry.h:795
bool conformAPlane(const std::vector< TPoint3D > &points)
Checks whether this polygon or set of points acceptably fits a plane.
Definition: geometry.cpp:985
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void skew_symmetric3_neg(const VECTOR &v, MATRIX &M)
Computes the negative version of a 3x3 skew symmetric matrix from a 3-vector or 3-array: ...
Definition: geometry.h:866
void getAsPose3D(mrpt::math::TPose3D &outPose) const
Definition: TPlane.cpp:75
CMatrixFixed< double, 3, 3 > CMatrixDouble33
Definition: CMatrixFixed.h:367
bool pointIntoQuadrangle(T x, T y, T v1x, T v1y, T v2x, T v2y, T v3x, T v3y, T v4x, T v4y)
Specialized method to check whether a point (x,y) falls into a quadrangle.
Definition: geometry.h:1000
This base provides a set of functions for maths stuff.
2D segment, consisting of two points.
Definition: TSegment2D.h:20
3D segment, consisting of two points.
Definition: TSegment3D.h:21
T square(const T x)
Inline function for the square of a number.
void getRectangleBounds(const std::vector< TPoint2D > &poly, TPoint2D &pMin, TPoint2D &pMax)
Gets the rectangular bounds of a 2D polygon or set of 2D points.
Definition: geometry.cpp:1902
void createPlaneFromPoseXZ(const mrpt::math::TPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its Y vector.
Definition: geometry.cpp:2052
void getAsPose2D(TPose2D &outPose) const
Definition: TLine2D.cpp:59
TPolygon3D poly
Actual polygon.
Definition: geometry.h:37
3D Plane, represented by its equation
Definition: TPlane.h:22
void composePoint(const TPoint3D &l, TPoint3D &g) const
Definition: TPose3D.cpp:80
int sign(T x)
Returns the sign of X as "1" or "-1".
double getRegressionPlane(const std::vector< TPoint3D > &points, TPlane &plane)
Using eigenvalues, gets the best fitting plane for a set of 3D points.
Definition: geometry.cpp:2146
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:242
void getAsPose2DForcingOrigin(const TPoint2D &origin, TPose2D &outPose) const
Definition: TLine2D.cpp:76
TPoint2D point2
Destiny point.
Definition: TSegment2D.h:30
TPolygon2D poly2D
Polygon, after being projected to the plane using inversePose.
Definition: geometry.h:46
bool pointIntoPolygon2D(double px, double py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys)
Returns true if the 2D point (px,py) falls INTO the given polygon.
Definition: geometry.cpp:233
double getAngle(const TPlane &p1, const TPlane &p2)
Computes the angle between two planes.
Definition: geometry.cpp:862
static void getPlanes(const std::vector< TPolygon3D > &oldPolys, std::vector< TPolygonWithPlane > &newPolys)
Static method for vectors.
Definition: geometry.cpp:626
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
Base template for TPoint2D and TPoint2Df.
Definition: TPoint2D.h:31
TPoint2D point1
Origin point.
Definition: TSegment2D.h:26
bool SegmentsIntersection(const double x1, const double y1, const double x2, const double y2, const double x3, const double y3, const double x4, const double y4, double &ix, double &iy)
Returns the intersection point, and if it exists, between two segments.
Definition: geometry.cpp:120
void clear()
Completely removes all elements, although maintaining the matrix&#39;s size.
TPolygonWithPlane()=default
Basic constructor.
double getRegressionLine(const std::vector< TPoint2D > &points, TLine2D &line)
Using eigenvalues, gets the best fitting line for a set of 2D points.
Definition: geometry.cpp:2103
double getEpsilon()
Gets the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:34
void createFromPoseX(const mrpt::math::TPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the X axis in a given pose.
Definition: geometry.cpp:933
mrpt::math::TPose3D pose
Plane&#39;s pose.
Definition: geometry.h:41
void getPrismBounds(const std::vector< TPoint3D > &poly, TPoint3D &pMin, TPoint3D &pMax)
Gets the prism bounds of a 3D polygon or set of 3D points.
Definition: geometry.cpp:2017
void resize(size_t nRows, size_t nCols)
Changes the size of the matrix.
CMatrixDouble33 generateAxisBaseFromDirection(double dx, double dy, double dz)
Computes an axis base (a set of three 3D normal vectors) with the given vector being the first of the...
Definition: geometry.cpp:2583
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
Lightweight 2D pose.
Definition: TPose2D.h:22
void project2D(const TPoint2D &point, const TPose2D &newXpose, TPoint2D &newPoint)
Uses the given pose 2D to project a point into a new base.
Definition: geometry.cpp:1168
void createFromPoseZ(const mrpt::math::TPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the Z axis in a given pose.
Definition: geometry.cpp:943
void createPlaneFromPoseYZ(const mrpt::math::TPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its X vector.
Definition: geometry.cpp:2057
bool areAligned(const std::vector< TPoint2D > &points)
Checks whether this set of points acceptably fits a 2D line.
Definition: geometry.cpp:1006
T distanceBetweenPoints(const T x1, const T y1, const T x2, const T y2)
Returns the distance between 2 points in 2D.
Definition: geometry.h:932
T distanceSqrBetweenPoints(const T x1, const T y1, const T x2, const T y2)
Returns the square distance between 2 points in 2D.
Definition: geometry.h:947
void closestFromPointToSegment(double Px, double Py, double x1, double y1, double x2, double y2, double &out_x, double &out_y)
Computes the closest point from a given point to a segment.
Definition: geometry.cpp:39
TPlane plane
Plane containing the polygon.
Definition: geometry.h:39
CMatrixFixed< double, 4, 4 > CMatrixDouble44
Definition: CMatrixFixed.h:368
double minimumDistanceFromPointToSegment(const double Px, const double Py, const double x1, const double y1, const double x2, const double y2, T &out_x, T &out_y)
Computes the closest point from a given point to a segment, and returns that minimum distance...
Definition: geometry.h:964
bool intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
Definition: geometry.cpp:634
2D polygon, inheriting from std::vector<TPoint2D>.
Definition: TPolygon2D.h:21
3D polygon, inheriting from std::vector<TPoint3D>
Definition: TPolygon3D.h:19
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1887
double distancePointToPolygon2D(double px, double py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys)
Returns the closest distance of a given 2D point to a polygon, or "0" if the point is INTO the polygo...
Definition: geometry.cpp:264
3D line, represented by a base point and a director vector.
Definition: TLine3D.h:19
2D line without bounds, represented by its equation .
Definition: TLine2D.h:19



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020