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