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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019