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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019