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



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020