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 angle between two 3D lines or segments (implicit constructor will be used if passing a segment instead of a line).
185  */
186  double BASE_IMPEXP getAngle(const TLine3D &r1,const TLine3D &r2);
187  /**
188  * Computes the angle between two 2D lines or segments (implicit constructor will be used if passing a segment instead of a line).
189  */
190  double BASE_IMPEXP getAngle(const TLine2D &r1,const TLine2D &r2);
191  /** @}
192  */
193 
194  /** @name Creation of lines from poses.
195  @{
196  */
197  /**
198  * Gets a 3D line corresponding to the X axis in a given pose. An implicit constructor is used if a TPose3D is given.
199  * \sa createFromPoseY,createFromPoseZ,createFromPoseAndVector
200  */
202  /**
203  * Gets a 3D line corresponding to the Y axis in a given pose. An implicit constructor is used if a TPose3D is given.
204  * \sa createFromPoseX,createFromPoseZ,createFromPoseAndVector
205  */
207  /**
208  * Gets a 3D line corresponding to the Z axis in a given pose. An implicit constructor is used if a TPose3D is given.
209  * \sa createFromPoseX,createFromPoseY,createFromPoseAndVector
210  */
212  /**
213  * 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.
214  * \sa createFromPoseX,createFromPoseY,createFromPoseZ
215  */
216  void BASE_IMPEXP createFromPoseAndVector(const mrpt::poses::CPose3D &p,const double (&vector)[3],TLine3D &r);
217  /**
218  * Gets a 2D line corresponding to the X axis in a given pose. An implicit constructor is used if a CPose2D is given.
219  * \sa createFromPoseY,createFromPoseAndVector
220  */
221  void BASE_IMPEXP createFromPoseX(const TPose2D &p,TLine2D &r);
222  /**
223  * Gets a 2D line corresponding to the Y axis in a given pose. An implicit constructor is used if a CPose2D is given.
224  * \sa createFromPoseX,createFromPoseAndVector
225  */
226  void BASE_IMPEXP createFromPoseY(const TPose2D &p,TLine2D &r);
227  /**
228  * 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.
229  * \sa createFromPoseY,createFromPoseAndVector
230  */
231  void BASE_IMPEXP createFromPoseAndVector(const TPose2D &p,const double (&vector)[2],TLine2D &r);
232  /** @}
233  */
234 
235  /** @name Other line or plane related methods.
236  @{
237  */
238  /**
239  * Checks whether this polygon or set of points acceptably fits a plane.
240  * \sa TPolygon3D,geometryEpsilon
241  */
242  bool BASE_IMPEXP conformAPlane(const std::vector<TPoint3D> &points);
243  /**
244  * Checks whether this polygon or set of points acceptably fits a plane, and if it's the case returns it in the second argument.
245  * \sa TPolygon3D,geometryEpsilon
246  */
247  bool BASE_IMPEXP conformAPlane(const std::vector<TPoint3D> &points,TPlane &p);
248  /**
249  * Checks whether this set of points acceptably fits a 2D line.
250  * \sa geometryEpsilon
251  */
252  bool BASE_IMPEXP areAligned(const std::vector<TPoint2D> &points);
253  /**
254  * Checks whether this set of points acceptably fits a 2D line, and if it's the case returns it in the second argument.
255  * \sa geometryEpsilon
256  */
257  bool BASE_IMPEXP areAligned(const std::vector<TPoint2D> &points,TLine2D &r);
258  /**
259  * Checks whether this set of points acceptably fits a 3D line.
260  * \sa geometryEpsilon
261  */
262  bool BASE_IMPEXP areAligned(const std::vector<TPoint3D> &points);
263  /**
264  * Checks whether this set of points acceptably fits a 3D line, and if it's the case returns it in the second argument.
265  */
266  bool BASE_IMPEXP areAligned(const std::vector<TPoint3D> &points,TLine3D &r);
267  /** @}
268  */
269 
270  /** @name Projections
271  @{
272  */
273  /** Uses the given pose 3D to project a point into a new base */
274  inline void project3D(const TPoint3D &point, const mrpt::poses::CPose3D &newXYpose,TPoint3D &newPoint) {
275  newXYpose.composePoint(point.x,point.y,point.z,newPoint.x,newPoint.y,newPoint.z);
276  }
277  /** Uses the given pose 3D to project a segment into a new base */
278  inline void project3D(const TSegment3D &segment,const mrpt::poses::CPose3D &newXYpose,TSegment3D &newSegment) {
279  project3D(segment.point1,newXYpose,newSegment.point1);
280  project3D(segment.point2,newXYpose,newSegment.point2);
281  }
282 
283  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
284  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
285  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
286  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.
287 
288  /** 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 */
289  template<class T> void project3D(const T &obj,const TPlane &newXYPlane,T &newObj) {
291  TPlane(newXYPlane).getAsPose3D(pose);
292  project3D(obj,-pose,newObj);
293  }
294 
295  /** 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 */
296  template<class T> void project3D(const T &obj,const TPlane &newXYPlane,const TPoint3D &newOrigin,T &newObj) {
298  //TPlane(newXYPlane).getAsPose3DForcingOrigin(newOrigin,pose);
299  TPlane(newXYPlane).getAsPose3D(pose);
300  project3D(obj,-pose,newObj);
301  }
302 
303  /** Projects a set of 3D objects into the plane's base. */
304  template<class T> void project3D(const std::vector<T> &objs,const mrpt::poses::CPose3D &newXYpose,std::vector<T> &newObjs) {
305  size_t N=objs.size();
306  newObjs.resize(N);
307  for (size_t i=0;i<N;i++) project3D(objs[i],newXYpose,newObjs[i]);
308  }
309 
310  /** Uses the given pose 2D to project a point into a new base. */
311  void BASE_IMPEXP project2D(const TPoint2D &point,const mrpt::poses::CPose2D &newXpose,TPoint2D &newPoint);
312 
313  /** Uses the given pose 2D to project a segment into a new base */
314  inline void project2D(const TSegment2D &segment,const mrpt::poses::CPose2D &newXpose,TSegment2D &newSegment) {
315  project2D(segment.point1,newXpose,newSegment.point1);
316  project2D(segment.point2,newXpose,newSegment.point2);
317  }
318 
319 
320  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
321  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.
322  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
324  /** 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.
325  * \tparam CPOSE2D set to mrpt::poses::CPose2D
326  */
327  template<class T,class CPOSE2D> void project2D(const T &obj,const TLine2D &newXLine,T &newObj) {
328  CPOSE2D pose;
329  newXLine.getAsPose2D(pose);
330  project2D(obj,CPOSE2D(0,0,0)-pose,newObj);
331  }
333  /** 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.
334  * \tparam CPOSE2D set to mrpt::poses::CPose2D
335  */
336  template<class T,class CPOSE2D> void project2D(const T &obj,const TLine2D &newXLine,const TPoint2D &newOrigin,T &newObj) {
337  CPOSE2D pose;
338  newXLine.getAsPose2DForcingOrigin(newOrigin,pose);
339  project2D(obj,CPOSE2D(0,0,0)-pose,newObj);
340  }
341 
342  /** Projects a set of 2D objects into the line's base */
343  template<class T> void project2D(const std::vector<T> &objs,const mrpt::poses::CPose2D &newXpose,std::vector<T> &newObjs) {
344  size_t N=objs.size();
345  newObjs.resize(N);
346  for (size_t i=0;i<N;i++) project2D(objs[i],newXpose,newObjs[i]);
347  }
348  /** @}
349  */
350 
351  /** @name Polygon intersections. These operations rely more on spatial reasoning than in raw numerical operations.
352  @{
353  */
354  /** Gets the intersection between a 2D polygon and a 2D segment. \sa TObject2D */
355  bool BASE_IMPEXP intersect(const TPolygon2D &p1,const TSegment2D &s2,TObject2D &obj);
356  /** Gets the intersection between a 2D polygon and a 2D line. \sa TObject2D */
357  bool BASE_IMPEXP intersect(const TPolygon2D &p1,const TLine2D &r2,TObject2D &obj);
358  /** Gets the intersection between two 2D polygons. \sa TObject2D */
359  bool BASE_IMPEXP intersect(const TPolygon2D &p1,const TPolygon2D &p2,TObject2D &obj);
360  /** Gets the intersection between a 2D segment and a 2D polygon. \sa TObject2D */
361  inline bool intersect(const TSegment2D &s1,const TPolygon2D &p2,TObject2D &obj) {
362  return intersect(p2,s1,obj);
363  }
364  /** Gets the intersection between a 2D line and a 2D polygon.\sa TObject2D */
365  inline bool intersect(const TLine2D &r1,const TPolygon2D &p2,TObject2D &obj) {
366  return intersect(p2,r1,obj);
367  }
368  /** Gets the intersection between a 3D polygon and a 3D segment. \sa TObject3D */
369  bool BASE_IMPEXP intersect(const TPolygon3D &p1,const TSegment3D &s2,TObject3D &obj);
370  /** Gets the intersection between a 3D polygon and a 3D line. \sa TObject3D */
371  bool BASE_IMPEXP intersect(const TPolygon3D &p1,const TLine3D &r2,TObject3D &obj);
372  /** Gets the intersection between a 3D polygon and a plane. \sa TObject3D */
373  bool BASE_IMPEXP intersect(const TPolygon3D &p1,const TPlane &p2,TObject3D &obj);
374  /** Gets the intersection between two 3D polygons. \sa TObject3D */
375  bool BASE_IMPEXP intersect(const TPolygon3D &p1,const TPolygon3D &p2,TObject3D &obj);
376  /** Gets the intersection between a 3D segment and a 3D polygon. \sa TObject3D */
377  inline bool intersect(const TSegment3D &s1,const TPolygon3D &p2,TObject3D &obj) {
378  return intersect(p2,s1,obj);
379  }
380  /** Gets the intersection between a 3D line and a 3D polygon.\sa TObject3D */
381  inline bool intersect(const TLine3D &r1,const TPolygon3D &p2,TObject3D &obj) {
382  return intersect(p2,r1,obj);
383  }
384  /** Gets the intersection between a plane and a 3D polygon. \sa TObject3D */
385  inline bool intersect(const TPlane &p1,const TPolygon3D &p2,TObject3D &obj) {
386  return intersect(p2,p1,obj);
387  }
388 
389  /** 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.
390  * \sa TObject3D,CSparseMatrixTemplate */
391  size_t BASE_IMPEXP intersect(const std::vector<TPolygon3D> &v1,const std::vector<TPolygon3D> &v2,CSparseMatrixTemplate<TObject3D> &objs);
392  /** 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.
393  * \sa TObject3D */
394  size_t BASE_IMPEXP intersect(const std::vector<TPolygon3D> &v1,const std::vector<TPolygon3D> &v2,std::vector<TObject3D> &objs);
395  /** @}
396  */
398  /** @name Other intersections
399  @{
400  */
401  /** Gets the intersection between vectors of geometric objects and returns it in a sparse matrix of either TObject2D or TObject3D.
402  * \sa TObject2D,TObject3D,CSparseMatrix */
403  template<class T,class U,class O> size_t intersect(const std::vector<T> &v1,const std::vector<U> &v2,CSparseMatrixTemplate<O> &objs) {
404  size_t M=v1.size(),N=v2.size();
405  O obj;
406  objs.clear();
407  objs.resize(M,N);
408  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;
409  return objs.getNonNullElements();
410  }
411 
412  /** Gets the intersection between vectors of geometric objects and returns it in a vector of either TObject2D or TObject3D.
413  * \sa TObject2D,TObject3D */
414  template<class T,class U,class O> size_t intersect(const std::vector<T> &v1,const std::vector<U> &v2,std::vector<O> objs) {
415  objs.resize(0);
416  O obj;
417  for (typename std::vector<T>::const_iterator it1=v1.begin();it1!=v1.end();++it1) {
418  const T &elem1=*it1;
419  for (typename std::vector<U>::const_iterator it2=v2.begin();it2!=v2.end();++it2) if (intersect(elem1,*it2,obj)) objs.push_back(obj);
420  }
421  return objs.size();
422  }
423 
424  /** Gets the intersection between any pair of 2D objects.*/
425  bool BASE_IMPEXP intersect(const TObject2D &o1,const TObject2D &o2,TObject2D &obj);
426  /** Gets the intersection between any pair of 3D objects.*/
427  bool BASE_IMPEXP intersect(const TObject3D &o1,const TObject3D &o2,TObject3D &obj);
428  /** @}
429  */
430 
431  /** @name Distances
432  @{
433  */
434  double BASE_IMPEXP distance(const TPoint2D &p1,const TPoint2D &p2); //!< Gets the distance between two points in a 2D space.
435  double BASE_IMPEXP distance(const TPoint3D &p1,const TPoint3D &p2); //!< Gets the distance between two points in a 3D space.
436  double BASE_IMPEXP distance(const TLine2D &r1,const TLine2D &r2); //!< Gets the distance between two lines in a 2D space.
437  double BASE_IMPEXP distance(const TLine3D &r1,const TLine3D &r2); //!< Gets the distance between two lines in a 3D space.
438  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.
439  double BASE_IMPEXP distance(const TPolygon2D &p1,const TPolygon2D &p2);//!< Gets the distance between two polygons in a 2D space.
440  double BASE_IMPEXP distance(const TPolygon2D &p1,const TSegment2D &s2);//!< Gets the distance between a polygon and a segment in a 2D space.
441  /** Gets the distance between a segment and a polygon in a 2D space. */
442  inline double distance(const TSegment2D &s1,const TPolygon2D &p2) {
443  return distance(p2,s1);
444  }
445  double BASE_IMPEXP distance(const TPolygon2D &p1,const TLine2D &l2);//!< Gets the distance between a polygon and a line in a 2D space.
446  inline double distance(const TLine2D &l1,const TPolygon2D &p2) {
447  return distance(p2,l1);
448  }
449  double BASE_IMPEXP distance(const TPolygon3D &p1,const TPolygon3D &p2);//!< Gets the distance between two polygons in a 3D space.
450  double BASE_IMPEXP distance(const TPolygon3D &p1,const TSegment3D &s2);//!< Gets the distance between a polygon and a segment in a 3D space.
451  /** Gets the distance between a segment and a polygon in a 3D space.*/
452  inline double distance(const TSegment3D &s1,const TPolygon3D &p2) {
453  return distance(p2,s1);
454  }
455  double BASE_IMPEXP distance(const TPolygon3D &p1,const TLine3D &l2); //!< Gets the distance between a polygon and a line in a 3D space.
456  /** Gets the distance between a line and a polygon in a 3D space */
457  inline double distance(const TLine3D &l1,const TPolygon3D &p2) {
458  return distance(p2,l1);
459  }
460  double BASE_IMPEXP distance(const TPolygon3D &po,const TPlane &pl);//!< Gets the distance between a polygon and a plane.
461  /** Gets the distance between a plane and a polygon.*/
462  inline double distance(const TPlane &pl,const TPolygon3D &po) {
463  return distance(po,pl);
464  }
465  /** @}
466  */
467 
468  /** @name Bound checkers
469  @{
470  */
471  /** Gets the rectangular bounds of a 2D polygon or set of 2D points */
472  void BASE_IMPEXP getRectangleBounds(const std::vector<TPoint2D> &poly,TPoint2D &pMin,TPoint2D &pMax);
473  /** Gets the prism bounds of a 3D polygon or set of 3D points. */
474  void BASE_IMPEXP getPrismBounds(const std::vector<TPoint3D> &poly,TPoint3D &pMin,TPoint3D &pMax);
475  /** @}
476  */
477 
478  /** @name Creation of planes from poses
479  @{
480  */
481  /**
482  * Given a pose, creates a plane orthogonal to its Z vector.
483  * \sa createPlaneFromPoseXZ,createPlaneFromPoseYZ,createPlaneFromPoseAndNormal
484  */
486  /**
487  * Given a pose, creates a plane orthogonal to its Y vector.
488  * \sa createPlaneFromPoseXY,createPlaneFromPoseYZ,createPlaneFromPoseAndNormal
489  */
491  /**
492  * Given a pose, creates a plane orthogonal to its X vector.
493  * \sa createPlaneFromPoseXY,createPlaneFromPoseXZ,createPlaneFromPoseAndNormal
494  */
496  /**
497  * Given a pose and any vector, creates a plane orthogonal to that vector in the pose's coordinates.
498  * \sa createPlaneFromPoseXY,createPlaneFromPoseXZ,createPlaneFromPoseYZ
499  */
500  void BASE_IMPEXP createPlaneFromPoseAndNormal(const mrpt::poses::CPose3D &pose,const double (&normal)[3],TPlane &plane);
501  /**
502  * Creates a homogeneus matrix (4x4) such that the coordinate given (0 for x, 1 for y, 2 for z) corresponds to the provided vector.
503  * \param[in] vec must be a *unitary* vector
504  * \sa generateAxisBaseFromDirectionAndAxis()
505  */
507  /** @}
508  */
509 
510  /** @name Linear regression methods
511  @{
512  */
513  /**
514  * Using eigenvalues, gets the best fitting line for a set of 2D points. Returns an estimation of the error.
515  * \sa spline, leastSquareLinearFit
516  */
517  double BASE_IMPEXP getRegressionLine(const std::vector<TPoint2D> &points,TLine2D &line);
518  /**
519  * Using eigenvalues, gets the best fitting line for a set of 3D points. Returns an estimation of the error.
520  * \sa spline, leastSquareLinearFit
521  */
522  double BASE_IMPEXP getRegressionLine(const std::vector<TPoint3D> &points,TLine3D &line);
523  /**
524  * Using eigenvalues, gets the best fitting plane for a set of 3D points. Returns an estimation of the error.
525  * \sa spline, leastSquareLinearFit
526  */
527  double BASE_IMPEXP getRegressionPlane(const std::vector<TPoint3D> &points,TPlane &plane);
528  /** @}
529  */
530 
531  /** @name Miscellaneous Geometry methods
532  @{
533  */
534  /**
535  * Tries to assemble a set of segments into a set of closed polygons.
536  */
537  void BASE_IMPEXP assemblePolygons(const std::vector<TSegment3D> &segms,std::vector<TPolygon3D> &polys);
538  /**
539  * Tries to assemble a set of segments into a set of closed polygons, returning the unused segments as another out parameter.
540  */
541  void BASE_IMPEXP assemblePolygons(const std::vector<TSegment3D> &segms,std::vector<TPolygon3D> &polys,std::vector<TSegment3D> &remainder);
542  /**
543  * Extracts all the polygons, including those formed from segments, from the set of objects.
544  */
545  void BASE_IMPEXP assemblePolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys);
546  /**
547  * Extracts all the polygons, including those formed from segments, from the set of objects.
548  */
549  void BASE_IMPEXP assemblePolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder);
550  /**
551  * Extracts all the polygons, including those formed from segments, from the set of objects.
552  */
553  void BASE_IMPEXP assemblePolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TSegment3D> &remainder1,std::vector<TObject3D> &remainder2);
554 
555  /**
556  * Changes the value of the geometric epsilon.
557  * \sa geometryEpsilon,getEpsilon
558  */
559  inline void setEpsilon(double nE) {
560  geometryEpsilon=nE;
561  }
562  /**
563  * Gets the value of the geometric epsilon.
564  * \sa geometryEpsilon,setEpsilon
565  */
566  inline double getEpsilon() {
567  return geometryEpsilon;
568  }
569 
570  /**
571  * Splits a 2D polygon into convex components.
572  */
573  bool BASE_IMPEXP splitInConvexComponents(const TPolygon2D &poly,std::vector<TPolygon2D> &components);
574  /**
575  * Splits a 3D polygon into convex components.
576  * \throw std::logic_error if the polygon can't be fit into a plane.
577  */
578  bool BASE_IMPEXP splitInConvexComponents(const TPolygon3D &poly,std::vector<TPolygon3D> &components);
579 
580  /**
581  * Gets the bisector of a 2D segment.
582  */
583  void BASE_IMPEXP getSegmentBisector(const TSegment2D &sgm,TLine2D &bis);
584  /**
585  * Gets the bisector of a 3D segment.
586  */
587  void BASE_IMPEXP getSegmentBisector(const TSegment3D &sgm,TPlane &bis);
588  /**
589  * Gets the bisector of two lines or segments (implicit constructor will be used if necessary)
590  */
591  void BASE_IMPEXP getAngleBisector(const TLine2D &l1,const TLine2D &l2,TLine2D &bis);
592  /**
593  * Gets the bisector of two lines or segments (implicit constructor will be used if necessary)
594  * \throw std::logic_error if the lines do not fit in a single plane.
595  */
596  void BASE_IMPEXP getAngleBisector(const TLine3D &l1,const TLine3D &l2,TLine3D &bis);
597 
598  /**
599  * Fast ray tracing method using polygons' properties.
600  * \sa CRenderizable::rayTrace
601  */
602  bool BASE_IMPEXP traceRay(const std::vector<TPolygonWithPlane> &vec,const mrpt::poses::CPose3D &pose,double &dist);
603  /**
604  * Fast ray tracing method using polygons' properties.
605  * \sa CRenderizable::rayTrace
606  */
607  inline bool traceRay(const std::vector<TPolygon3D> &vec,const mrpt::poses::CPose3D &pose,double &dist) {
608  std::vector<TPolygonWithPlane> pwp;
610  return traceRay(pwp,pose,dist);
611  }
612 
613  /** Computes the cross product of two 3D vectors, returning a vector normal to both.
614  * It uses the simple implementation:
615 
616  \f[ v_out = \left(
617  \begin{array}{c c c}
618  \hat{i} ~ \hat{j} ~ \hat{k} \\
619  x0 ~ y0 ~ z0 \\
620  x1 ~ y1 ~ z1 \\
621  \end{array} \right)
622  \f]
623  */
624  template<class T,class U,class V>
625  inline void crossProduct3D(const T &v0,const U &v1,V& vOut) {
626  vOut[0]=v0[1]*v1[2]-v0[2]*v1[1];
627  vOut[1]=v0[2]*v1[0]-v0[0]*v1[2];
628  vOut[2]=v0[0]*v1[1]-v0[1]*v1[0];
629  }
630 
631  //! \overload
632  template<class T>
633  inline void crossProduct3D(
634  const std::vector<T> &v0,
635  const std::vector<T> &v1,
636  std::vector<T> &v_out )
637  {
638  ASSERT_(v0.size()==3)
639  ASSERT_(v1.size()==3);
640  v_out.resize(3);
641  v_out[0] = v0[1]*v1[2] - v0[2]*v1[1];
642  v_out[1] = -v0[0]*v1[2] + v0[2]*v1[0];
643  v_out[2] = v0[0]*v1[1] - v0[1]*v1[0];
644  }
645 
646  //! overload (returning a vector of size 3 by value).
647  template<class VEC1,class VEC2>
648  inline Eigen::Matrix<double,3,1> crossProduct3D(const VEC1 &v0,const VEC2 &v1) {
649  Eigen::Matrix<double,3,1> vOut;
650  vOut[0]=v0[1]*v1[2]-v0[2]*v1[1];
651  vOut[1]=v0[2]*v1[0]-v0[0]*v1[2];
652  vOut[2]=v0[0]*v1[1]-v0[1]*v1[0];
653  return vOut;
654  }
656  /** Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array:
657  * \f[ M([x ~ y ~ z]^\top) = \left(
658  * \begin{array}{c c c}
659  * 0 & -z & y \\
660  * z & 0 & -x \\
661  * -y & x & 0
662  * \end{array} \right)
663  * \f]
664  */
665  template<class VECTOR,class MATRIX>
666  inline void skew_symmetric3(const VECTOR &v,MATRIX& M) {
667  ASSERT_(v.size()==3)
668  M.setSize(3,3);
669  M.set_unsafe(0,0, 0); M.set_unsafe(0,1, -v[2]); M.set_unsafe(0,2, v[1]);
670  M.set_unsafe(1,0, v[2]); M.set_unsafe(1,1, 0); M.set_unsafe(1,2, -v[0]);
671  M.set_unsafe(2,0, -v[1]); M.set_unsafe(2,1, v[0]); M.set_unsafe(2,2, 0);
672  }
673  //! \overload
674  template<class VECTOR>
675  inline mrpt::math::CMatrixDouble33 skew_symmetric3(const VECTOR &v) {
677  skew_symmetric3(v,M);
678  return M;
679  }
681  /** Computes the negative version of a 3x3 skew symmetric matrix from a 3-vector or 3-array:
682  * \f[ -M([x ~ y ~ z]^\top) = \left(
683  * \begin{array}{c c c}
684  * 0 & z & -y \\
685  * -z & 0 & x \\
686  * y & -x & 0
687  * \end{array} \right)
688  * \f]
689  */
690  template<class VECTOR,class MATRIX>
691  inline void skew_symmetric3_neg(const VECTOR &v,MATRIX& M) {
692  ASSERT_(v.size()==3)
693  M.setSize(3,3);
694  M.set_unsafe(0,0, 0); M.set_unsafe(0,1, v[2]); M.set_unsafe(0,2, -v[1]);
695  M.set_unsafe(1,0, -v[2]); M.set_unsafe(1,1, 0); M.set_unsafe(1,2, v[0]);
696  M.set_unsafe(2,0, v[1]); M.set_unsafe(2,1, -v[0]); M.set_unsafe(2,2, 0);
697  }
698  //! \overload
699  template<class VECTOR>
700  inline mrpt::math::CMatrixDouble33 skew_symmetric3_neg(const VECTOR &v) {
702  skew_symmetric3_neg(v,M);
703  return M;
704  }
705 
706 
707  /**
708  * Returns true if two 2D vectors are parallel. The arguments may be points, arrays, etc.
709  */
710  template<class T,class U> inline bool vectorsAreParallel2D(const T &v1,const U &v2) {
711  return abs(v1[0]*v2[1]-v2[0]*v1[1])<geometryEpsilon;
712  }
713 
714  /**
715  * Returns true if two 3D vectors are parallel. The arguments may be points, arrays, etc.
716  */
717  template<class T,class U>
718  inline bool vectorsAreParallel3D(const T &v1,const U &v2) {
719  if (abs(v1[0]*v2[1]-v2[0]*v1[1])>=geometryEpsilon) return false;
720  if (abs(v1[1]*v2[2]-v2[1]*v1[2])>=geometryEpsilon) return false;
721  return abs(v1[2]*v2[0]-v2[2]*v1[0])<geometryEpsilon;
722  }
723 
724  /** Computes the closest point from a given point to a segment.
725  * \sa closestFromPointToLine
726  */
728  const double & Px,
729  const double & Py,
730  const double & x1,
731  const double & y1,
732  const double & x2,
733  const double & y2,
734  double &out_x,
735  double &out_y);
736 
737  /** Computes the closest point from a given point to a (infinite) line.
738  * \sa closestFromPointToSegment
739  */
741  const double & Px,
742  const double & Py,
743  const double & x1,
744  const double & y1,
745  const double & x2,
746  const double & y2,
747  double &out_x,
748  double &out_y);
749 
750  /** Returns the square distance from a point to a line.
751  */
753  const double & Px,
754  const double & Py,
755  const double & x1,
756  const double & y1,
757  const double & x2,
758  const double & y2 );
759 
760 
761  /** Returns the distance between 2 points in 2D. */
762  template <typename T>
763  T distanceBetweenPoints(const T x1,const T y1,const T x2,const T y2) {
764  return std::sqrt( square(x1-x2)+square(y1-y2) );
765  }
766 
767  /** Returns the distance between 2 points in 3D. */
768  template <typename T>
769  T distanceBetweenPoints(const T x1,const T y1,const T z1, const T x2,const T y2, const T z2) {
770  return std::sqrt( square(x1-x2)+square(y1-y2)+square(z1-z2) );
771  }
772 
773  /** Returns the square distance between 2 points in 2D. */
774  template <typename T>
775  T distanceSqrBetweenPoints(const T x1,const T y1,const T x2,const T y2) {
776  return square(x1-x2)+square(y1-y2);
777  }
778 
779  /** Returns the square distance between 2 points in 3D. */
780  template <typename T>
781  T distanceSqrBetweenPoints(const T x1,const T y1,const T z1, const T x2,const T y2, const T z2) {
782  return square(x1-x2)+square(y1-y2)+square(z1-z2);
783  }
784 
785  /** Computes the closest point from a given point to a segment, and returns that minimum distance.
786  */
787  template <typename T>
789  const double Px,
790  const double Py,
791  const double x1,
792  const double y1,
793  const double x2,
794  const double y2,
795  T & out_x,
796  T & out_y)
797  {
798  double ox, oy;
799  closestFromPointToSegment(Px, Py, x1, y1, x2, y2, ox, oy);
800  out_x = static_cast<T>(ox);
801  out_y = static_cast<T>(oy);
802  return distanceBetweenPoints(Px, Py, ox, oy);
803  }
804 
805 
806  /** Returns the intersection point, and if it exists, between two segments.
807  */
809  const double x1,const double y1,
810  const double x2,const double y2,
811  const double x3,const double y3,
812  const double x4,const double y4,
813  double &ix,double &iy);
814 
815  /** Returns the intersection point, and if it exists, between two segments.
816  */
818  const double x1,const double y1,
819  const double x2,const double y2,
820  const double x3,const double y3,
821  const double x4,const double y4,
822  float &ix,float &iy);
823 
824  /** Returns true if the 2D point (px,py) falls INTO the given polygon.
825  * \sa pointIntoQuadrangle
826  */
827  bool BASE_IMPEXP pointIntoPolygon2D(const double & px, const double & py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys );
828 
829  /** Specialized method to check whether a point (x,y) falls into a quadrangle.
830  * \sa pointIntoPolygon2D
831  */
832  template <typename T>
833  bool pointIntoQuadrangle(
834  T x, T y,
835  T v1x, T v1y,
836  T v2x, T v2y,
837  T v3x, T v3y,
838  T v4x, T v4y )
839  {
840  using mrpt::utils::sign;
841 
842  const T a1 = atan2( v1y - y , v1x - x );
843  const T a2 = atan2( v2y - y , v2x - x );
844  const T a3 = atan2( v3y - y , v3x - x );
845  const T a4 = atan2( v4y - y , v4x - x );
846 
847  // The point is INSIDE iff all the signs of the angles between each vertex
848  // and the next one are equal.
849  const T da1 = mrpt::math::wrapToPi( a2-a1 );
850  const T da2 = mrpt::math::wrapToPi( a3-a2 );
851  if (sign(da1)!=sign(da2)) return false;
852 
853  const T da3 = mrpt::math::wrapToPi( a4-a3 );
854  if (sign(da2)!=sign(da3)) return false;
855 
856  const T da4 = mrpt::math::wrapToPi( a1-a4 );
857  return (sign(da3)==sign(da4) && (sign(da4)==sign(da1)));
858  }
859 
860  /** Returns the closest distance of a given 2D point to a polygon, or "0" if the point is INTO the polygon or its perimeter.
861  */
862  double BASE_IMPEXP distancePointToPolygon2D(const double & px, const double & py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys );
863 
864  /** Calculates the minimum distance between a pair of lines.
865  The lines are given by:
866  - Line 1 = P1 + f (P2-P1)
867  - Line 2 = P3 + f (P4-P3)
868  The Euclidean distance is returned in "dist", and the mid point between the lines in (x,y,z)
869  \return It returns false if there is no solution, i.e. lines are (almost, up to EPS) parallel.
870  */
872  const double & p1_x, const double & p1_y, const double & p1_z,
873  const double & p2_x, const double & p2_y, const double & p2_z,
874  const double & p3_x, const double & p3_y, const double & p3_z,
875  const double & p4_x, const double & p4_y, const double & p4_z,
876  double &x, double &y, double &z,
877  double &dist);
878 
879  /** Returns whether two rotated rectangles intersect.
880  * The first rectangle is not rotated and given by (R1_x_min,R1_x_max)-(R1_y_min,R1_y_max).
881  * The second rectangle is given is a similar way, but it is internally rotated according
882  * to the given coordinates translation (R2_pose_x,R2_pose_y,R2_pose_phi(radians)), relative
883  * to the coordinates system of rectangle 1.
884  */
886  const double & R1_x_min, const double & R1_x_max,
887  const double & R1_y_min, const double & R1_y_max,
888  const double & R2_x_min, const double & R2_x_max,
889  const double & R2_y_min, const double & R2_y_max,
890  const double & R2_pose_x,
891  const double & R2_pose_y,
892  const double & R2_pose_phi );
893 
894  /** Computes an axis base (a set of three 3D normal vectors) with the given vector being the first of them ("X")
895  * NOTE: Make sure of passing all floats or doubles and that the template of the receiving matrix is of the same type!
896  *
897  * If \f$ d = [ dx ~ dy ~ dz ] \f$ is the input vector, then this function returns a matrix \f$ M \f$ such as:
898  *
899  \f[ M = \left(
900  \begin{array}{c c c}
901  v^1_x ~ v^2_x ~ v^3_x \\
902  v^1_y ~ v^2_y ~ v^3_y \\
903  v^1_z ~ v^2_z ~ v^3_z
904  \end{array} \right)
905  \f]
906  *
907  * And the three normal vectors are computed as:
908  *
909  * \f[ v^1 = \frac{d}{|d|} \f]
910  *
911  * If (dx!=0 or dy!=0):
912  * \f[ v^2 = \frac{[-dy ~ dx ~ 0 ]}{\sqrt{dx^2+dy^2}} \f]
913  * otherwise (the direction vector is vertical):
914  * \f[ v^2 = [1 ~ 0 ~ 0] \f]
915  *
916  * And finally, the third vector is the cross product of the others:
917  *
918  * \f[ v^3 = v^1 \times v^2 \f]
919  *
920  * \return The 3x3 matrix (CMatrixTemplateNumeric<T>), containing one vector per column.
921  * \except Throws an std::exception on invalid input (i.e. null direction vector)
922  * \sa generateAxisBaseFromDirectionAndAxis()
923  *
924  * (JLB @ 18-SEP-2007)
925  */
926  template<class T>
928  {
929  MRPT_START
930 
931  if (dx==0 && dy==0 && dz==0)
932  THROW_EXCEPTION("Invalid input: Direction vector is (0,0,0)!");
933 
935 
936  // 1st vector:
937  T n_xy = square(dx)+square(dy);
938  T n = sqrt(n_xy+square(dz));
939  n_xy = sqrt(n_xy);
940  P(0,0) = dx / n;
941  P(1,0) = dy / n;
942  P(2,0) = dz / n;
943 
944  // 2nd perpendicular vector:
945  if (fabs(dx)>1e-4 || fabs(dy)>1e-4)
946  {
947  P(0,1) = -dy / n_xy;
948  P(1,1) = dx / n_xy;
949  P(2,1) = 0;
950  }
951  else
952  {
953  // Any vector in the XY plane will work:
954  P(0,1) = 1;
955  P(1,1) = 0;
956  P(2,1) = 0;
957  }
958 
959  // 3rd perpendicular vector: cross product of the two last vectors:
960  P.col(2) = crossProduct3D(P.col(0),P.col(1));
961 
962  return P;
963  MRPT_END
964  }
965 
966  /** @} */ // end of misc. geom. methods
967 
968  /** @} */ // end of grouping
969 
970  } // End of namespace
971 
972 } // End of namespace
973 #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:1642
const GLdouble * v
Definition: glew.h:1296
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
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:1861
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:1652
void getPlanes(const std::vector< TPolygon3D > &polys, std::vector< TPlane > &planes)
Definition: geometry.cpp:1351
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
void skew_symmetric3(const VECTOR &v, MATRIX &M)
Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array: .
Definition: geometry.h:655
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
GLuint coord
Definition: glew.h:4333
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:1996
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:1727
void BASE_IMPEXP getSegmentBisector(const TSegment2D &sgm, TLine2D &bis)
Gets the bisector of a 2D segment.
Definition: geometry.cpp:1936
#define THROW_EXCEPTION(msg)
GLfloat GLfloat v1
Definition: glew.h:1759
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:699
void getAsPose2D(mrpt::poses::CPose2D &outPose) const
Get a pose2D whose X axis corresponds to the line.
GLfloat GLfloat GLfloat v2
Definition: glew.h:1763
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:800
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.
bool vectorsAreParallel3D(const T &v1, const U &v2)
Returns true if two 3D vectors are parallel.
Definition: geometry.h:707
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:1955
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:968
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:792
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:614
TPoint3D point1
Origin point.
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:829
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:788
TPolygonWithPlane()
Basic constructor.
Definition: geometry.h:42
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.
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:680
GLuint GLenum GLsizei GLsizei GLboolean void * points
Definition: glew.h:7748
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:822
2D segment, consisting of two points.
GLsizei n
Definition: glew.h:5051
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:1630
#define MRPT_END
TPoint3D point2
Destiny point.
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
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:1634
size_t getNonNullElements() const
Gets the amount of non-null elements inside the matrix.
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:1524
TPolygon3D poly
Actual polygon.
Definition: geometry.h:35
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
void setEpsilon(double nE)
Changes the value of the geometric epsilon.
Definition: geometry.h:548
3D Plane, represented by its equation
GLhandleARB obj
Definition: glew.h:3276
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:1710
TPoint2D point2
Destiny point.
TPolygon2D poly2D
Polygon, after being projected to the plane using inversePose.
Definition: geometry.h:39
GLfloat GLfloat p
Definition: glew.h:10113
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
GLenum GLenum GLuint components
Definition: glew.h:7127
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
GLuint GLenum matrix
Definition: glew.h:8832
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
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLdouble GLdouble z
Definition: glew.h:1464
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:916
GLfloat v0
Definition: glew.h:1755
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:1674
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
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:1606
double getEpsilon()
Gets the value of the geometric epsilon.
Definition: geometry.h:555
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:270
Lightweight 2D pose.
GLdouble GLdouble GLdouble r
Definition: glew.h:1311
#define ASSERT_(f)
mrpt::poses::CPose3D inversePose
Plane&#39;s inverse pose.
Definition: geometry.h:38
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:1638
bool BASE_IMPEXP areAligned(const std::vector< TPoint2D > &points)
Checks whether this set of points acceptably fits a 2D line.
Definition: geometry.cpp:847
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:752
INT64 INT64 INT64 remainder
Definition: wglew.h:919
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:796
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:764
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:777
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:1511
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.11 for MRPT 1.5.7 Git: 2190203 Tue May 15 02:01:15 2018 +0200 at miƩ may 16 12:40:16 CEST 2018