Main MRPT website > C++ reference
MRPT logo

geometry.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef  GEO_H
00029 #define  GEO_H
00030 
00031 #include <mrpt/utils/utils_defs.h>
00032 #include <mrpt/math/CMatrixTemplateNumeric.h>
00033 #include <mrpt/poses/CPoint2D.h>
00034 #include <mrpt/poses/CPoint3D.h>
00035 #include <mrpt/poses/CPose2D.h>
00036 #include <mrpt/poses/CPose3D.h>
00037 #include <mrpt/math/lightweight_geom_data.h>
00038 #include <mrpt/math/CSparseMatrixTemplate.h>
00039 #include <mrpt/math/utils.h>
00040 
00041 /*---------------------------------------------------------------
00042         Class
00043   ---------------------------------------------------------------*/
00044 namespace mrpt
00045 {
00046         namespace math
00047         {
00048                 using std::vector;
00049                 using namespace mrpt::utils;
00050                 using namespace mrpt::poses;
00051 
00052                 /**
00053                   * Global epsilon to overcome small precision errors
00054                   */
00055                 extern double BASE_IMPEXP geometryEpsilon;
00056 
00057                 /**
00058                   * Slightly heavyweight type to speed-up calculations with polygons in 3D
00059                   * \sa TPolygon3D,TPlane
00060                   */
00061                 class BASE_IMPEXP TPolygonWithPlane     {
00062                 public:
00063                         /**
00064                           * Actual polygon.
00065                           */
00066                         TPolygon3D poly;
00067                         /**
00068                           * Plane containing the polygon.
00069                           */
00070                         TPlane plane;
00071                         /**
00072                           * Plane's pose.
00073                           * \sa inversePose
00074                           */
00075                         mrpt::poses::CPose3D pose;
00076                         /**
00077                           * Plane's inverse pose.
00078                           * \sa pose
00079                           */
00080                         mrpt::poses::CPose3D inversePose;
00081                         /**
00082                           * Polygon, after being projected to the plane using inversePose.
00083                           * \sa inversePose
00084                           */
00085                         TPolygon2D poly2D;
00086                         /**
00087                           * Constructor. Takes a polygon and computes each parameter.
00088                           */
00089                         TPolygonWithPlane(const TPolygon3D &p);
00090                         /**
00091                           * Basic constructor. Needed to create containers.
00092                           * \sa TPolygonWithPlane(const TPolygon3D &)
00093                           */
00094                         TPolygonWithPlane()     {}
00095                         /**
00096                           * Static method for vectors. Takes a set of polygons and creates every TPolygonWithPlane
00097                           */
00098                         static void getPlanes(const vector<TPolygon3D> &oldPolys,vector<TPolygonWithPlane> &newPolys);
00099                 };
00100 
00101                 /** @name Simple intersection operations, relying basically on geometrical operations.
00102                         @{
00103                  */
00104                 /**
00105                   * Gets the intersection between two 3D segments.
00106                   * \sa TObject3D
00107                   */
00108                 bool BASE_IMPEXP intersect(const TSegment3D &s1,const TSegment3D &s2,TObject3D &obj);
00109                 /**
00110                   * Gets the intersection between a 3D segment and a plane.
00111                   * \sa TObject3D
00112                   */
00113                 bool BASE_IMPEXP intersect(const TSegment3D &s1,const TPlane &p2,TObject3D &obj);
00114                 /**
00115                   * Gets the intersection between a 3D segment and a 3D line.
00116                   * \sa TObject3D
00117                   */
00118                 bool BASE_IMPEXP intersect(const TSegment3D &s1,const TLine3D &r2,TObject3D &obj);
00119                 /**
00120                   * Gets the intersection between a plane and a 3D segment.
00121                   * \sa TObject3D
00122                   */
00123                 inline bool intersect(const TPlane &p1,const TSegment3D &s2,TObject3D &obj)     {
00124                         return intersect(s2,p1,obj);
00125                 }
00126                 /**
00127                   * Gets the intersection between two planes.
00128                   * \sa TObject3D
00129                   */
00130                 bool BASE_IMPEXP intersect(const TPlane &p1,const TPlane &p2,TObject3D &obj);
00131                 /**
00132                   * Gets the intersection between a plane and a 3D line.
00133                   * \sa TObject3D
00134                   */
00135                 bool BASE_IMPEXP intersect(const TPlane &p1,const TLine3D &p2,TObject3D &obj);
00136                 /**
00137                   * Gets the intersection between a 3D line and a 3D segment.
00138                   * \sa TObject3D
00139                   */
00140                 inline bool intersect(const TLine3D &r1,const TSegment3D &s2,TObject3D &obj)    {
00141                         return intersect(s2,r1,obj);
00142                 }
00143                 /**
00144                   * Gets the intersection between a 3D line and a plane.
00145                   * \sa TObject3D
00146                   */
00147                 inline bool intersect(const TLine3D &r1,const TPlane &p2,TObject3D &obj)        {
00148                         return intersect(p2,r1,obj);
00149                 }
00150                 /**
00151                   * Gets the intersection between two 3D lines.
00152                   * \sa TObject3D
00153                   */
00154                 bool BASE_IMPEXP intersect(const TLine3D &r1,const TLine3D &r2,TObject3D &obj);
00155                 /**
00156                   * Gets the intersection between two 2D lines.
00157                   * \sa TObject2D
00158                   */
00159                 bool BASE_IMPEXP intersect(const TLine2D &r1,const TLine2D &r2,TObject2D &obj);
00160                 /**
00161                   * Gets the intersection between a 2D line and a 2D segment.
00162                   * \sa TObject2D
00163                   */
00164                 bool BASE_IMPEXP intersect(const TLine2D &r1,const TSegment2D &s2,TObject2D &obj);
00165                 /**
00166                   * Gets the intersection between a 2D line and a 2D segment.
00167                   * \sa TObject2D
00168                   */
00169                 inline bool intersect(const TSegment2D &s1,const TLine2D &r2,TObject2D &obj)    {
00170                         return intersect(r2,s1,obj);
00171                 }
00172                 /**
00173                   * Gets the intersection between two 2D segments.
00174                   * \sa TObject2D
00175                   */
00176                 bool BASE_IMPEXP intersect(const TSegment2D &s1,const TSegment2D &s2,TObject2D &obj);
00177                 /** @}
00178                  */
00179 
00180                 /** @name Angle retrieval methods. Methods which use TSegments will automatically use TLines' implicit constructors.
00181                         @{
00182                  */
00183                 /**
00184                   * Computes the angle between two planes.
00185                   */
00186                 double BASE_IMPEXP getAngle(const TPlane &p1,const TPlane &p2);
00187                 /**
00188                   * 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).
00189                   */
00190                 double BASE_IMPEXP getAngle(const TPlane &p1,const TLine3D &r2);
00191                 /**
00192                   * 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).
00193                   */
00194                 inline double getAngle(const TLine3D &r1,const TPlane &p2)      {
00195                         return getAngle(p2,r1);
00196                 }
00197                 /**
00198                   * Computes the angle between two 3D lines or segments (implicit constructor will be used if passing a segment instead of a line).
00199                   */
00200                 double BASE_IMPEXP getAngle(const TLine3D &r1,const TLine3D &r2);
00201                 /**
00202                   * Computes the angle between two 2D lines or segments (implicit constructor will be used if passing a segment instead of a line).
00203                   */
00204                 double BASE_IMPEXP getAngle(const TLine2D &r1,const TLine2D &r2);
00205                 /** @}
00206                  */
00207 
00208                 /** @name Creation of lines from poses.
00209                         @{
00210                  */
00211                 /**
00212                   * Gets a 3D line corresponding to the X axis in a given pose. An implicit constructor is used if a TPose3D is given.
00213                   * \sa createFromPoseY,createFromPoseZ,createFromPoseAndVector
00214                   */
00215                 void BASE_IMPEXP createFromPoseX(const CPose3D &p,TLine3D &r);
00216                 /**
00217                   * Gets a 3D line corresponding to the Y axis in a given pose. An implicit constructor is used if a TPose3D is given.
00218                   * \sa createFromPoseX,createFromPoseZ,createFromPoseAndVector
00219                   */
00220                 void BASE_IMPEXP createFromPoseY(const CPose3D &p,TLine3D &r);
00221                 /**
00222                   * Gets a 3D line corresponding to the Z axis in a given pose. An implicit constructor is used if a TPose3D is given.
00223                   * \sa createFromPoseX,createFromPoseY,createFromPoseAndVector
00224                   */
00225                 void BASE_IMPEXP createFromPoseZ(const CPose3D &p,TLine3D &r);
00226                 /**
00227                   * 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.
00228                   * \sa createFromPoseX,createFromPoseY,createFromPoseZ
00229                   */
00230                 void BASE_IMPEXP createFromPoseAndVector(const CPose3D &p,const double (&vector)[3],TLine3D &r);
00231                 /**
00232                   * Gets a 2D line corresponding to the X axis in a given pose. An implicit constructor is used if a CPose2D is given.
00233                   * \sa createFromPoseY,createFromPoseAndVector
00234                   */
00235                 void BASE_IMPEXP createFromPoseX(const TPose2D &p,TLine2D &r);
00236                 /**
00237                   * Gets a 2D line corresponding to the Y axis in a given pose. An implicit constructor is used if a CPose2D is given.
00238                   * \sa createFromPoseX,createFromPoseAndVector
00239                   */
00240                 void BASE_IMPEXP createFromPoseY(const TPose2D &p,TLine2D &r);
00241                 /**
00242                   * 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.
00243                   * \sa createFromPoseY,createFromPoseAndVector
00244                   */
00245                 void BASE_IMPEXP createFromPoseAndVector(const TPose2D &p,const double (&vector)[2],TLine2D &r);
00246                 /** @}
00247                  */
00248 
00249                 /** @name Other line or plane related methods.
00250                         @{
00251                  */
00252                 /**
00253                   * Checks whether this polygon or set of points acceptably fits a plane.
00254                   * \sa TPolygon3D,geometryEpsilon
00255                   */
00256                 bool BASE_IMPEXP conformAPlane(const std::vector<TPoint3D> &points);
00257                 /**
00258                   * Checks whether this polygon or set of points acceptably fits a plane, and if it's the case returns it in the second argument.
00259                   * \sa TPolygon3D,geometryEpsilon
00260                   */
00261                 bool BASE_IMPEXP conformAPlane(const std::vector<TPoint3D> &points,TPlane &p);
00262                 /**
00263                   * Checks whether this set of points acceptably fits a 2D line.
00264                   * \sa geometryEpsilon
00265                   */
00266                 bool BASE_IMPEXP areAligned(const std::vector<TPoint2D> &points);
00267                 /**
00268                   * Checks whether this set of points acceptably fits a 2D line, and if it's the case returns it in the second argument.
00269                   * \sa geometryEpsilon
00270                   */
00271                 bool BASE_IMPEXP areAligned(const std::vector<TPoint2D> &points,TLine2D &r);
00272                 /**
00273                   * Checks whether this set of points acceptably fits a 3D line.
00274                   * \sa geometryEpsilon
00275                   */
00276                 bool BASE_IMPEXP areAligned(const std::vector<TPoint3D> &points);
00277                 /**
00278                   * Checks whether this set of points acceptably fits a 3D line, and if it's the case returns it in the second argument.
00279                   */
00280                 bool BASE_IMPEXP areAligned(const std::vector<TPoint3D> &points,TLine3D &r);
00281                 /** @}
00282                  */
00283 
00284                 /** @name Projections
00285                         @{
00286                  */
00287                 /**
00288                   * Uses the given pose 3D to project a point into a new base.
00289                   */
00290                 inline void project3D(const TPoint3D &point, const CPose3D &newXYpose,TPoint3D &newPoint)       {
00291                         newXYpose.composePoint(point.x,point.y,point.z,newPoint.x,newPoint.y,newPoint.z);
00292                 }
00293                 /**
00294                   * Uses the given pose 3D to project a segment into a new base.
00295                   */
00296                 inline void project3D(const TSegment3D &segment,const CPose3D &newXYpose,TSegment3D &newSegment)        {
00297                         project3D(segment.point1,newXYpose,newSegment.point1);
00298                         project3D(segment.point2,newXYpose,newSegment.point2);
00299                 }
00300                 /**
00301                   * Uses the given pose 3D to project a line into a new base.
00302                   */
00303                 void BASE_IMPEXP project3D(const TLine3D &line,const CPose3D &newXYpose,TLine3D &newLine);
00304                 /**
00305                   * Uses the given pose 3D to project a plane into a new base.
00306                   */
00307                 void BASE_IMPEXP project3D(const TPlane &plane,const CPose3D &newXYpose,TPlane &newPlane);
00308                 /**
00309                   * Uses the given pose 3D to project a polygon into a new base.
00310                   */
00311                 void BASE_IMPEXP project3D(const TPolygon3D &polygon,const CPose3D &newXYpose,TPolygon3D &newPolygon);
00312                 /**
00313                   * Uses the given pose 3D to project any 3D object into a new base.
00314                   */
00315                 void BASE_IMPEXP project3D(const TObject3D &object,const CPose3D &newXYPose,TObject3D &newObject);
00316 
00317                 /**
00318                   * 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.
00319                   */
00320                 template<class T> void project3D(const T &obj,const TPlane &newXYPlane,T &newObj)       {
00321                         CPose3D pose;
00322                         TPlane(newXYPlane).getAsPose3D(pose);
00323                         project3D(obj,-pose,newObj);
00324                 }
00325 
00326                 /**
00327                   * 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.
00328                   */
00329                 template<class T> void project3D(const T &obj,const TPlane &newXYPlane,const TPoint3D &newOrigin,T &newObj)     {
00330                         CPose3D pose;
00331                         //TPlane(newXYPlane).getAsPose3DForcingOrigin(newOrigin,pose);
00332                         TPlane(newXYPlane).getAsPose3D(pose);
00333                         project3D(obj,-pose,newObj);
00334                 }
00335 
00336                 /**
00337                   * Projects a set of 3D objects into the plane's base.
00338                   */
00339                 template<class T> void project3D(const std::vector<T> &objs,const CPose3D &newXYpose,std::vector<T> &newObjs)   {
00340                         size_t N=objs.size();
00341                         newObjs.resize(N);
00342                         for (size_t i=0;i<N;i++) project3D(objs[i],newXYpose,newObjs[i]);
00343                 }
00344 
00345                 /**
00346                   * Uses the given pose 2D to project a point into a new base.
00347                   */
00348                 inline void project2D(const TPoint2D &point,const CPose2D &newXpose,TPoint2D &newPoint) {
00349                         newPoint=newXpose+CPoint2D(point);
00350                 }
00351                 /**
00352                   * Uses the given pose 2D to project a segment into a new base.
00353                   */
00354                 inline void project2D(const TSegment2D &segment,const CPose2D &newXpose,TSegment2D &newSegment) {
00355                         project2D(segment.point1,newXpose,newSegment.point1);
00356                         project2D(segment.point2,newXpose,newSegment.point2);
00357                 }
00358 
00359                 /**
00360                   * Uses the given pose 2D to project a line into a new base.
00361                   */
00362                 void BASE_IMPEXP project2D(const TLine2D &line,const CPose2D &newXpose,TLine2D &newLine);
00363                 /**
00364                   * Uses the given pose 2D to project a polygon into a new base.
00365                   */
00366                 void BASE_IMPEXP project2D(const TPolygon2D &polygon,const CPose2D &newXpose,TPolygon2D &newPolygon);
00367                 /**
00368                   * Uses the given pose 2D to project any 2D object into a new base.
00369                   */
00370                 void BASE_IMPEXP project2D(const TObject2D &object,const CPose2D &newXpose,TObject2D &newObject);
00371 
00372                 /**
00373                   * 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.
00374                   */
00375                 template<class T> void project2D(const T &obj,const TLine2D &newXLine,T &newObj)        {
00376                         CPose2D pose;
00377                         newXLine.getAsPose2D(pose);
00378                         project2D(obj,CPose2D(0,0,0)-pose,newObj);
00379                 }
00380 
00381                 /**
00382                   * 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.
00383                   */
00384                 template<class T> void project2D(const T &obj,const TLine2D &newXLine,const TPoint2D &newOrigin,T &newObj)      {
00385                         CPose2D pose;
00386                         newXLine.getAsPose2DForcingOrigin(newOrigin,pose);
00387                         project2D(obj,CPose2D(0,0,0)-pose,newObj);
00388                 }
00389 
00390                 /**
00391                   * Projects a set of 2D objects into the line's base.
00392                   */
00393                 template<class T> void project2D(const std::vector<T> &objs,const CPose2D &newXpose,std::vector<T> &newObjs)    {
00394                         size_t N=objs.size();
00395                         newObjs.resize(N);
00396                         for (size_t i=0;i<N;i++) project2D(objs[i],newXpose,newObjs[i]);
00397                 }
00398                 /** @}
00399                  */
00400 
00401                 /** @name Polygon intersections. These operations rely more on spatial reasoning than in raw numerical operations.
00402                         @{
00403                  */
00404                 /**
00405                   * Gets the intersection between a 2D polygon and a 2D segment.
00406                   * \sa TObject2D
00407                   */
00408                 bool BASE_IMPEXP intersect(const TPolygon2D &p1,const TSegment2D &s2,TObject2D &obj);
00409                 /**
00410                   * Gets the intersection between a 2D polygon and a 2D line.
00411                   * \sa TObject2D
00412                   */
00413                 bool BASE_IMPEXP intersect(const TPolygon2D &p1,const TLine2D &r2,TObject2D &obj);
00414                 /**
00415                   * Gets the intersection between two 2D polygons.
00416                   * \sa TObject2D
00417                   */
00418                 bool BASE_IMPEXP intersect(const TPolygon2D &p1,const TPolygon2D &p2,TObject2D &obj);
00419                 /**
00420                   * Gets the intersection between a 2D segment and a 2D polygon.
00421                   * \sa TObject2D
00422                   */
00423                 inline bool intersect(const TSegment2D &s1,const TPolygon2D &p2,TObject2D &obj) {
00424                         return intersect(p2,s1,obj);
00425                 }
00426                 /**
00427                   * Gets the intersection between a 2D line and a 2D polygon.
00428                   * \sa TObject2D
00429                   */
00430                 inline bool intersect(const TLine2D &r1,const TPolygon2D &p2,TObject2D &obj)    {
00431                         return intersect(p2,r1,obj);
00432                 }
00433                 /**
00434                   * Gets the intersection between a 3D polygon and a 3D segment.
00435                   * \sa TObject3D
00436                   */
00437                 bool BASE_IMPEXP intersect(const TPolygon3D &p1,const TSegment3D &s2,TObject3D &obj);
00438                 /**
00439                   * Gets the intersection between a 3D polygon and a 3D line.
00440                   * \sa TObject3D
00441                   */
00442                 bool BASE_IMPEXP intersect(const TPolygon3D &p1,const TLine3D &r2,TObject3D &obj);
00443                 /**
00444                   * Gets the intersection between a 3D polygon and a plane.
00445                   * \sa TObject3D
00446                   */
00447                 bool BASE_IMPEXP intersect(const TPolygon3D &p1,const TPlane &p2,TObject3D &obj);
00448                 /**
00449                   * Gets the intersection between two 3D polygons.
00450                   * \sa TObject3D
00451                   */
00452                 bool BASE_IMPEXP intersect(const TPolygon3D &p1,const TPolygon3D &p2,TObject3D &obj);
00453                 /**
00454                   * Gets the intersection between a 3D segment and a 3D polygon.
00455                   * \sa TObject3D
00456                   */
00457                 inline bool intersect(const TSegment3D &s1,const TPolygon3D &p2,TObject3D &obj) {
00458                         return intersect(p2,s1,obj);
00459                 }
00460                 /**
00461                   * Gets the intersection between a 3D line and a 3D polygon.
00462                   * \sa TObject3D
00463                   */
00464                 inline bool intersect(const TLine3D &r1,const TPolygon3D &p2,TObject3D &obj)    {
00465                         return intersect(p2,r1,obj);
00466                 }
00467                 /**
00468                   * Gets the intersection between a plane and a 3D polygon.
00469                   * \sa TObject3D
00470                   */
00471                 inline bool intersect(const TPlane &p1,const TPolygon3D &p2,TObject3D &obj)     {
00472                         return intersect(p2,p1,obj);
00473                 }
00474 
00475                 /**
00476                   * 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.
00477                   * \sa TObject3D,CSparseMatrixTemplate
00478                   */
00479                 size_t BASE_IMPEXP intersect(const std::vector<TPolygon3D> &v1,const std::vector<TPolygon3D> &v2,CSparseMatrixTemplate<TObject3D> &objs);
00480                 /**
00481                   * 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.
00482                   * \sa TObject3D
00483                   */
00484                 size_t BASE_IMPEXP intersect(const std::vector<TPolygon3D> &v1,const std::vector<TPolygon3D> &v2,std::vector<TObject3D> &objs);
00485                 /** @}
00486                  */
00487 
00488                 /** @name Other intersections
00489                         @{
00490                  */
00491                 /**
00492                   * Gets the intersection between vectors of geometric objects and returns it in a sparse matrix of either TObject2D or TObject3D.
00493                   * \sa TObject2D,TObject3D,CSparseMatrix
00494                   */
00495                 template<class T,class U,class O> size_t intersect(const std::vector<T> &v1,const std::vector<U> &v2,CSparseMatrixTemplate<O> &objs)    {
00496                         size_t M=v1.size(),N=v2.size();
00497                         O obj;
00498                         objs.clear();
00499                         objs.resize(M,N);
00500                         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;
00501                         return objs.getNonNullElements();
00502                 }
00503 
00504                 /**
00505                   * Gets the intersection between vectors of geometric objects and returns it in a vector of either TObject2D or TObject3D.
00506                   * \sa TObject2D,TObject3D
00507                   */
00508                 template<class T,class U,class O> size_t intersect(const std::vector<T> &v1,const std::vector<U> &v2,std::vector<O> objs)       {
00509                         objs.resize(0);
00510                         O obj;
00511                         for (typename std::vector<T>::const_iterator it1=v1.begin();it1!=v1.end();it1++)        {
00512                                 const T &elem1=*it1;
00513                                 for (typename std::vector<U>::const_iterator it2=v2.begin();it2!=v2.end();it2++) if (intersect(elem1,*it2,obj)) objs.push_back(obj);
00514                         }
00515                         return objs.size();
00516                 }
00517 
00518                 /**
00519                   * Gets the intersection between any pair of 2D objects.
00520                   */
00521                 bool BASE_IMPEXP intersect(const TObject2D &o1,const TObject2D &o2,TObject2D &obj);
00522                 /**
00523                   * Gets the intersection between any pair of 3D objects.
00524                   */
00525                 bool BASE_IMPEXP intersect(const TObject3D &o1,const TObject3D &o2,TObject3D &obj);
00526                 /** @}
00527                  */
00528 
00529                 /** @name Distances
00530                         @{
00531                  */
00532                 /**
00533                   * Gets the distance between two points in a 2D space.
00534                   */
00535                 double BASE_IMPEXP distance(const TPoint2D &p1,const TPoint2D &p2);
00536                 /**
00537                   * Gets the distance between two points in a 3D space.
00538                   */
00539                 double BASE_IMPEXP distance(const TPoint3D &p1,const TPoint3D &p2);
00540                 /**
00541                   * Gets the distance between two lines in a 2D space.
00542                   */
00543                 double BASE_IMPEXP distance(const TLine2D &r1,const TLine2D &r2);
00544                 /**
00545                   * Gets the distance between two lines in a 3D space.
00546                   */
00547                 double BASE_IMPEXP distance(const TLine3D &r1,const TLine3D &r2);
00548                 /**
00549                   * Gets the distance between two planes. It will be zero if the planes are not parallel.
00550                   */
00551                 double BASE_IMPEXP distance(const TPlane &p1,const TPlane &p2);
00552 
00553                 /**
00554                   * Gets the distance between two polygons in a 2D space.
00555                   */
00556                 double BASE_IMPEXP distance(const TPolygon2D &p1,const TPolygon2D &p2);
00557                 /**
00558                   * Gets the distance between a polygon and a segment in a 2D space.
00559                   */
00560                 double BASE_IMPEXP distance(const TPolygon2D &p1,const TSegment2D &s2);
00561                 /**
00562                   * Gets the distance between a segment and a polygon in a 2D space.
00563                   */
00564                 inline double distance(const TSegment2D &s1,const TPolygon2D &p2)       {
00565                         return distance(p2,s1);
00566                 }
00567                 /**
00568                   * Gets the distance between a polygon and a line in a 2D space.
00569                   */
00570                 double BASE_IMPEXP distance(const TPolygon2D &p1,const TLine2D &l2);
00571                 inline double distance(const TLine2D &l1,const TPolygon2D &p2)  {
00572                         return distance(p2,l1);
00573                 }
00574                 /**
00575                   * Gets the distance between two polygons in a 3D space.
00576                   */
00577                 double BASE_IMPEXP distance(const TPolygon3D &p1,const TPolygon3D &p2);
00578                 /**
00579                   * Gets the distance between a polygon and a segment in a 3D space.
00580                   */
00581                 double BASE_IMPEXP distance(const TPolygon3D &p1,const TSegment3D &s2);
00582                 /**
00583                   * Gets the distance between a segment and a polygon in a 3D space.
00584                   */
00585                 inline double distance(const TSegment3D &s1,const TPolygon3D &p2)       {
00586                         return distance(p2,s1);
00587                 }
00588                 /**
00589                   * Gets the distance between a polygon and a line in a 3D space.
00590                   */
00591                 double BASE_IMPEXP distance(const TPolygon3D &p1,const TLine3D &l2);
00592                 /**
00593                   * Gets the distance between a line and a polygon in a 3D space.
00594                   */
00595                 inline double distance(const TLine3D &l1,const TPolygon3D &p2)  {
00596                         return distance(p2,l1);
00597                 }
00598                 /**
00599                   * Gets the distance between a polygon and a plane.
00600                   */
00601                 double BASE_IMPEXP distance(const TPolygon3D &po,const TPlane &pl);
00602                 /**
00603                   * Gets the distance between a plane and a polygon.
00604                   */
00605                 inline double distance(const TPlane &pl,const TPolygon3D &po)   {
00606                         return distance(po,pl);
00607                 }
00608                 /** @}
00609                  */
00610 
00611                 /** @name Bound checkers
00612                         @{
00613                  */
00614                 /**
00615                   * Gets the rectangular bounds of a 2D polygon or set of 2D points.
00616                   */
00617                 void BASE_IMPEXP getRectangleBounds(const std::vector<TPoint2D> &poly,TPoint2D &pMin,TPoint2D &pMax);
00618                 /**
00619                   * Gets the prism bounds of a 3D polygon or set of 3D points.
00620                   */
00621                 void BASE_IMPEXP getPrismBounds(const std::vector<TPoint3D> &poly,TPoint3D &pMin,TPoint3D &pMax);
00622                 /** @}
00623                  */
00624 
00625                 /** @name Creation of planes from poses
00626                         @{
00627                  */
00628                 /**
00629                   * Given a pose, creates a plane orthogonal to its Z vector.
00630                   * \sa createPlaneFromPoseXZ,createPlaneFromPoseYZ,createPlaneFromPoseAndNormal
00631                   */
00632                 void BASE_IMPEXP createPlaneFromPoseXY(const CPose3D &pose,TPlane &plane);
00633                 /**
00634                   * Given a pose, creates a plane orthogonal to its Y vector.
00635                   * \sa createPlaneFromPoseXY,createPlaneFromPoseYZ,createPlaneFromPoseAndNormal
00636                   */
00637                 void BASE_IMPEXP createPlaneFromPoseXZ(const CPose3D &pose,TPlane &plane);
00638                 /**
00639                   * Given a pose, creates a plane orthogonal to its X vector.
00640                   * \sa createPlaneFromPoseXY,createPlaneFromPoseXZ,createPlaneFromPoseAndNormal
00641                   */
00642                 void BASE_IMPEXP createPlaneFromPoseYZ(const CPose3D &pose,TPlane &plane);
00643                 /**
00644                   * Given a pose and any vector, creates a plane orthogonal to that vector in the pose's coordinates.
00645                   * \sa createPlaneFromPoseXY,createPlaneFromPoseXZ,createPlaneFromPoseYZ
00646                   */
00647                 void BASE_IMPEXP createPlaneFromPoseAndNormal(const CPose3D &pose,const double (&normal)[3],TPlane &plane);
00648                 /**
00649                   * Creates a rotation matrix so that the coordinate given (0 for x, 1 for y, 2 for z) corresponds to the vector.
00650                   */
00651                 void BASE_IMPEXP generateAxisBaseFromDirectionAndAxis(const double (&vec)[3],char coord,CMatrixDouble &matrix);
00652                 /** @}
00653                  */
00654 
00655                 /** @name Linear regression methods
00656                         @{
00657                  */
00658                 /**
00659                   * Using eigenvalues, gets the best fitting line for a set of 2D points. Returns an estimation of the error.
00660                   * \sa spline, leastSquareLinearFit
00661                   */
00662                 double BASE_IMPEXP getRegressionLine(const std::vector<TPoint2D> &points,TLine2D &line);
00663                 /**
00664                   * Using eigenvalues, gets the best fitting line for a set of 3D points. Returns an estimation of the error.
00665                   * \sa spline, leastSquareLinearFit
00666                   */
00667                 double BASE_IMPEXP getRegressionLine(const std::vector<TPoint3D> &points,TLine3D &line);
00668                 /**
00669                   * Using eigenvalues, gets the best fitting plane for a set of 3D points. Returns an estimation of the error.
00670                   * \sa spline, leastSquareLinearFit
00671                   */
00672                 double BASE_IMPEXP getRegressionPlane(const std::vector<TPoint3D> &points,TPlane &plane);
00673                 /** @}
00674                  */
00675 
00676                 /** @name Miscellaneous Geometry methods
00677                         @{
00678                  */
00679                 /**
00680                   * Tries to assemble a set of segments into a set of closed polygons.
00681                   */
00682                 void BASE_IMPEXP assemblePolygons(const std::vector<TSegment3D> &segms,std::vector<TPolygon3D> &polys);
00683                 /**
00684                   * Tries to assemble a set of segments into a set of closed polygons, returning the unused segments as another out parameter.
00685                   */
00686                 void BASE_IMPEXP assemblePolygons(const std::vector<TSegment3D> &segms,std::vector<TPolygon3D> &polys,std::vector<TSegment3D> &remainder);
00687                 /**
00688                   * Extracts all the polygons, including those formed from segments, from the set of objects.
00689                   */
00690                 void BASE_IMPEXP assemblePolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys);
00691                 /**
00692                   * Extracts all the polygons, including those formed from segments, from the set of objects.
00693                   */
00694                 void BASE_IMPEXP assemblePolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder);
00695                 /**
00696                   * Extracts all the polygons, including those formed from segments, from the set of objects.
00697                   */
00698                 void BASE_IMPEXP assemblePolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TSegment3D> &remainder1,std::vector<TObject3D> &remainder2);
00699 
00700                 /**
00701                   * Changes the value of the geometric epsilon.
00702                   * \sa geometryEpsilon,getEpsilon
00703                   */
00704                 inline void setEpsilon(double nE)       {
00705                         geometryEpsilon=nE;
00706                 }
00707                 /**
00708                   * Gets the value of the geometric epsilon.
00709                   * \sa geometryEpsilon,setEpsilon
00710                   */
00711                 inline double getEpsilon()      {
00712                         return geometryEpsilon;
00713                 }
00714 
00715                 /**
00716                   * Splits a 2D polygon into convex components.
00717                   */
00718                 bool BASE_IMPEXP splitInConvexComponents(const TPolygon2D &poly,vector<TPolygon2D> &components);
00719                 /**
00720                   * Splits a 3D polygon into convex components.
00721                   * \throw std::logic_error if the polygon can't be fit into a plane.
00722                   */
00723                 bool BASE_IMPEXP splitInConvexComponents(const TPolygon3D &poly,vector<TPolygon3D> &components);
00724 
00725                 /**
00726                   * Gets the bisector of a 2D segment.
00727                   */
00728                 void BASE_IMPEXP getSegmentBisector(const TSegment2D &sgm,TLine2D &bis);
00729                 /**
00730                   * Gets the bisector of a 3D segment.
00731                   */
00732                 void BASE_IMPEXP getSegmentBisector(const TSegment3D &sgm,TPlane &bis);
00733                 /**
00734                   * Gets the bisector of two lines or segments (implicit constructor will be used if necessary)
00735                   */
00736                 void BASE_IMPEXP getAngleBisector(const TLine2D &l1,const TLine2D &l2,TLine2D &bis);
00737                 /**
00738                   * Gets the bisector of two lines or segments (implicit constructor will be used if necessary)
00739                   * \throw std::logic_error if the lines do not fit in a single plane.
00740                   */
00741                 void BASE_IMPEXP getAngleBisector(const TLine3D &l1,const TLine3D &l2,TLine3D &bis);
00742 
00743                 /**
00744                   * Fast ray tracing method using polygons' properties.
00745                   * \sa CRenderizable::rayTrace
00746                   */
00747                 bool BASE_IMPEXP traceRay(const vector<TPolygonWithPlane> &vec,const mrpt::poses::CPose3D &pose,double &dist);
00748                 /**
00749                   * Fast ray tracing method using polygons' properties.
00750                   * \sa CRenderizable::rayTrace
00751                   */
00752                 inline bool traceRay(const vector<TPolygon3D> &vec,const mrpt::poses::CPose3D &pose,double &dist)       {
00753                         vector<TPolygonWithPlane> pwp;
00754                         TPolygonWithPlane::getPlanes(vec,pwp);
00755                         return traceRay(pwp,pose,dist);
00756                 }
00757 
00758                 /** Computes the cross product of two 3D vectors, returning a vector normal to both.
00759                   *  It uses the simple implementation:
00760 
00761                     \f[  v_out = \left(
00762                                         \begin{array}{c c c}
00763                                         \hat{i} ~ \hat{j} ~ \hat{k} \\
00764                                         x0 ~ y0 ~ z0 \\
00765                                         x1 ~ y1 ~ z1 \\
00766                                         \end{array} \right)
00767                         \f]
00768                   */
00769                 template<class T,class U,class V>
00770                 inline void crossProduct3D(const T &v0,const U &v1,V& vOut)     {
00771                         vOut[0]=v0[1]*v1[2]-v0[2]*v1[1];
00772                         vOut[1]=v0[2]*v1[0]-v0[0]*v1[2];
00773                         vOut[2]=v0[0]*v1[1]-v0[1]*v1[0];
00774                 }
00775 
00776                 //! \overload
00777                 template<class T>
00778                 inline void crossProduct3D(
00779                         const std::vector<T> &v0,
00780                         const std::vector<T> &v1,
00781                         std::vector<T> &v_out )
00782                 {
00783                         ASSERT_(v0.size()==3)
00784                         ASSERT_(v1.size()==3);
00785                         v_out.resize(3);
00786                         v_out[0] =  v0[1]*v1[2] - v0[2]*v1[1];
00787                         v_out[1] = -v0[0]*v1[2] + v0[2]*v1[0];
00788                         v_out[2] =  v0[0]*v1[1] - v0[1]*v1[0];
00789                 }
00790 
00791                 //! \overload (returning a vector of size 3 by value).
00792                 template<class VEC1,class VEC2>
00793                 inline Eigen::Matrix<double,3,1> crossProduct3D(const VEC1 &v0,const VEC2 &v1)  {
00794                         Eigen::Matrix<double,3,1> vOut;
00795                         vOut[0]=v0[1]*v1[2]-v0[2]*v1[1];
00796                         vOut[1]=v0[2]*v1[0]-v0[0]*v1[2];
00797                         vOut[2]=v0[0]*v1[1]-v0[1]*v1[0];
00798                         return vOut;
00799                 }
00800 
00801                 /** Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array:
00802                   * \f[  M([x ~ y ~ z]^\top) = \left(
00803                   *     \begin{array}{c c c}
00804                   *     0 & -z & y \\
00805                   *     z & 0 & -x \\
00806                   *     -y & x & 0
00807                   *     \end{array} \right)
00808                   * \f]
00809                   */
00810                 template<class VECTOR,class MATRIX>
00811                 inline void skew_symmetric3(const VECTOR &v,MATRIX& M)  {
00812                         ASSERT_(v.size()==3)
00813                         M.setSize(3,3);
00814                         M.set_unsafe(0,0, 0); M.set_unsafe(0,1, -v[2]); M.set_unsafe(0,2, v[1]);
00815                         M.set_unsafe(1,0, v[2]); M.set_unsafe(1,1, 0); M.set_unsafe(1,2, -v[0]);
00816                         M.set_unsafe(2,0, -v[1]); M.set_unsafe(2,1, v[0]); M.set_unsafe(2,2, 0);
00817                 }
00818                 //! \overload
00819                 template<class VECTOR>
00820                 inline mrpt::math::CMatrixDouble33 skew_symmetric3(const VECTOR &v)     {
00821                         mrpt::math::CMatrixDouble33 M(UNINITIALIZED_MATRIX);
00822                         skew_symmetric3(v,M);
00823                         return M;
00824                 }
00825 
00826                 /** Computes the negative version of a 3x3 skew symmetric matrix from a 3-vector or 3-array:
00827                   * \f[  -M([x ~ y ~ z]^\top) = \left(
00828                   *     \begin{array}{c c c}
00829                   *     0 & z & -y \\
00830                   *     -z & 0 & x \\
00831                   *     y & -x & 0
00832                   *     \end{array} \right)
00833                   * \f]
00834                   */
00835                 template<class VECTOR,class MATRIX>
00836                 inline void skew_symmetric3_neg(const VECTOR &v,MATRIX& M)      {
00837                         ASSERT_(v.size()==3)
00838                         M.setSize(3,3);
00839                         M.set_unsafe(0,0, 0); M.set_unsafe(0,1, v[2]); M.set_unsafe(0,2, -v[1]);
00840                         M.set_unsafe(1,0, -v[2]); M.set_unsafe(1,1, 0); M.set_unsafe(1,2, v[0]);
00841                         M.set_unsafe(2,0, v[1]); M.set_unsafe(2,1, -v[0]); M.set_unsafe(2,2, 0);
00842                 }
00843                 //! \overload
00844                 template<class VECTOR>
00845                 inline mrpt::math::CMatrixDouble33 skew_symmetric3_neg(const VECTOR &v) {
00846                         mrpt::math::CMatrixDouble33 M(UNINITIALIZED_MATRIX);
00847                         skew_symmetric3_neg(v,M);
00848                         return M;
00849                 }
00850 
00851 
00852                 /**
00853                   * Returns true if two 2D vectors are parallel. The arguments may be points, arrays, etc.
00854                   */
00855                 template<class T,class U> inline bool vectorsAreParallel2D(const T &v1,const U &v2)     {
00856                         return abs(v1[0]*v2[1]-v2[0]*v1[1])<geometryEpsilon;
00857                 }
00858 
00859                 /**
00860                   * Returns true if two 3D vectors are parallel. The arguments may be points, arrays, etc.
00861                   */
00862                 template<class T,class U>
00863                 inline bool vectorsAreParallel3D(const T &v1,const U &v2)       {
00864                         if (abs(v1[0]*v2[1]-v2[0]*v1[1])>=geometryEpsilon) return false;
00865                         if (abs(v1[1]*v2[2]-v2[1]*v1[2])>=geometryEpsilon) return false;
00866                         return abs(v1[2]*v2[0]-v2[2]*v1[0])<geometryEpsilon;
00867                 }
00868 
00869                 /** Computes the closest point from a given point to a segment, and returns that minimum distance.
00870                   */
00871                 double BASE_IMPEXP minimumDistanceFromPointToSegment(
00872                                 const double &  Px,
00873                                 const double &  Py,
00874                                 const double &  x1,
00875                                 const double &  y1,
00876                                 const double &  x2,
00877                                 const double &  y2,
00878                                 double &        out_x,
00879                                 double &        out_y);
00880 
00881                 /** Computes the closest point from a given point to a segment, and returns that minimum distance.
00882                   */
00883                 double BASE_IMPEXP minimumDistanceFromPointToSegment(
00884                                 const double &  Px,
00885                                 const double &  Py,
00886                                 const double &  x1,
00887                                 const double &  y1,
00888                                 const double &  x2,
00889                                 const double &  y2,
00890                                 float & out_x,
00891                                 float & out_y);
00892 
00893 
00894                 /** Computes the closest point from a given point to a segment.
00895                   * \sa closestFromPointToLine
00896                   */
00897                 void BASE_IMPEXP closestFromPointToSegment(
00898                                 const double &  Px,
00899                                 const double &  Py,
00900                                 const double &  x1,
00901                                 const double &  y1,
00902                                 const double &  x2,
00903                                 const double &  y2,
00904                                 double &out_x,
00905                                 double &out_y);
00906 
00907                 /** Computes the closest point from a given point to a (infinite) line.
00908                   * \sa closestFromPointToSegment
00909                   */
00910                 void BASE_IMPEXP closestFromPointToLine(
00911                                 const double &  Px,
00912                                 const double &  Py,
00913                                 const double &  x1,
00914                                 const double &  y1,
00915                                 const double &  x2,
00916                                 const double &  y2,
00917                                 double &out_x,
00918                                 double &out_y);
00919 
00920                 /** Returns the square distance from a point to a line.
00921                   */
00922                 double BASE_IMPEXP closestSquareDistanceFromPointToLine(
00923                                 const double &  Px,
00924                                 const double &  Py,
00925                                 const double &  x1,
00926                                 const double &  y1,
00927                                 const double &  x2,
00928                                 const double &  y2 );
00929 
00930 
00931                 /** Returns the distance between 2 points in 2D. */
00932                 template <typename T>
00933                 T distanceBetweenPoints(const T x1,const T y1,const T x2,const T y2) {
00934                         return std::sqrt( square(x1-x2)+square(y1-y2) );
00935                 }
00936 
00937                 /** Returns the distance between 2 points in 3D. */
00938                 template <typename T>
00939                 T distanceBetweenPoints(const T x1,const T y1,const T z1, const T x2,const T y2, const T z2) {
00940                         return std::sqrt( square(x1-x2)+square(y1-y2)+square(z1-z2) );
00941                 }
00942 
00943                 /** Returns the square distance between 2 points in 2D. */
00944                 template <typename T>
00945                 T distanceSqrBetweenPoints(const T x1,const T y1,const T x2,const T y2) {
00946                         return square(x1-x2)+square(y1-y2);
00947                 }
00948 
00949                 /** Returns the square distance between 2 points in 3D. */
00950                 template <typename T>
00951                 T distanceSqrBetweenPoints(const T x1,const T y1,const T z1, const T x2,const T y2, const T z2) {
00952                         return square(x1-x2)+square(y1-y2)+square(z1-z2);
00953                 }
00954 
00955                 /** Returns the intersection point, and if it exists, between two segments.
00956                   */
00957                 bool  BASE_IMPEXP SegmentsIntersection(
00958                                         const double &  x1,const double &       y1,
00959                                         const double &  x2,const double &       y2,
00960                                         const double &  x3,const double &       y3,
00961                                         const double &  x4,const double &       y4,
00962                                         double &ix,double &iy);
00963 
00964                 /** Returns the intersection point, and if it exists, between two segments.
00965                   */
00966                 bool  BASE_IMPEXP SegmentsIntersection(
00967                                         const double &  x1,const double &       y1,
00968                                         const double &  x2,const double &       y2,
00969                                         const double &  x3,const double &       y3,
00970                                         const double &  x4,const double &       y4,
00971                                         float &ix,float &iy);
00972 
00973                 /** Returns true if the 2D point (px,py) falls INTO the given polygon.
00974                   * \sa pointIntoQuadrangle
00975                   */
00976                 bool  BASE_IMPEXP pointIntoPolygon2D(const double & px, const double & py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys );
00977 
00978                 /** Specialized method to check whether a point (x,y) falls into a quadrangle.
00979                   * \sa pointIntoPolygon2D
00980                   */
00981                 template <typename T>
00982                 bool pointIntoQuadrangle(
00983                         T x, T y,
00984                         T v1x, T v1y,
00985                         T v2x, T v2y,
00986                         T v3x, T v3y,
00987                         T v4x, T v4y )
00988                 {
00989                         using mrpt::utils::sign;
00990 
00991                         const T a1 = atan2( v1y - y , v1x - x );
00992                         const T a2 = atan2( v2y - y , v2x - x );
00993                         const T a3 = atan2( v3y - y , v3x - x );
00994                         const T a4 = atan2( v4y - y , v4x - x );
00995 
00996                         // The point is INSIDE iff all the signs of the angles between each vertex
00997                         //  and the next one are equal.
00998                         const T da1 = mrpt::math::wrapToPi( a2-a1 );
00999                         const T da2 = mrpt::math::wrapToPi( a3-a2 );
01000                         if (sign(da1)!=sign(da2)) return false;
01001 
01002                         const T da3 = mrpt::math::wrapToPi( a4-a3 );
01003                         if (sign(da2)!=sign(da3)) return false;
01004 
01005                         const T da4 = mrpt::math::wrapToPi( a1-a4 );
01006                         return (sign(da3)==sign(da4) && (sign(da4)==sign(da1)));
01007                 }
01008 
01009                 /** Returns the closest distance of a given 2D point to a polygon, or "0" if the point is INTO the polygon or its perimeter.
01010                   */
01011                 double BASE_IMPEXP distancePointToPolygon2D(const double & px, const double & py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys );
01012 
01013                 /** Calculates the minimum distance between a pair of lines.
01014                   The lines are given by:
01015                         - Line 1 = P1 + f (P2-P1)
01016                         - Line 2 = P3 + f (P4-P3)
01017                   The Euclidean distance is returned in "dist", and the mid point between the lines in (x,y,z)
01018                   \return It returns false if there is no solution, i.e. lines are (almost, up to EPS) parallel.
01019                  */
01020                 bool  BASE_IMPEXP minDistBetweenLines(
01021                                                         const double &  p1_x, const double &    p1_y, const double & p1_z,
01022                                                         const double &  p2_x, const double &    p2_y, const double & p2_z,
01023                                                         const double &  p3_x, const double & p3_y, const double & p3_z,
01024                                                         const double &  p4_x, const double & p4_y, const double & p4_z,
01025                                                         double &x,   double &y,   double &z,
01026                                                         double &dist);
01027 
01028                 /** Returns wether two rotated rectangles intersect.
01029                  *  The first rectangle is not rotated and given by (R1_x_min,R1_x_max)-(R1_y_min,R1_y_max).
01030                  *  The second rectangle is given is a similar way, but it is internally rotated according
01031                  *   to the given coordinates translation (R2_pose_x,R2_pose_y,R2_pose_phi(radians)), relative
01032                  *   to the coordinates system of rectangle 1.
01033                  */
01034                 bool  BASE_IMPEXP RectanglesIntersection(
01035                                         const double &  R1_x_min,       const double &  R1_x_max,
01036                                         const double &  R1_y_min,       const double &  R1_y_max,
01037                                         const double &  R2_x_min,       const double &  R2_x_max,
01038                                         const double &  R2_y_min,       const double &  R2_y_max,
01039                                         const double &  R2_pose_x,
01040                                         const double &  R2_pose_y,
01041                                         const double &  R2_pose_phi );
01042 
01043                 /** Computes an axis base (a set of three 3D normal vectors) with the given vector being the first of them.
01044                   * NOTE: Make sure of passing all floats or doubles and that the template of the receiving matrix is of the same type!
01045                   *
01046                   *  If   \f$ d = [ dx ~ dy ~ dz ] \f$ is the input vector, then this function returns a matrix \f$ M \f$ such as:
01047                   *
01048                     \f[  M = \left(
01049                                         \begin{array}{c c c}
01050                                         v^1_x ~ v^2_x ~ v^3_x \\
01051                                         v^1_y ~ v^2_y ~ v^3_y \\
01052                                         v^1_z ~ v^2_z ~ v^3_z
01053                                         \end{array} \right)
01054                         \f]
01055                   *
01056                   *   And the three normal vectors are computed as:
01057 
01058                           \f[ v^1 = \frac{d}{|d|}  \f]
01059 
01060                           If (dx!=0 or dy!=0):
01061                                 \f[ v^2 = \frac{[-dy ~ dx ~ 0 ]}{\sqrt{dx^2+dy^2}}  \f]
01062                           otherwise (the direction vector is vertical):
01063                                 \f[ v^2 = [1 ~ 0 ~ 0]  \f]
01064 
01065                           And finally, the third vector is the cross product of the others:
01066 
01067                             \f[ v^3 = v^1 \times v^2  \f]
01068                   *
01069                   * \return The 3x3 matrix (CMatrixTemplateNumeric<T>), containing one vector per column.
01070                   * \except Throws an std::exception on invalid input (i.e. null direction vector)
01071                   *
01072                   * (JLB @ 18-SEP-2007)
01073                   */
01074                 template<class T>
01075                 CMatrixTemplateNumeric<T> generateAxisBaseFromDirection( T dx, T dy, T dz )
01076                 {
01077                         MRPT_START;
01078 
01079                         if (dx==0 && dy==0 && dz==0)
01080                                 THROW_EXCEPTION("Invalid input: Direction vector is (0,0,0)!");
01081 
01082                         CMatrixTemplateNumeric<T>       P(3,3);
01083 
01084                         // 1st vector:
01085                         T       n_xy = square(dx)+square(dy);
01086                         T       n = sqrt(n_xy+square(dz));
01087                         n_xy = sqrt(n_xy);
01088                         P(0,0) = dx / n;
01089                         P(1,0) = dy / n;
01090                         P(2,0) = dz / n;
01091 
01092                         // 2nd perpendicular vector:
01093                         if (fabs(dx)>1e-4 || fabs(dy)>1e-4)
01094                         {
01095                                 P(0,1) = -dy / n_xy;
01096                                 P(1,1) = dx / n_xy;
01097                                 P(2,1) = 0;
01098                         }
01099                         else
01100                         {
01101                                 // Any vector in the XY plane will work:
01102                                 P(0,1) = 1;
01103                                 P(1,1) = 0;
01104                                 P(2,1) = 0;
01105                         }
01106 
01107                         // 3rd perpendicular vector: cross product of the two last vectors:
01108                         P.col(2) = crossProduct3D(P.col(0),P.col(1));
01109 
01110                         return P;
01111                         MRPT_END;
01112                 }
01113 
01114 
01115                 /** Compute a rotation exponential using the Rodrigues Formula.
01116                   * The rotation axis is given by \f$\vec{w}\f$, and the rotation angle must
01117                   * be computed using \f$ \theta = |\vec{w}|\f$. This is provided as a separate
01118                   * function primarily to allow fast and rough matrix exponentials using fast
01119                   * and rough approximations to \e A and \e B.
01120                   *
01121                   * \param w Vector about which to rotate.
01122                   * \param A \f$\frac{\sin \theta}{\theta}\f$
01123                   * \param B \f$\frac{1 - \cos \theta}{\theta^2}\f$
01124                   * \param R Matrix to hold the return value.
01125                   * \sa CPose3D
01126                   * \note Method from TooN (C) Tom Drummond (GNU GPL)
01127                   */
01128                 template <typename VECTOR_LIKE, typename Precision, typename MATRIX_LIKE>
01129                 inline void rodrigues_so3_exp(const VECTOR_LIKE& w, const Precision A,const Precision B,MATRIX_LIKE & R)
01130                 {
01131                         ASSERT_EQUAL_(w.size(),3)
01132                         ASSERT_EQUAL_(R.getColCount(),3)
01133                         ASSERT_EQUAL_(R.getRowCount(),3)
01134                         {
01135                                 const Precision wx2 = (Precision)w[0]*w[0];
01136                                 const Precision wy2 = (Precision)w[1]*w[1];
01137                                 const Precision wz2 = (Precision)w[2]*w[2];
01138                                 R(0,0) = 1.0 - B*(wy2 + wz2);
01139                                 R(1,1) = 1.0 - B*(wx2 + wz2);
01140                                 R(2,2) = 1.0 - B*(wx2 + wy2);
01141                         }
01142                         {
01143                                 const Precision a = A*w[2];
01144                                 const Precision b = B*(w[0]*w[1]);
01145                                 R(0,1) = b - a;
01146                                 R(1,0) = b + a;
01147                         }
01148                         {
01149                                 const Precision a = A*w[1];
01150                                 const Precision b = B*(w[0]*w[2]);
01151                                 R(0,2) = b + a;
01152                                 R(2,0) = b - a;
01153                         }
01154                         {
01155                                 const Precision a = A*w[0];
01156                                 const Precision b = B*(w[1]*w[2]);
01157                                 R(1,2) = b - a;
01158                                 R(2,1) = b + a;
01159                         }
01160                 }
01161 
01162                 /** @} */  // end of misc. geom. methods
01163 
01164         } // End of namespace
01165 
01166 } // End of namespace
01167 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011