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