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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 1de0e027c Sat Sep 14 16:15:22 2019 +0200 at sáb sep 14 16:20:14 CEST 2019