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


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013