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