Main MRPT website > C++ reference
MRPT logo

lightweight_geom_data.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 LIGHTWEIGHT_GEOM_DATA_H
00029 #define LIGHTWEIGHT_GEOM_DATA_H
00030 
00031 #include <mrpt/utils/utils_defs.h>
00032 #include <mrpt/utils/stl_extensions.h>
00033 
00034 #include <mrpt/math/math_frwds.h>  // Fordward declarations
00035 
00036 
00037 
00038 namespace mrpt  {
00039         namespace poses {
00040                 template <class DERIVEDCLASS> class CPoseOrPoint;
00041                 class CPoint2D;
00042                 class CPoint3D;
00043                 class CPose2D;
00044                 class CPose3D;
00045         }
00046         namespace utils { class CStream; }
00047 }
00048 
00049 namespace mrpt  {
00050 namespace math  {
00051         using namespace mrpt::utils; // For "square"
00052 
00053         struct TPoint2D;
00054         struct TPose2D;
00055         struct TPoint3D;
00056         struct TPose3D;
00057         struct TPose3DQuat;
00058 
00059         //Pragma defined to ensure no structure packing
00060 #pragma pack(push,1)
00061         //Set of typedefs for lightweight geometric items.
00062         /**
00063           * Lightweight 2D point. Allows coordinate access using [] operator.
00064           * \sa mrpt::poses::CPoint2D
00065           */
00066 //#define TOBJECTS_USE_UNIONS
00067         struct BASE_IMPEXP TPoint2D     {
00068                 /**
00069                   * X coordinate.
00070                   */
00071                 double x;
00072                 /**
00073                   * Y coordinate.
00074                   */
00075                 double y;
00076                 /**
00077                   * Constructor from TPose2D, discarding phi.
00078                   * \sa TPose2D
00079                   */
00080                 explicit TPoint2D(const TPose2D &p);
00081                 /**
00082                   * Constructor from TPoint3D, discarding z.
00083                   * \sa TPoint3D
00084                   */
00085                 explicit TPoint2D(const TPoint3D &p);
00086                 /**
00087                   * Constructor from TPose3D, discarding z and the angular coordinates.
00088                   * \sa TPose3D
00089                   */
00090                 explicit TPoint2D(const TPose3D &p);
00091                 /**
00092                   * Constructor from CPoseOrPoint, perhaps losing 3D information
00093                   * \sa CPoseOrPoint,CPoint3D,CPose2D,CPose3D
00094                   */
00095                 template <class DERIVEDCLASS>
00096                 explicit TPoint2D(const mrpt::poses::CPoseOrPoint<DERIVEDCLASS> &p) :x(p.x()),y(p.y())  {}
00097 
00098                 /** Implicit transformation constructor from TPixelCoordf */
00099                 inline TPoint2D(const mrpt::utils::TPixelCoordf &p) :x(p.x),y(p.y)      {}
00100 
00101                 /** Implicit constructor from CPoint2D  */
00102                 TPoint2D(const mrpt::poses::CPoint2D &p);
00103                 /**
00104                   * Constructor from coordinates.
00105                   */
00106                 inline TPoint2D(double xx,double yy):x(xx),y(yy)        {}
00107                 /**
00108                   * Default fast constructor. Initializes to garbage.
00109                   */
00110                 inline TPoint2D()       {}
00111                 /**
00112                   * Unsafe coordinate access using operator[]. Intended for loops.
00113                   */
00114                 inline double &operator[](size_t i)     {
00115                         return (&x)[i];
00116                 }
00117                 /**
00118                   * Unsafe coordinate access using operator[]. Intended for loops.
00119                   */
00120                 inline const double &operator[](size_t i) const {
00121                         return (&x)[i];
00122                 }
00123                 /**
00124                   * Transformation into vector.
00125                   */
00126                 inline void getAsVector(vector_double &v) const {
00127                         v.resize(2);
00128                         v[0]=x; v[1]=y;
00129                 }
00130 
00131                 bool operator<(const TPoint2D &p) const;
00132 
00133                 inline TPoint2D &operator+=(const TPoint2D &p)  {
00134                         x+=p.x;
00135                         y+=p.y;
00136                         return *this;
00137                 }
00138 
00139                 inline TPoint2D &operator-=(const TPoint2D &p)  {
00140                         x-=p.x;
00141                         y-=p.y;
00142                         return *this;
00143                 }
00144 
00145                 inline TPoint2D &operator*=(double d)   {
00146                         x*=d;
00147                         y*=d;
00148                         return *this;
00149                 }
00150 
00151                 inline TPoint2D &operator/=(double d)   {
00152                         x/=d;
00153                         y/=d;
00154                         return *this;
00155                 }
00156 
00157                 inline TPoint2D operator+(const TPoint2D &p) const      {
00158                         TPoint2D r(*this);
00159                         return r+=p;
00160                 }
00161 
00162                 inline TPoint2D operator-(const TPoint2D &p) const      {
00163                         TPoint2D r(*this);
00164                         return r-=p;
00165                 }
00166 
00167                 inline TPoint2D operator*(double d) const       {
00168                         TPoint2D r(*this);
00169                         return r*=d;
00170                 }
00171 
00172                 inline TPoint2D operator/(double d) const       {
00173                         TPoint2D r(*this);
00174                         return r/=d;
00175                 }
00176                  /** Returns a human-readable textual representation of the object (eg: "[0.02 1.04]" )
00177                    * \sa fromString
00178                    */
00179                 void asString(std::string &s) const { s = mrpt::format("[%f %f]",x,y); }
00180                 inline std::string asString() const { std::string s; asString(s); return s; }
00181 
00182                  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04]" )
00183                    * \sa asString
00184                    * \exception std::exception On invalid format
00185                    */
00186                  void fromString(const std::string &s);
00187                  static size_t size() { return 2; }
00188         };
00189 
00190         /**
00191           * Lightweight 2D pose. Allows coordinate access using [] operator.
00192           * \sa mrpt::poses::CPose2D
00193           */
00194         struct BASE_IMPEXP TPose2D      {
00195                 /**
00196                   * X coordinate.
00197                   */
00198                 double x;
00199                 /**
00200                   * Y coordinate.
00201                   */
00202                 double y;
00203                 /**
00204                   * Phi coordinate.
00205                   */
00206                 double phi;
00207                 /**
00208                   * Implicit constructor from TPoint2D. Zeroes the phi coordinate.
00209                   * \sa TPoint2D
00210                   */
00211                 TPose2D(const TPoint2D &p);
00212                 /**
00213                   * Constructor from TPoint3D, losing information. Zeroes the phi coordinate.
00214                   * \sa TPoint3D
00215                   */
00216                 explicit TPose2D(const TPoint3D &p);
00217                 /**
00218                   * Constructor from TPose3D, losing information. The phi corresponds to the original pose's yaw.
00219                   * \sa TPose3D
00220                   */
00221                 explicit TPose2D(const TPose3D &p);
00222                 /**
00223                   * Implicit constructor from heavyweight type.
00224                   * \sa mrpt::poses::CPose2D
00225                   */
00226                 TPose2D(const mrpt::poses::CPose2D &p);
00227                 /**
00228                   * Constructor from coordinates.
00229                   */
00230                 inline TPose2D(double xx,double yy,double pphi):x(xx),y(yy),phi(pphi)   {}
00231                 /**
00232                   * Default fast constructor. Initializes to garbage.
00233                   */
00234                 inline TPose2D()        {}
00235                 /**
00236                   * Unsafe coordinate access using operator[]. Intended for loops.
00237                   */
00238                 inline double &operator[](size_t i)     {
00239                         return (&x)[i];
00240                 }
00241                 /**
00242                   * Unsafe coordinate access using operator[]. Intended for loops.
00243                   */
00244                 inline const double &operator[](size_t i) const {
00245                         return (&x)[i];
00246                 }
00247                 /**
00248                   * Transformation into vector.
00249                   */
00250                 inline void getAsVector(vector_double &v) const {
00251                         v.resize(3);
00252                         v[0]=x; v[1]=y; v[2]=phi;
00253                 }
00254                  /** Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees)
00255                    * \sa fromString
00256                    */
00257                 void asString(std::string &s) const { s = mrpt::format("[%f %f %f]",x,y,RAD2DEG(phi)); }
00258                 inline std::string asString() const { std::string s; asString(s); return s; }
00259 
00260                  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -45.0]" )
00261                    * \sa asString
00262                    * \exception std::exception On invalid format
00263                    */
00264                  void fromString(const std::string &s);
00265                  static size_t size() { return 3; }
00266         };
00267 
00268         /** Lightweight 3D point (float version).
00269           * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3D
00270           */
00271         struct BASE_IMPEXP TPoint3Df
00272         {
00273                 float x;
00274                 float y;
00275                 float z;
00276 
00277                 inline TPoint3Df() { }
00278                 inline TPoint3Df(const float xx,const float yy,const float zz) : x(xx), y(yy),z(zz) { }
00279                 inline TPoint3Df & operator +=(const TPoint3Df &p) { x+=p.x; y+=p.y; z+=p.z; return *this; }
00280                 inline TPoint3Df   operator *(const float s) { return TPoint3Df(x*s,y*s,z*s); }
00281         };
00282 
00283         /**
00284           * Lightweight 3D point. Allows coordinate access using [] operator.
00285           * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3Df
00286           */
00287         struct BASE_IMPEXP TPoint3D     {
00288                 double x; //!< X coordinate
00289                 double y; //!< Y coordinate
00290                 double z; //!< Z coordinate
00291 
00292                 /** Constructor from coordinates.  */
00293                 inline TPoint3D(double xx,double yy,double zz):x(xx),y(yy),z(zz)        {}
00294                 /** Default fast constructor. Initializes to garbage. */
00295                 inline TPoint3D()       {}
00296                 /** Explicit constructor from coordinates.  */
00297                 explicit inline TPoint3D(const TPoint3Df &p):x(p.x),y(p.y),z(p.z) {}
00298 
00299                 /** Implicit constructor from TPoint2D. Zeroes the z.
00300                   * \sa TPoint2D
00301                   */
00302                 TPoint3D(const TPoint2D &p);
00303                 /**
00304                   * Constructor from TPose2D, losing information. Zeroes the z.
00305                   * \sa TPose2D
00306                   */
00307                 explicit TPoint3D(const TPose2D &p);
00308                 /**
00309                   * Constructor from TPose3D, losing information.
00310                   * \sa TPose3D
00311                   */
00312                 explicit TPoint3D(const TPose3D &p);
00313                 /**
00314                   * Implicit constructor from heavyweight type.
00315                   * \sa mrpt::poses::CPoint3D
00316                   */
00317                 TPoint3D(const mrpt::poses::CPoint3D &p);
00318                 /**
00319                   * Constructor from heavyweight 3D pose.
00320                   * \sa mrpt::poses::CPose3D.
00321                   */
00322                 explicit TPoint3D(const mrpt::poses::CPose3D &p);
00323                 /**
00324                   * Unsafe coordinate access using operator[]. Intended for loops.
00325                   */
00326                 inline double &operator[](size_t i)     {
00327                         return (&x)[i];
00328                 }
00329                 /**
00330                   * Unsafe coordinate access using operator[]. Intended for loops.
00331                   */
00332                 inline const double &operator[](size_t i) const {
00333                         return (&x)[i];
00334                 }
00335                 /**
00336                   * Point-to-point distance.
00337                   */
00338                 inline double distanceTo(const TPoint3D &p) const       {
00339                         return sqrt(square(p.x-x)+square(p.y-y)+square(p.z-z));
00340                 }
00341                 /**
00342                   * Point-to-point distance, squared.
00343                   */
00344                 inline double sqrDistanceTo(const TPoint3D &p) const    {
00345                         return square(p.x-x)+square(p.y-y)+square(p.z-z);
00346                 }
00347                 /**
00348                   * Point norm.
00349                   */
00350                 inline double norm() const      {
00351                         return sqrt(square(x)+square(y)+square(z));
00352                 }
00353                 /**
00354                   * Point scale.
00355                   */
00356                 inline TPoint3D &operator*=(const double f) {
00357                         x*=f;y*=f;z*=f;
00358                         return *this;
00359                 }
00360                 /**
00361                   * Transformation into vector.
00362                   */
00363                 void getAsVector(vector_double &v) const {
00364                         v.resize(3);
00365                         v[0]=x; v[1]=y; v[2]=z;
00366                 }
00367                 /**
00368                   * Translation.
00369                   */
00370                 inline TPoint3D &operator+=(const TPoint3D &p)  {
00371                         x+=p.x;
00372                         y+=p.y;
00373                         z+=p.z;
00374                         return *this;
00375                 }
00376                 /**
00377                   * Difference between points.
00378                   */
00379                 inline TPoint3D &operator-=(const TPoint3D &p)  {
00380                         x-=p.x;
00381                         y-=p.y;
00382                         z-=p.z;
00383                         return *this;
00384                 }
00385                 /**
00386                   * Points addition.
00387                   */
00388                 inline TPoint3D operator+(const TPoint3D &p) const      {
00389                         return TPoint3D(x+p.x,y+p.y,z+p.z);
00390                 }
00391                 /**
00392                   * Points substraction.
00393                   */
00394                 inline TPoint3D operator-(const TPoint3D &p) const      {
00395                         return TPoint3D(x-p.x,y-p.y,z-p.z);
00396                 }
00397 
00398                 inline TPoint3D operator*(double d) const       {
00399                         return TPoint3D(x*d,y*d,z*d);
00400                 }
00401 
00402                 inline TPoint3D operator/(double d) const       {
00403                         return TPoint3D(x/d,y/d,z/d);
00404                 }
00405 
00406                 bool operator<(const TPoint3D &p) const;
00407 
00408                  /** Returns a human-readable textual representation of the object (eg: "[0.02 1.04 -0.8]" )
00409                    * \sa fromString
00410                    */
00411                 void asString(std::string &s) const { s = mrpt::format("[%f %f %f]",x,y,z); }
00412                 inline std::string asString() const { std::string s; asString(s); return s; }
00413 
00414                  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" )
00415                    * \sa asString
00416                    * \exception std::exception On invalid format
00417                    */
00418                  void fromString(const std::string &s);
00419                  static size_t size() { return 3; }
00420         };
00421 
00422         /**
00423           * Lightweight 3D pose (three spatial coordinates, plus three angular coordinates). Allows coordinate access using [] operator.
00424           * \sa mrpt::poses::CPose3D
00425           */
00426         struct BASE_IMPEXP TPose3D      {
00427                 /**
00428                   * X coordinate.
00429                   */
00430                 double x;
00431                 /**
00432                   * Y coordinate.
00433                   */
00434                 double y;
00435                 /**
00436                   * Z coordinate.
00437                   */
00438                 double z;
00439                 /**
00440                   * Yaw coordinate (rotation angle over Z axis).
00441                   */
00442                 double yaw;
00443                 /**
00444                   * Pitch coordinate (rotation angle over Y axis).
00445                   */
00446                 double pitch;
00447                 /**
00448                   * Roll coordinate (rotation angle over X coordinate).
00449                   */
00450                 double roll;
00451                 /**
00452                   * Implicit constructor from TPoint2D. Zeroes all the unprovided information.
00453                   * \sa TPoint2D
00454                   */
00455                 TPose3D(const TPoint2D &p);
00456                 /**
00457                   * Implicit constructor from TPose2D. Gets the yaw from the 2D pose's phi, zeroing all the unprovided information.
00458                   * \sa TPose2D
00459                   */
00460                 TPose3D(const TPose2D &p);
00461                 /**
00462                   * Implicit constructor from TPoint3D. Zeroes angular information.
00463                   * \sa TPoint3D
00464                   */
00465                 TPose3D(const TPoint3D &p);
00466                 /**
00467                   * Implicit constructor from heavyweight type.
00468                   * \sa mrpt::poses::CPose3D
00469                   */
00470                 TPose3D(const mrpt::poses::CPose3D &p);
00471                 /**
00472                   * Constructor from coordinates.
00473                   */
00474                 TPose3D(double _x,double _y,double _z,double _yaw,double _pitch,double _roll):x(_x),y(_y),z(_z),yaw(_yaw),pitch(_pitch),roll(_roll)     {}
00475                 /**
00476                   * Default fast constructor. Initializes to garbage.
00477                   */
00478                 inline TPose3D()        {}
00479                 /**
00480                   * Unsafe coordinate access using operator[]. Intended for loops.
00481                   */
00482                 inline double &operator[](size_t i)     {
00483                         return (&x)[i];
00484                 }
00485                 /**
00486                   * Unsafe coordinate access using operator[]. Intended for loops.
00487                   */
00488                 inline const double &operator[](size_t i) const {
00489                         return (&x)[i];
00490                 }
00491                 /**
00492                   * Pose's spatial coordinates norm.
00493                   */
00494                 double norm() const {
00495                         return sqrt(square(x)+square(y)+square(z));
00496                 }
00497                 /**
00498                   * Gets the pose as a vector of doubles.
00499                   */
00500                 void getAsVector(vector_double &v) const {
00501                         v.resize(6);
00502                         v[0]=x; v[1]=y; v[2]=z; v[3]=yaw; v[4]=pitch; v[5]=roll;
00503                 }
00504                  /** Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]", angles in degrees.)
00505                    * \sa fromString
00506                    */
00507                 void asString(std::string &s) const { s = mrpt::format("[%f %f %f %f %f %f]",x,y,z,RAD2DEG(yaw),RAD2DEG(pitch),RAD2DEG(roll)); }
00508                 inline std::string asString() const { std::string s; asString(s); return s; }
00509 
00510                  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8]" )
00511                    * \sa asString
00512                    * \exception std::exception On invalid format
00513                    */
00514                  void fromString(const std::string &s);
00515                  static size_t size() { return 6; }
00516         };
00517 
00518         /** Lightweight 3D pose (three spatial coordinates, plus a quaternion ). Allows coordinate access using [] operator.
00519           * \sa mrpt::poses::CPose3DQuat
00520           */
00521         struct BASE_IMPEXP TPose3DQuat  {
00522                 double x;       //!< Translation in x
00523                 double y;       //!< Translation in y
00524                 double z;       //!< Translation in z
00525                 double qr;  //!< Quaternion part, r
00526                 double qx;  //!< Quaternion part, x
00527                 double qy;  //!< Quaternion part, y
00528                 double qz;  //!< Quaternion part, z
00529 
00530                 /** Constructor from coordinates. */
00531                 inline TPose3DQuat(double _x,double _y,double _z,double _qr,double _qx, double _qy, double _qz):x(_x),y(_y),z(_z),qr(_qr),qx(_qx),qy(_qy),qz(_qz) {  }
00532                 /** Default fast constructor. Initializes to garbage. */
00533                 inline TPose3DQuat()    {}
00534                 /** Constructor from a CPose3DQuat */
00535                 TPose3DQuat(const mrpt::poses::CPose3DQuat &p);
00536 
00537                 /** Unsafe coordinate access using operator[]. Intended for loops. */
00538                 inline double &operator[](size_t i)     {
00539                         return (&x)[i];
00540                 }
00541                 /** Unsafe coordinate access using operator[]. Intended for loops. */
00542                 inline const double &operator[](size_t i) const {
00543                         return (&x)[i];
00544                 }
00545                 /** Pose's spatial coordinates norm. */
00546                 double norm() const {
00547                         return sqrt(square(x)+square(y)+square(z));
00548                 }
00549                 /** Gets the pose as a vector of doubles. */
00550                 void getAsVector(vector_double &v) const {
00551                         v.resize(7);
00552                         for (size_t i=0;i<7;i++) v[i]=(*this)[i];
00553                 }
00554                  /** Returns a human-readable textual representation of the object as "[x y z qr qx qy qz]"
00555                    * \sa fromString
00556                    */
00557                 void asString(std::string &s) const { s = mrpt::format("[%f %f %f %f %f %f %f]",x,y,z,qr,qx,qy,qz); }
00558                 inline std::string asString() const { std::string s; asString(s); return s; }
00559 
00560                  /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8 1.0 0.0 0.0 0.0]" )
00561                    * \sa asString
00562                    * \exception std::exception On invalid format
00563                    */
00564                  void fromString(const std::string &s);
00565                  static size_t size() { return 7; }
00566         };
00567 #pragma pack(pop)
00568 
00569         // Text streaming functions:
00570         std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPoint2D & p);
00571         std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPoint3D & p);
00572         std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose2D & p);
00573         std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose3D & p);
00574         std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose3DQuat & p);
00575 
00576 
00577         /**
00578           * Unary minus operator for 3D points.
00579           */
00580         inline TPoint3D operator-(const TPoint3D &p1)   {
00581                 return TPoint3D(-p1.x,-p1.y,-p1.z);
00582         }
00583         /**
00584           * Exact comparison between 2D points.
00585           */
00586         inline bool operator==(const TPoint2D &p1,const TPoint2D &p2)   {
00587                 return (p1.x==p2.x)&&(p1.y==p2.y);
00588         }
00589         /**
00590           * Exact comparison between 2D points.
00591           */
00592         inline bool operator!=(const TPoint2D &p1,const TPoint2D &p2)   {
00593                 return (p1.x!=p2.x)||(p1.y!=p2.y);
00594         }
00595         /**
00596           * Exact comparison between 3D points.
00597           */
00598         inline bool operator==(const TPoint3D &p1,const TPoint3D &p2)   {
00599                 return (p1.x==p2.x)&&(p1.y==p2.y)&&(p1.z==p2.z);
00600         }
00601         /**
00602           * Exact comparison between 3D points.
00603           */
00604         inline bool operator!=(const TPoint3D &p1,const TPoint3D &p2)   {
00605                 return (p1.x!=p2.x)||(p1.y!=p2.y)||(p1.z!=p2.z);
00606         }
00607         /**
00608           * Exact comparison between 2D poses, taking possible cycles into account.
00609           */
00610         inline bool operator==(const TPose2D &p1,const TPose2D &p2)     {
00611                 return (p1.x==p2.x)&&(p1.y==p2.y)&&(mrpt::math::wrapTo2Pi(p1.phi)==mrpt::math::wrapTo2Pi(p2.phi));
00612         }
00613         /**
00614           * Exact comparison between 2D poses, taking possible cycles into account.
00615           */
00616         inline bool operator!=(const TPose2D &p1,const TPose2D &p2)     {
00617                 return (p1.x!=p2.x)||(p1.y!=p2.y)||(mrpt::math::wrapTo2Pi(p1.phi)!=mrpt::math::wrapTo2Pi(p2.phi));
00618         }
00619         /**
00620           * Exact comparison between 3D poses, taking possible cycles into account.
00621           */
00622         inline bool operator==(const TPose3D &p1,const TPose3D &p2)     {
00623                 return (p1.x==p2.x)&&(p1.y==p2.y)&&(p1.z==p2.z)&&(mrpt::math::wrapTo2Pi(p1.yaw)==mrpt::math::wrapTo2Pi(p2.yaw))&&(mrpt::math::wrapTo2Pi(p1.pitch)==mrpt::math::wrapTo2Pi(p2.pitch))&&(mrpt::math::wrapTo2Pi(p1.roll)==mrpt::math::wrapTo2Pi(p2.roll));
00624         }
00625         /**
00626           * Exact comparison between 3D poses, taking possible cycles into account.
00627           */
00628         inline bool operator!=(const TPose3D &p1,const TPose3D &p2)     {
00629                 return (p1.x!=p2.x)||(p1.y!=p2.y)||(p1.z!=p2.z)||(mrpt::math::wrapTo2Pi(p1.yaw)!=mrpt::math::wrapTo2Pi(p2.yaw))||(mrpt::math::wrapTo2Pi(p1.pitch)!=mrpt::math::wrapTo2Pi(p2.pitch))||(mrpt::math::wrapTo2Pi(p1.roll)!=mrpt::math::wrapTo2Pi(p2.roll));
00630         }
00631         //Forward declarations
00632         struct BASE_IMPEXP TSegment3D;
00633         struct BASE_IMPEXP TLine3D;
00634         class BASE_IMPEXP TPolygon3D;
00635         struct BASE_IMPEXP TObject3D;
00636 
00637         //Pragma defined to ensure no structure packing
00638 #pragma pack(push,1)
00639         /**
00640           * 2D segment, consisting of two points.
00641           * \sa TSegment3D,TLine2D,TPolygon2D,TPoint2D
00642           */
00643         struct BASE_IMPEXP TSegment2D   {
00644         public:
00645                 /**
00646                   * Origin point.
00647                   */
00648                 TPoint2D point1;
00649                 /**
00650                   * Destiny point.
00651                   */
00652                 TPoint2D point2;
00653                 /**
00654                   * Segment length.
00655                   */
00656                 double length() const;
00657                 /**
00658                   * Distance to point.
00659                   */
00660                 double distance(const TPoint2D &point) const;
00661                 /**
00662                   * Distance with sign to point (sign indicates which side the point is).
00663                   */
00664                 double signedDistance(const TPoint2D &point) const;
00665                 /**
00666                   * Check whether a point is inside a segment.
00667                   */
00668                 bool contains(const TPoint2D &point) const;
00669                 /**
00670                   * Unsafe point access using [] operator, intended for loops.
00671                   */
00672                 inline TPoint2D &operator[](size_t i)   {
00673                         return (&point1)[i];
00674                 }
00675                 /**
00676                   * Unsafe point access using [] operator, intended for loops.
00677                   */
00678                 inline const TPoint2D &operator[](size_t i) const       {
00679                         return (&point1)[i];
00680                 }
00681                 /**
00682                   * Project into 3D space, setting the z to 0.
00683                   */
00684                 void generate3DObject(TSegment3D &s) const;
00685                 /**
00686                   * Segment's central point.
00687                   */
00688                 inline void getCenter(TPoint2D &p) const        {
00689                         p.x=(point1.x+point2.x)/2;
00690                         p.y=(point1.y+point2.y)/2;
00691                 }
00692                 /**
00693                   * Constructor from both points.
00694                   */
00695                 TSegment2D(const TPoint2D &p1,const TPoint2D &p2):point1(p1),point2(p2) {}
00696                 /**
00697                   * Fast default constructor. Initializes to garbage.
00698                   */
00699                 TSegment2D()    {}
00700                 /**
00701                   * Explicit constructor from 3D object, discarding the z.
00702                   */
00703                 explicit TSegment2D(const TSegment3D &s);
00704 
00705                 bool operator<(const TSegment2D &s) const;
00706         };
00707         /**
00708           * 3D segment, consisting of two points.
00709           * \sa TSegment2D,TLine3D,TPlane,TPolygon3D,TPoint3D
00710           */
00711         struct BASE_IMPEXP TSegment3D   {
00712         public:
00713                 /**
00714                   * Origin point.
00715                   */
00716                 TPoint3D point1;
00717                 /**
00718                   * Destiny point.
00719                   */
00720                 TPoint3D point2;
00721                 /**
00722                   * Segment length.
00723                   */
00724                 double length() const;
00725                 /**
00726                   * Distance to point.
00727                   */
00728                 double distance(const TPoint3D &point) const;
00729                 /**
00730                   * Check whether a point is inside the segment.
00731                   */
00732                 bool contains(const TPoint3D &point) const;
00733                 /**
00734                   * Unsafe point access using [] operator, intended for loops.
00735                   */
00736                 inline TPoint3D &operator[](size_t i)   {
00737                         return (&point1)[i];
00738                 }
00739                 /**
00740                   * Unsafe point access using [] operator, intended for loops.
00741                   */
00742                 inline const TPoint3D &operator[](size_t i) const       {
00743                         return (&point1)[i];
00744                 }
00745                 /**
00746                   * Projection into 2D space, discarding the z.
00747                   */
00748                 inline void generate2DObject(TSegment2D &s) const       {
00749                         s=TSegment2D(*this);
00750                 }
00751                 /**
00752                   * Segment's central point.
00753                   */
00754                 inline void getCenter(TPoint3D &p) const        {
00755                         p.x=(point1.x+point2.x)/2;
00756                         p.y=(point1.y+point2.y)/2;
00757                         p.z=(point1.z+point2.z)/2;
00758                 }
00759                 /**
00760                   * Constructor from both points.
00761                   */
00762                 TSegment3D(const TPoint3D &p1,const TPoint3D &p2):point1(p1),point2(p2) {}
00763                 /**
00764                   * Fast default constructor. Initializes to garbage.
00765                   */
00766                 TSegment3D()    {}
00767                 /**
00768                   * Constructor from 2D object. Sets the z to zero.
00769                   */
00770                 TSegment3D(const TSegment2D &s):point1(s.point1),point2(s.point2)       {}
00771 
00772                 bool operator<(const TSegment3D &s) const;
00773         };
00774 #pragma pack(pop)
00775 
00776         inline bool operator==(const TSegment2D &s1,const TSegment2D &s2)       {
00777                 return (s1.point1==s2.point1)&&(s1.point2==s2.point2);
00778         }
00779 
00780         inline bool operator!=(const TSegment2D &s1,const TSegment2D &s2)       {
00781                 return (s1.point1!=s1.point1)||(s1.point2!=s2.point2);
00782         }
00783 
00784         inline bool operator==(const TSegment3D &s1,const TSegment3D &s2)       {
00785                 return (s1.point1==s2.point1)&&(s1.point2==s2.point2);
00786         }
00787 
00788         inline bool operator!=(const TSegment3D &s1,const TSegment3D &s2)       {
00789                 return (s1.point1!=s1.point1)||(s1.point2!=s2.point2);
00790         }
00791 
00792         /**
00793           * 2D line without bounds, represented by its equation \f$Ax+By+C=0\f$.
00794           * \sa TLine3D,TSegment2D,TPolygon2D,TPoint2D
00795           */
00796         struct BASE_IMPEXP TLine2D      {
00797         public:
00798                 /**
00799                   * Line coefficients, stored as an array: \f$\left[A,B,C\right]\f$.
00800                   */
00801                 double coefs[3];
00802                 /**
00803                   * Evaluate point in the line's equation.
00804                   */
00805                 double evaluatePoint(const TPoint2D &point) const;
00806                 /**
00807                   * Check whether a point is inside the line.
00808                   */
00809                 bool contains(const TPoint2D &point) const;
00810                 /**
00811                   * Distance from a given point.
00812                   */
00813                 double distance(const TPoint2D &point) const;
00814                 /**
00815                   * Distance with sign from a given point (sign indicates side).
00816                   */
00817                 double signedDistance(const TPoint2D &point) const;
00818                 /**
00819                   * Get line's normal vector.
00820                   */
00821                 void getNormalVector(double (&vector)[2]) const;
00822                 /**
00823                   * Unitarize line's normal vector.
00824                   */
00825                 void unitarize();
00826                 /**
00827                   * Get line's normal vector after unitarizing line.
00828                   */
00829                 inline void getUnitaryNormalVector(double (&vector)[2]) {
00830                         unitarize();
00831                         getNormalVector(vector);
00832                 }
00833                 /**
00834                   * Get line's director vector.
00835                   */
00836                 void getDirectorVector(double (&vector)[2]) const;
00837                 /**
00838                   * Unitarize line and then get director vector.
00839                   */
00840                 inline void getUnitaryDirectorVector(double (&vector)[2])       {
00841                         unitarize();
00842                         getDirectorVector(vector);
00843                 }
00844                 /**
00845                   * Project into 3D space, setting the z to 0.
00846                   */
00847                 void generate3DObject(TLine3D &l) const;
00848                 /**
00849                   * Get a pose2D whose X axis corresponds to the line.
00850                   * \sa mrpt::poses::CPose2D.
00851                   */
00852                 void getAsPose2D(mrpt::poses::CPose2D &outPose) const;
00853                 /**
00854                   * Get a pose2D whose X axis corresponds to the line, forcing the base point to one given.
00855                   * \throw logic_error if the point is not inside the line.
00856                   * \sa mrpt::poses::CPose2D.
00857                   */
00858                 void getAsPose2DForcingOrigin(const TPoint2D &origin,mrpt::poses::CPose2D &outPose) const;
00859                 /**
00860                   * Constructor from two points, through which the line will pass.
00861                   * \throw logic_error if both points are the same
00862                   */
00863                 TLine2D(const TPoint2D &p1,const TPoint2D &p2) throw(std::logic_error);
00864                 /**
00865                   * Constructor from a segment.
00866                   */
00867                 explicit TLine2D(const TSegment2D &s);
00868                 /**
00869                   * Fast default constructor. Initializes to garbage.
00870                   */
00871                 TLine2D()       {}
00872                 /**
00873                   * Constructor from line's coefficients.
00874                   */
00875                 inline TLine2D(double A,double B,double C)      {
00876                         coefs[0]=A;
00877                         coefs[1]=B;
00878                         coefs[2]=C;
00879                 }
00880                 /**
00881                   * Construction from 3D object, discarding the Z.
00882                   * \throw std::logic_error if the line is normal to the XY plane.
00883                   */
00884                 explicit TLine2D(const TLine3D &l);
00885         };
00886 
00887         /**
00888           * 3D line, represented by a base point and a director vector.
00889           * \sa TLine2D,TSegment3D,TPlane,TPolygon3D,TPoint3D
00890           */
00891         struct BASE_IMPEXP TLine3D      {
00892         public:
00893                 /**
00894                   * Base point.
00895                   */
00896                 TPoint3D pBase;
00897                 /**
00898                   * Director vector.
00899                   */
00900                 double director[3];
00901                 /**
00902                   * Check whether a point is inside the line.
00903                   */
00904                 bool contains(const TPoint3D &point) const;
00905                 /**
00906                   * Distance between the line and a point.
00907                   */
00908                 double distance(const TPoint3D &point) const;
00909                 /**
00910                   * Unitarize director vector.
00911                   */
00912                 void unitarize();
00913                 /**
00914                   * Get director vector.
00915                   */
00916                 inline void getDirectorVector(double (&vector)[3]) const        {
00917                         for (size_t i=0;i<3;i++) vector[i]=director[i];
00918                 }
00919                 /**
00920                   * Unitarize and then get director vector.
00921                   */
00922                 inline void getUnitaryDirectorVector(double (&vector)[3])       {
00923                         unitarize();
00924                         getDirectorVector(vector);
00925                 }
00926                 /**
00927                   * Project into 2D space, discarding the Z coordinate.
00928                   * \throw std::logic_error if the line's director vector is orthogonal to the XY plane.
00929                   */
00930                 inline void generate2DObject(TLine2D &l) const  {
00931                         l=TLine2D(*this);
00932                 }
00933                 /**
00934                   * Constructor from two points, through which the line will pass.
00935                   * \throw std::logic_error if both points are the same.
00936                   */
00937                 TLine3D(const TPoint3D &p1,const TPoint3D &p2) throw(std::logic_error);
00938                 /**
00939                   * Constructor from 3D segment.
00940                   */
00941                 explicit TLine3D(const TSegment3D &s);
00942                 /**
00943                   * Fast default constructor. Initializes to garbage.
00944                   */
00945                 TLine3D()       {}
00946                 /**
00947                   * Implicit constructor from 2D object. Zeroes the z.
00948                   */
00949                 TLine3D(const TLine2D &l);
00950         };
00951 
00952         /**
00953           * 3D Plane, represented by its equation \f$Ax+By+Cz+D=0\f$
00954           * \sa TSegment3D,TLine3D,TPolygon3D,TPoint3D
00955           */
00956         struct BASE_IMPEXP TPlane       {
00957         public:
00958                 /**
00959                   * Plane coefficients, stored as an array: \f$\left[A,B,C,D\right]\f$
00960                   */
00961                 double coefs[4];
00962                 /**
00963                   * Evaluate a point in the plane's equation.
00964                   */
00965                 double evaluatePoint(const TPoint3D &point) const;
00966                 /**
00967                   * Check whether a point is contained into the plane.
00968                   */
00969                 bool contains(const TPoint3D &point) const;
00970                 /**
00971                   * Check whether a segment is fully contained into the plane.
00972                   */
00973                 inline bool contains(const TSegment3D &segment) const   {
00974                         return contains(segment.point1)&&contains(segment.point2);
00975                 }
00976                 /**
00977                   * Check whether a line is fully contained into the plane.
00978                   */
00979                 bool contains(const TLine3D &line) const;
00980                 /**
00981                   * Distance to 3D point.
00982                   */
00983                 double distance(const TPoint3D &point) const;
00984                 /**
00985                   * Distance to 3D line. Will be zero if the line is not parallel to the plane.
00986                   */
00987                 double distance(const TLine3D &line) const;
00988                 /**
00989                   * Get plane's normal vector.
00990                   */
00991                 void getNormalVector(double (&vec)[3]) const;
00992                 /**
00993                   * Unitarize normal vector.
00994                   */
00995                 void unitarize();
00996                 /**
00997                   * Unitarize, then get normal vector.
00998                   */
00999                 inline void getUnitaryNormalVector(double (&vec)[3])    {
01000                         unitarize();
01001                         getNormalVector(vec);
01002                 }
01003                 /**
01004                   * Gets a pose whose XY plane corresponds to this plane.
01005                   */
01006                 void getAsPose3D(mrpt::poses::CPose3D &outPose);
01007                 /**
01008                   * Gets a pose whose XY plane corresponds to this plane.
01009                   */
01010                 inline void getAsPose3D(mrpt::poses::CPose3D &outPose) const    {
01011                         TPlane p=*this;
01012                         p.getAsPose3D(outPose);
01013                 }
01014                 /**
01015                   * Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates.
01016                   * \throw std::logic_error if the point is not inside the plane.
01017                   */
01018                 void getAsPose3DForcingOrigin(const TPoint3D &newOrigin,mrpt::poses::CPose3D &pose);
01019                 /**
01020                   * Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates.
01021                   * \throw std::logic_error if the point is not inside the plane.
01022                   */
01023                 inline void getAsPose3DForcingOrigin(const TPoint3D &newOrigin,mrpt::poses::CPose3D &pose) const        {
01024                         TPlane p=*this;
01025                         p.getAsPose3DForcingOrigin(newOrigin,pose);
01026                 }
01027                 /**
01028                   * Gets a plane which contains these three points.
01029                   * \throw std::logic_error if the points are linearly dependants.
01030                   */
01031                 TPlane(const TPoint3D &p1,const TPoint3D &p2,const TPoint3D &p3) throw(std::logic_error);
01032                 /**
01033                   * Gets a plane which contains this point and this line.
01034                   * \throw std::logic_error if the point is inside the line.
01035                   */
01036                 TPlane(const TPoint3D &p1,const TLine3D &r2) throw(std::logic_error);
01037                 /**
01038                   * Gets a plane which contains the two lines.
01039                   * \throw std::logic_error if the lines do not cross.
01040                   */
01041                 TPlane(const TLine3D &r1,const TLine3D &r2) throw(std::logic_error);
01042                 /**
01043                   * Fast default constructor. Initializes to garbage.
01044                   */
01045                 TPlane()        {}
01046                 /**
01047                   * Constructor from plane coefficients.
01048                   */
01049                 inline TPlane(double A,double B,double C,double D)      {
01050                         coefs[0]=A;
01051                         coefs[1]=B;
01052                         coefs[2]=C;
01053                         coefs[3]=D;
01054                 }
01055                 /**
01056                   * Constructor from an array of coefficients.
01057                   */
01058                 inline TPlane(const double (&vec)[4])   {
01059                         for (size_t i=0;i<4;i++) coefs[i]=vec[i];
01060                 }
01061         };
01062 
01063         typedef TPlane TPlane3D;
01064 
01065         /**
01066           * 2D polygon, inheriting from std::vector<TPoint2D>.
01067           * \sa TPolygon3D,TSegment2D,TLine2D,TPoint2D, CPolygon
01068           */
01069         class BASE_IMPEXP TPolygon2D:public std::vector<TPoint2D>       {
01070         public:
01071                 /**
01072                   * Distance to a point.
01073                   */
01074                 double distance(const TPoint2D &point) const;
01075                 /**
01076                   * Check whether a point is inside the polygon.
01077                   */
01078                 bool contains(const TPoint2D &point) const;
01079                 /**
01080                   * Gets as set of segments, instead of points.
01081                   */
01082                 void getAsSegmentList(std::vector<TSegment2D> &v) const;
01083                 /**
01084                   * Projects into 3D space, zeroing the z.
01085                   */
01086                 void generate3DObject(TPolygon3D &p) const;
01087                 /**
01088                   * Polygon's central point.
01089                   */
01090                 void getCenter(TPoint2D &p) const;
01091                 /**
01092                   * Checks whether is convex.
01093                   */
01094                 bool isConvex() const;
01095                 /**
01096                   * Erase repeated vertices.
01097                   * \sa removeRedundantVertices
01098                   */
01099                 void removeRepeatedVertices();
01100                 /**
01101                   * Erase every redundant vertex from the polygon, saving space.
01102                   * \sa removeRepeatedVertices
01103                   */
01104                 void removeRedundantVertices();
01105                 /**
01106                   * Gets plot data, ready to use on a 2D plot.
01107                   * \sa mrpt::gui::CDisplayWindowPlots
01108                   */
01109                 void getPlotData(std::vector<double> &x,std::vector<double> &y) const;
01110                 /**
01111                   * Default constructor.
01112                   */
01113                 TPolygon2D():std::vector<TPoint2D>()    {}
01114                 /**
01115                   * Constructor for a given number of vertices, intializing them as garbage.
01116                   */
01117                 explicit TPolygon2D(size_t N):std::vector<TPoint2D>(N)  {}
01118                 /**
01119                   * Implicit constructor from a vector of 2D points.
01120                   */
01121                 TPolygon2D(const std::vector<TPoint2D> &v):std::vector<TPoint2D>(v)     {}
01122                 /**
01123                   * Constructor from a 3D object.
01124                   */
01125                 explicit TPolygon2D(const TPolygon3D &p);
01126                 /**
01127                   * Static method to create a regular polygon, given its size and radius.
01128                   * \throw std::logic_error if radius is near zero or the number of edges is less than three.
01129                   */
01130                 static void createRegularPolygon(size_t numEdges,double radius,TPolygon2D &poly);
01131                 /**
01132                   * Static method to create a regular polygon from its size and radius. The center will correspond to the given pose.
01133                   * \throw std::logic_error if radius is near zero or the number of edges is less than three.
01134                   */
01135                 static inline void createRegularPolygon(size_t numEdges,double radius,TPolygon2D &poly,const mrpt::poses::CPose2D &pose);
01136         };
01137 
01138         /**
01139           * 3D polygon, inheriting from std::vector<TPoint3D>
01140           * \sa TPolygon2D,TSegment3D,TLine3D,TPlane,TPoint3D
01141           */
01142         class BASE_IMPEXP TPolygon3D:public std::vector<TPoint3D>       {
01143         public:
01144                 /**
01145                   * Distance to point.
01146                   */
01147                 double distance(const TPoint3D &point) const;
01148                 /**
01149                   * Check whether a point is inside the polygon.
01150                   */
01151                 bool contains(const TPoint3D &point) const;
01152                 /**
01153                   * Gets as set of segments, instead of set of points.
01154                   */
01155                 void getAsSegmentList(std::vector<TSegment3D> &v) const;
01156                 /**
01157                   * Gets a plane which contains the polygon. Returns false if the polygon is skew and cannot be fit inside a plane.
01158                   */
01159                 bool getPlane(TPlane &p) const;
01160                 /**
01161                   * Gets the best fitting plane, disregarding whether the polygon actually fits inside or not.
01162                   * \sa getBestFittingPlane
01163                   */
01164                 void getBestFittingPlane(TPlane &p) const;
01165                 /**
01166                   * Projects into a 2D space, discarding the z.
01167                   * \get getPlane,isSkew
01168                   */
01169                 inline void generate2DObject(TPolygon2D &p) const       {
01170                         p=TPolygon2D(*this);
01171                 }
01172                 /**
01173                   * Get polygon's central point.
01174                   */
01175                 void getCenter(TPoint3D &p) const;
01176                 /**
01177                   * Check whether the polygon is skew. Returns true if there doesn't exist a plane in which the polygon can fit.
01178                   * \sa getBestFittingPlane
01179                   */
01180                 bool isSkew() const;
01181                 /**
01182                   * Remove polygon's repeated vertices.
01183                   */
01184                 void removeRepeatedVertices();
01185                 /**
01186                   * Erase every redundant vertex, thus saving space.
01187                   */
01188                 void removeRedundantVertices();
01189                 /**
01190                   * Default constructor. Creates a polygon with no vertices.
01191                   */
01192                 TPolygon3D():std::vector<TPoint3D>()    {}
01193                 /**
01194                   * Constructor for a given size. Creates a polygon with a fixed number of vertices, which are initialized to garbage.
01195                   */
01196                 explicit TPolygon3D(size_t N):std::vector<TPoint3D>(N)  {}
01197                 /**
01198                   * Implicit constructor from a 3D points vector.
01199                   */
01200                 TPolygon3D(const std::vector<TPoint3D> &v):std::vector<TPoint3D>(v)     {}
01201                 /**
01202                   * Constructor from a 2D object. Zeroes the z.
01203                   */
01204                 TPolygon3D(const TPolygon2D &p);
01205                 /**
01206                   * Static method to create a regular polygon, given its size and radius.
01207                   * \throw std::logic_error if number of edges is less than three, or radius is near zero.
01208                   */
01209                 static void createRegularPolygon(size_t numEdges,double radius,TPolygon3D &poly);
01210                 /**
01211                   * Static method to create a regular polygon, given its size and radius. The center will be located on the given pose.
01212                   * \throw std::logic_error if number of edges is less than three, or radius is near zero.
01213                   */
01214                 static inline void createRegularPolygon(size_t numEdges,double radius,TPolygon3D &poly,const mrpt::poses::CPose3D &pose);
01215         };
01216 
01217         /**
01218           * Object type identifier for TPoint2D or TPoint3D.
01219           * \sa TObject2D,TObject3D
01220           */
01221         const unsigned char GEOMETRIC_TYPE_POINT=0;
01222         /**
01223           * Object type identifier for TSegment2D or TSegment3D.
01224           * \sa TObject2D,TObject3D
01225           */
01226         const unsigned char GEOMETRIC_TYPE_SEGMENT=1;
01227         /**
01228           * Object type identifier for TLine2D or TLine3D.
01229           * \sa TObject2D,TObject3D
01230           */
01231         const unsigned char GEOMETRIC_TYPE_LINE=2;
01232         /**
01233           * Object type identifier for TPolygon2D or TPolygon3D.
01234           * \sa TObject2D,TObject3D
01235           */
01236         const unsigned char GEOMETRIC_TYPE_POLYGON=3;
01237         /**
01238           * Object type identifier for TPlane.
01239           * \sa TObject3D
01240           */
01241         const unsigned char GEOMETRIC_TYPE_PLANE=4;
01242         /**
01243           * Object type identifier for empty TObject2D or TObject3D.
01244           * \sa TObject2D,TObject3D
01245           */
01246         const unsigned char GEOMETRIC_TYPE_UNDEFINED=255;
01247 
01248         /**
01249           * Standard type for storing any lightweight 2D type. Do not inherit from this class.
01250           * \sa TPoint2D,TSegment2D,TLine2D,TPolygon2D
01251           */
01252 #ifdef TOBJECTS_USE_UNIONS
01253         struct BASE_IMPEXP TObject2D    {
01254         private:
01255                 /**
01256                   * Object type identifier.
01257                   */
01258                 unsigned char type;
01259                 /**
01260                   * Union type storing pointers to every allowed type.
01261                   */
01262                 union   {
01263                         TPoint2D *point;
01264                         TSegment2D *segment;
01265                         TLine2D *line;
01266                         TPolygon2D *polygon;
01267                 }       data;
01268                 /**
01269                   * Destroys the object, releasing the pointer to the content (if any).
01270                   */
01271                 void destroy()  {
01272                         switch(type)    {
01273                                 case GEOMETRIC_TYPE_POINT:
01274                                         delete data.point;
01275                                         break;
01276                                 case GEOMETRIC_TYPE_SEGMENT:
01277                                         delete data.segment;
01278                                         break;
01279                                 case GEOMETRIC_TYPE_LINE:
01280                                         delete data.line;
01281                                         break;
01282                                 case GEOMETRIC_TYPE_POLYGON:
01283                                         delete data.polygon;
01284                                         break;
01285                         }
01286                         type=GEOMETRIC_TYPE_UNDEFINED;
01287                 }
01288         public:
01289                 /**
01290                   * Implicit constructor from point.
01291                   */
01292                 TObject2D(const TPoint2D &p):type(GEOMETRIC_TYPE_POINT) {
01293                         data.point=new TPoint2D(p);
01294                 }
01295                 /**
01296                   * Implicit constructor from segment.
01297                   */
01298                 TObject2D(const TSegment2D &s):type(GEOMETRIC_TYPE_SEGMENT)     {
01299                         data.segment=new TSegment2D(s);
01300                 }
01301                 /**
01302                   * Implicit constructor from line.
01303                   */
01304                 TObject2D(const TLine2D &r):type(GEOMETRIC_TYPE_LINE)   {
01305                         data.line=new TLine2D(r);
01306                 }
01307                 /**
01308                   * Implicit constructor from polygon.
01309                   */
01310                 TObject2D(const TPolygon2D &p):type(GEOMETRIC_TYPE_POLYGON)     {
01311                         data.polygon=new TPolygon2D(p);
01312                 }
01313                 /**
01314                   * Implicit constructor from polygon.
01315                   */
01316                 TObject2D():type(GEOMETRIC_TYPE_UNDEFINED)      {}
01317                 /**
01318                   * Object destruction.
01319                   */
01320                 ~TObject2D()    {
01321                         destroy();
01322                 }
01323                 /**
01324                   * Checks whether content is a point.
01325                   */
01326                 inline bool isPoint() const     {
01327                         return type==GEOMETRIC_TYPE_POINT;
01328                 }
01329                 /**
01330                   * Checks whether content is a segment.
01331                   */
01332                 inline bool isSegment() const   {
01333                         return type==GEOMETRIC_TYPE_SEGMENT;
01334                 }
01335                 /**
01336                   * Checks whether content is a line.
01337                   */
01338                 inline bool isLine() const      {
01339                         return type==GEOMETRIC_TYPE_LINE;
01340                 }
01341                 /**
01342                   * Checks whether content is a polygon.
01343                   */
01344                 inline bool isPolygon() const   {
01345                         return type==GEOMETRIC_TYPE_POLYGON;
01346                 }
01347                 /**
01348                   * Gets content type.
01349                   */
01350                 inline unsigned char getType() const    {
01351                         return type;
01352                 }
01353                 /**
01354                   * Gets the content as a point, returning false if the type is inadequate.
01355                   */
01356                 inline bool getPoint(TPoint2D &p) const {
01357                         if (isPoint())  {
01358                                 p=*(data.point);
01359                                 return true;
01360                         }       else return false;
01361                 }
01362                 /**
01363                   * Gets the content as a segment, returning false if the type is inadequate.
01364                   */
01365                 inline bool getSegment(TSegment2D &s) const     {
01366                         if (isSegment())        {
01367                                 s=*(data.segment);
01368                                 return true;
01369                         }       else return false;
01370                 }
01371                 /**
01372                   * Gets the content as a line, returning false if the type is inadequate.
01373                   */
01374                 inline bool getLine(TLine2D &r) const   {
01375                         if (isLine())   {
01376                                 r=*(data.line);
01377                                 return true;
01378                         }       else return false;
01379                 }
01380                 /**
01381                   * Gets the content as a polygon, returning false if the type is inadequate.
01382                   */
01383                 inline bool getPolygon(TPolygon2D &p) const     {
01384                         if (isPolygon())        {
01385                                 p=*(data.polygon);
01386                                 return true;
01387                         }       else return false;
01388                 }
01389                 /**
01390                   * Assign another TObject2D. Pointers are not shared.
01391                   */
01392                 void operator=(const TObject2D &obj)    {
01393                         if (this==&obj) return;
01394                         destroy();
01395                         switch (type=obj.type)  {
01396                                 case GEOMETRIC_TYPE_POINT:
01397                                         data.point=new TPoint2D(*(obj.data.point));
01398                                         break;
01399                                 case GEOMETRIC_TYPE_SEGMENT:
01400                                         data.segment=new TSegment2D(*(obj.data.segment));
01401                                         break;
01402                                 case GEOMETRIC_TYPE_LINE:
01403                                         data.line=new TLine2D(*(obj.data.line));
01404                                         break;
01405                                 case GEOMETRIC_TYPE_POLYGON:
01406                                         data.polygon=new TPolygon2D(*(obj.data.polygon));
01407                                         break;
01408                         }
01409                 }
01410                 /**
01411                   * Assign a point to this object.
01412                   */
01413                 inline void operator=(const TPoint2D &p)        {
01414                         destroy();
01415                         type=GEOMETRIC_TYPE_POINT;
01416                         data.point=new TPoint2D(p);
01417                 }
01418                 /**
01419                   * Assign a segment to this object.
01420                   */
01421                 inline void operator=(const TSegment2D &s)      {
01422                         destroy();
01423                         type=GEOMETRIC_TYPE_SEGMENT;
01424                         data.segment=new TSegment2D(s);
01425                 }
01426                 /**
01427                   * Assign a line to this object.
01428                   */
01429                 inline void operator=(const TLine2D &l) {
01430                         destroy();
01431                         type=GEOMETRIC_TYPE_LINE;
01432                         data.line=new TLine2D(l);
01433                 }
01434                 /**
01435                   * Assign a polygon to this object.
01436                   */
01437                 inline void operator=(const TPolygon2D &p)      {
01438                         destroy();
01439                         type=GEOMETRIC_TYPE_POLYGON;
01440                         data.polygon=new TPolygon2D(p);
01441                 }
01442                 /**
01443                   * Project into 3D space.
01444                   */
01445                 void generate3DObject(TObject3D &obj) const;
01446                 /**
01447                   * Constructor from another TObject2D.
01448                   */
01449                 TObject2D(const TObject2D &obj):type(GEOMETRIC_TYPE_UNDEFINED)  {
01450                         operator=(obj);
01451                 }
01452                 /**
01453                   * Static method to retrieve all the points in a vector of TObject2D.
01454                   */
01455                 static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts);
01456                 /**
01457                   * Static method to retrieve all the segments in a vector of TObject2D.
01458                   */
01459                 static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms);
01460                 /**
01461                   * Static method to retrieve all the lines in a vector of TObject2D.
01462                   */
01463                 static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins);
01464                 /**
01465                   * Static method to retrieve all the polygons in a vector of TObject2D.
01466                   */
01467                 static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys);
01468                 /**
01469                   * Static method to retrieve all the points in a vector of TObject2D, returning the remainder objects in another parameter.
01470                   */
01471                 static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts,std::vector<TObject2D> &remainder);
01472                 /**
01473                   * Static method to retrieve all the segments in a vector of TObject2D, returning the remainder objects in another parameter.
01474                   */
01475                 static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms,std::vector<TObject2D> &remainder);
01476                 /**
01477                   * Static method to retrieve all the lines in a vector of TObject2D, returning the remainder objects in another parameter.
01478                   */
01479                 static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins,std::vector<TObject2D> &remainder);
01480                 /**
01481                   * Static method to retrieve all the polygons in a vector of TObject2D, returning the remainder objects in another parameter.
01482                   */
01483                 static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys,std::vector<TObject2D> &remainder);
01484         };
01485         /**
01486           * Standard object for storing any 3D lightweight object. Do not inherit from this class.
01487           * \sa TPoint3D,TSegment3D,TLine3D,TPlane,TPolygon3D
01488           */
01489         struct BASE_IMPEXP TObject3D    {
01490         private:
01491                 /**
01492                   * Object type identifier.
01493                   */
01494                 unsigned char type;
01495                 /**
01496                   * Union containing pointer to actual data.
01497                   */
01498                 union   {
01499                         TPoint3D *point;
01500                         TSegment3D *segment;
01501                         TLine3D *line;
01502                         TPolygon3D *polygon;
01503                         TPlane *plane;
01504                 }       data;
01505                 /**
01506                   * Destroys the object and releases the pointer, if any.
01507                   */
01508                 void destroy()  {
01509                         switch (type)   {
01510                                 case GEOMETRIC_TYPE_POINT:
01511                                         delete data.point;
01512                                         break;
01513                                 case GEOMETRIC_TYPE_SEGMENT:
01514                                         delete data.segment;
01515                                         break;
01516                                 case GEOMETRIC_TYPE_LINE:
01517                                         delete data.line;
01518                                         break;
01519                                 case GEOMETRIC_TYPE_POLYGON:
01520                                         delete data.polygon;
01521                                         break;
01522                                 case GEOMETRIC_TYPE_PLANE:
01523                                         delete data.plane;
01524                                         break;
01525                                 case GEOMETRIC_TYPE_UNDEFINED:
01526                                         break;
01527                                 default:
01528                                         THROW_EXCEPTION("Invalid TObject2D object");
01529                         }
01530                         type=GEOMETRIC_TYPE_UNDEFINED;
01531                 }
01532         public:
01533                 /**
01534                   * Constructor from point.
01535                   */
01536                 TObject3D(const TPoint3D &p):type(GEOMETRIC_TYPE_POINT) {
01537                         data.point=new TPoint3D(p);
01538                 }
01539                 /**
01540                   * Constructor from segment.
01541                   */
01542                 TObject3D(const TSegment3D &s):type(GEOMETRIC_TYPE_SEGMENT)     {
01543                         data.segment=new TSegment3D(s);
01544                 }
01545                 /**
01546                   * Constructor from line.
01547                   */
01548                 TObject3D(const TLine3D &r):type(GEOMETRIC_TYPE_LINE)   {
01549                         data.line=new TLine3D(r);
01550                 }
01551                 /**
01552                   * Constructor from polygon.
01553                   */
01554                 TObject3D(const TPolygon3D &p):type(GEOMETRIC_TYPE_POLYGON)     {
01555                         data.polygon=new TPolygon3D(p);
01556                 }
01557                 /**
01558                   * Constructor from plane.
01559                   */
01560                 TObject3D(const TPlane &p):type(GEOMETRIC_TYPE_PLANE)   {
01561                         data.plane=new TPlane(p);
01562                 }
01563                 /**
01564                   * Empty constructor.
01565                   */
01566                 TObject3D():type(GEOMETRIC_TYPE_UNDEFINED)      {}
01567                 /**
01568                   * Destructor.
01569                   */
01570                 ~TObject3D()    {
01571                         destroy();
01572                 }
01573                 /**
01574                   * Checks whether content is a point.
01575                   */
01576                 inline bool isPoint() const     {
01577                         return type==GEOMETRIC_TYPE_POINT;
01578                 }
01579                 /**
01580                   * Checks whether content is a segment.
01581                   */
01582                 inline bool isSegment() const   {
01583                         return type==GEOMETRIC_TYPE_SEGMENT;
01584                 }
01585                 /**
01586                   * Checks whether content is a line.
01587                   */
01588                 inline bool isLine() const      {
01589                         return type==GEOMETRIC_TYPE_LINE;
01590                 }
01591                 /**
01592                   * Checks whether content is a polygon.
01593                   */
01594                 inline bool isPolygon() const   {
01595                         return type==GEOMETRIC_TYPE_POLYGON;
01596                 }
01597                 /**
01598                   * Checks whether content is a plane.
01599                   */
01600                 inline bool isPlane() const     {
01601                         return type==GEOMETRIC_TYPE_PLANE;
01602                 }
01603                 /**
01604                   * Gets object type.
01605                   */
01606                 inline unsigned char getType() const    {
01607                         return type;
01608                 }
01609                 /**
01610                   * Gets the content as a point, returning false if the type is not adequate.
01611                   */
01612                 inline bool getPoint(TPoint3D &p) const {
01613                         if (isPoint())  {
01614                                 p=*(data.point);
01615                                 return true;
01616                         }       else return false;
01617                 }
01618                 /**
01619                   * Gets the content as a segment, returning false if the type is not adequate.
01620                   */
01621                 inline bool getSegment(TSegment3D &s) const     {
01622                         if (isSegment())        {
01623                                 s=*(data.segment);
01624                                 return true;
01625                         }       else return false;
01626                 }
01627                 /**
01628                   * Gets the content as a line, returning false if the type is not adequate.
01629                   */
01630                 inline bool getLine(TLine3D &r) const   {
01631                         if (isLine())   {
01632                                 r=*(data.line);
01633                                 return true;
01634                         }       else return false;
01635                 }
01636                 /**
01637                   * Gets the content as a polygon, returning false if the type is not adequate.
01638                   */
01639                 inline bool getPolygon(TPolygon3D &p) const     {
01640                         if (isPolygon())        {
01641                                 p=*(data.polygon);
01642                                 return true;
01643                         }       else return false;
01644                 }
01645                 /**
01646                   * Gets the content as a plane, returning false if the type is not adequate.
01647                   */
01648                 inline bool getPlane(TPlane &p) const   {
01649                         if (isPlane())  {
01650                                 p=*(data.plane);
01651                                 return true;
01652                         }       else return false;
01653                 }
01654                 /**
01655                   * Assigns another object, creating a new pointer if needed.
01656                   */
01657                 void operator=(const TObject3D &obj)    {
01658                         if (this==&obj) return;
01659                         destroy();
01660                         switch (type=obj.type)  {
01661                                 case GEOMETRIC_TYPE_POINT:
01662                                         data.point=new TPoint3D(*(obj.data.point));
01663                                         break;
01664                                 case GEOMETRIC_TYPE_SEGMENT:
01665                                         data.segment=new TSegment3D(*(obj.data.segment));
01666                                         break;
01667                                 case GEOMETRIC_TYPE_LINE:
01668                                         data.line=new TLine3D(*(obj.data.line));
01669                                         break;
01670                                 case GEOMETRIC_TYPE_POLYGON:
01671                                         data.polygon=new TPolygon3D(*(obj.data.polygon));
01672                                         break;
01673                                 case GEOMETRIC_TYPE_PLANE:
01674                                         data.plane=new TPlane(*(obj.data.plane));
01675                                         break;
01676                                 case GEOMETRIC_TYPE_UNDEFINED:
01677                                         break;
01678                                 default:
01679                                         THROW_EXCEPTION("Invalid TObject3D object");
01680                         }
01681                 }
01682                 /**
01683                   * Assigns a point to this object.
01684                   */
01685                 inline void operator=(const TPoint3D &p)        {
01686                         destroy();
01687                         type=GEOMETRIC_TYPE_POINT;
01688                         data.point=new TPoint3D(p);
01689                 }
01690                 /**
01691                   * Assigns a segment to this object.
01692                   */
01693                 inline void operator=(const TSegment3D &s)      {
01694                         destroy();
01695                         type=GEOMETRIC_TYPE_SEGMENT;
01696                         data.segment=new TSegment3D(s);
01697                 }
01698                 /**
01699                   * Assigns a line to this object.
01700                   */
01701                 inline void operator=(const TLine3D &l) {
01702                         destroy();
01703                         type=GEOMETRIC_TYPE_LINE;
01704                         data.line=new TLine3D(l);
01705                 }
01706                 /**
01707                   * Assigns a polygon to this object.
01708                   */
01709                 inline void operator=(const TPolygon3D &p)      {
01710                         destroy();
01711                         type=GEOMETRIC_TYPE_POLYGON;
01712                         data.polygon=new TPolygon3D(p);
01713                 }
01714                 /**
01715                   * Assigns a plane to this object.
01716                   */
01717                 inline void operator=(const TPlane &p)  {
01718                         destroy();
01719                         type=GEOMETRIC_TYPE_PLANE;
01720                         data.plane=new TPlane(p);
01721                 }
01722                 /**
01723                   * Projects into 2D space.
01724                   * \throw std::logic_error if the 3D object loses its properties when projecting into 2D space (for example, it's a plane or a vertical line).
01725                   */
01726                 inline void generate2DObject(TObject2D &obj) const      {
01727                         switch (type)   {
01728                                 case GEOMETRIC_TYPE_POINT:
01729                                         obj=TPoint2D(*(data.point));
01730                                         break;
01731                                 case GEOMETRIC_TYPE_SEGMENT:
01732                                         obj=TSegment2D(*(data.segment));
01733                                         break;
01734                                 case GEOMETRIC_TYPE_LINE:
01735                                         obj=TLine2D(*(data.line));
01736                                         break;
01737                                 case GEOMETRIC_TYPE_POLYGON:
01738                                         obj=TPolygon2D(*(data.polygon));
01739                                         break;
01740                                 case GEOMETRIC_TYPE_PLANE:
01741                                         throw std::logic_error("Too many dimensions");
01742                                 default:
01743                                         obj=TObject2D();
01744                                         break;
01745                         }
01746                 }
01747                 /**
01748                   * Constructs from another object.
01749                   */
01750                 TObject3D(const TObject3D &obj):type(GEOMETRIC_TYPE_UNDEFINED)  {
01751                         operator=(obj);
01752                 }
01753                 /**
01754                   * Static method to retrieve every point included in a vector of objects.
01755                   */
01756                 static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts);
01757                 /**
01758                   * Static method to retrieve every segment included in a vector of objects.
01759                   */
01760                 static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms);
01761                 /**
01762                   * Static method to retrieve every line included in a vector of objects.
01763                   */
01764                 static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins);
01765                 /**
01766                   * Static method to retrieve every plane included in a vector of objects.
01767                   */
01768                 static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns);
01769                 /**
01770                   * Static method to retrieve every polygon included in a vector of objects.
01771                   */
01772                 static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys);
01773                 /**
01774                   * Static method to retrieve every point included in a vector of objects, returning the remaining objects in another argument.
01775                   */
01776                 static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts,std::vector<TObject3D> &remainder);
01777                 /**
01778                   * Static method to retrieve every segment included in a vector of objects, returning the remaining objects in another argument.
01779                   */
01780                 static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms,std::vector<TObject3D> &remainder);
01781                 /**
01782                   * Static method to retrieve every line included in a vector of objects, returning the remaining objects in another argument.
01783                   */
01784                 static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins,std::vector<TObject3D> &remainder);
01785                 /**
01786                   * Static method to retrieve every plane included in a vector of objects, returning the remaining objects in another argument.
01787                   */
01788                 static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns,std::vector<TObject3D> &remainder);
01789                 /**
01790                   * Static method to retrieve every polygon included in a vector of objects, returning the remaining objects in another argument.
01791                   */
01792                 static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder);
01793         };
01794 #else
01795         struct BASE_IMPEXP TObject2D    {
01796         private:
01797                 /**
01798                   * Object type identifier.
01799                   */
01800                 unsigned char type;
01801                 /**
01802                   * Union type storing pointers to every allowed type.
01803                   */
01804                 struct  {
01805                         TPoint2D point;
01806                         TSegment2D segment;
01807                         TLine2D line;
01808                         TPolygon2D *polygon;
01809                 }       data;
01810                 /**
01811                   * Destroys the object, releasing the pointer to the content (if any).
01812                   */
01813                 inline void destroy()   {
01814                         if (type==GEOMETRIC_TYPE_POLYGON) delete data.polygon;
01815                         type=GEOMETRIC_TYPE_UNDEFINED;
01816                 }
01817         public:
01818                 /**
01819                   * Implicit constructor from point.
01820                   */
01821                 inline TObject2D(const TPoint2D &p):type(GEOMETRIC_TYPE_POINT)  {
01822                         data.point=p;
01823                 }
01824                 /**
01825                   * Implicit constructor from segment.
01826                   */
01827                 inline TObject2D(const TSegment2D &s):type(GEOMETRIC_TYPE_SEGMENT)      {
01828                         data.segment=s;
01829                 }
01830                 /**
01831                   * Implicit constructor from line.
01832                   */
01833                 inline TObject2D(const TLine2D &r):type(GEOMETRIC_TYPE_LINE)    {
01834                         data.line=r;
01835                 }
01836                 /**
01837                   * Implicit constructor from polygon.
01838                   */
01839                 inline TObject2D(const TPolygon2D &p):type(GEOMETRIC_TYPE_POLYGON)      {
01840                         data.polygon=new TPolygon2D(p);
01841                 }
01842                 /**
01843                   * Implicit constructor from polygon.
01844                   */
01845                 TObject2D():type(GEOMETRIC_TYPE_UNDEFINED)      {}
01846                 /**
01847                   * Object destruction.
01848                   */
01849                 ~TObject2D()    {
01850                         destroy();
01851                 }
01852                 /**
01853                   * Checks whether content is a point.
01854                   */
01855                 inline bool isPoint() const     {
01856                         return type==GEOMETRIC_TYPE_POINT;
01857                 }
01858                 /**
01859                   * Checks whether content is a segment.
01860                   */
01861                 inline bool isSegment() const   {
01862                         return type==GEOMETRIC_TYPE_SEGMENT;
01863                 }
01864                 /**
01865                   * Checks whether content is a line.
01866                   */
01867                 inline bool isLine() const      {
01868                         return type==GEOMETRIC_TYPE_LINE;
01869                 }
01870                 /**
01871                   * Checks whether content is a polygon.
01872                   */
01873                 inline bool isPolygon() const   {
01874                         return type==GEOMETRIC_TYPE_POLYGON;
01875                 }
01876                 /**
01877                   * Gets content type.
01878                   */
01879                 inline unsigned char getType() const    {
01880                         return type;
01881                 }
01882                 /**
01883                   * Gets the content as a point, returning false if the type is inadequate.
01884                   */
01885                 inline bool getPoint(TPoint2D &p) const {
01886                         if (isPoint())  {
01887                                 p=data.point;
01888                                 return true;
01889                         }       else return false;
01890                 }
01891                 /**
01892                   * Gets the content as a segment, returning false if the type is inadequate.
01893                   */
01894                 inline bool getSegment(TSegment2D &s) const     {
01895                         if (isSegment())        {
01896                                 s=data.segment;
01897                                 return true;
01898                         }       else return false;
01899                 }
01900                 /**
01901                   * Gets the content as a line, returning false if the type is inadequate.
01902                   */
01903                 inline bool getLine(TLine2D &r) const   {
01904                         if (isLine())   {
01905                                 r=data.line;
01906                                 return true;
01907                         }       else return false;
01908                 }
01909                 /**
01910                   * Gets the content as a polygon, returning false if the type is inadequate.
01911                   */
01912                 inline bool getPolygon(TPolygon2D &p) const     {
01913                         if (isPolygon())        {
01914                                 p=*(data.polygon);
01915                                 return true;
01916                         }       else return false;
01917                 }
01918                 /**
01919                   * Assign another TObject2D. Pointers are not shared.
01920                   */
01921                 void operator=(const TObject2D &obj)    {
01922                         if (this==&obj) return;
01923                         destroy();
01924                         switch (type=obj.type)  {
01925                                 case GEOMETRIC_TYPE_POINT:
01926                                         data.point=obj.data.point;
01927                                         break;
01928                                 case GEOMETRIC_TYPE_SEGMENT:
01929                                         data.segment=obj.data.segment;
01930                                         break;
01931                                 case GEOMETRIC_TYPE_LINE:
01932                                         data.line=obj.data.line;
01933                                         break;
01934                                 case GEOMETRIC_TYPE_POLYGON:
01935                                         data.polygon=new TPolygon2D(*(obj.data.polygon));
01936                                         break;
01937                         }
01938                 }
01939                 /**
01940                   * Assign a point to this object.
01941                   */
01942                 inline void operator=(const TPoint2D &p)        {
01943                         destroy();
01944                         type=GEOMETRIC_TYPE_POINT;
01945                         data.point=p;
01946                 }
01947                 /**
01948                   * Assign a segment to this object.
01949                   */
01950                 inline void operator=(const TSegment2D &s)      {
01951                         destroy();
01952                         type=GEOMETRIC_TYPE_SEGMENT;
01953                         data.segment=s;
01954                 }
01955                 /**
01956                   * Assign a line to this object.
01957                   */
01958                 inline void operator=(const TLine2D &l) {
01959                         destroy();
01960                         type=GEOMETRIC_TYPE_LINE;
01961                         data.line=l;
01962                 }
01963                 /**
01964                   * Assign a polygon to this object.
01965                   */
01966                 inline void operator=(const TPolygon2D &p)      {
01967                         destroy();
01968                         type=GEOMETRIC_TYPE_POLYGON;
01969                         data.polygon=new TPolygon2D(p);
01970                 }
01971                 /**
01972                   * Project into 3D space.
01973                   */
01974                 void generate3DObject(TObject3D &obj) const;
01975                 /**
01976                   * Constructor from another TObject2D.
01977                   */
01978                 TObject2D(const TObject2D &obj):type(GEOMETRIC_TYPE_UNDEFINED)  {
01979                         operator=(obj);
01980                 }
01981                 /**
01982                   * Static method to retrieve all the points in a vector of TObject2D.
01983                   */
01984                 static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts);
01985                 /**
01986                   * Static method to retrieve all the segments in a vector of TObject2D.
01987                   */
01988                 static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms);
01989                 /**
01990                   * Static method to retrieve all the lines in a vector of TObject2D.
01991                   */
01992                 static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins);
01993                 /**
01994                   * Static method to retrieve all the polygons in a vector of TObject2D.
01995                   */
01996                 static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys);
01997                 /**
01998                   * Static method to retrieve all the points in a vector of TObject2D, returning the remainder objects in another parameter.
01999                   */
02000                 static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts,std::vector<TObject2D> &remainder);
02001                 /**
02002                   * Static method to retrieve all the segments in a vector of TObject2D, returning the remainder objects in another parameter.
02003                   */
02004                 static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms,std::vector<TObject2D> &remainder);
02005                 /**
02006                   * Static method to retrieve all the lines in a vector of TObject2D, returning the remainder objects in another parameter.
02007                   */
02008                 static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins,std::vector<TObject2D> &remainder);
02009                 /**
02010                   * Static method to retrieve all the polygons in a vector of TObject2D, returning the remainder objects in another parameter.
02011                   */
02012                 static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys,std::vector<TObject2D> &remainder);
02013         };
02014         /**
02015           * Standard object for storing any 3D lightweight object. Do not inherit from this class.
02016           * \sa TPoint3D,TSegment3D,TLine3D,TPlane,TPolygon3D
02017           */
02018         struct BASE_IMPEXP TObject3D    {
02019         private:
02020                 /**
02021                   * Object type identifier.
02022                   */
02023                 unsigned char type;
02024                 /**
02025                   * Union containing pointer to actual data.
02026                   */
02027                 struct  {
02028                         TPoint3D point;
02029                         TSegment3D segment;
02030                         TLine3D line;
02031                         TPolygon3D *polygon;
02032                         TPlane plane;
02033                 }       data;
02034                 /**
02035                   * Destroys the object and releases the pointer, if any.
02036                   */
02037                 void destroy()  {
02038                         if (type==GEOMETRIC_TYPE_POLYGON) delete data.polygon;
02039                         type=GEOMETRIC_TYPE_UNDEFINED;
02040                 }
02041         public:
02042                 /**
02043                   * Constructor from point.
02044                   */
02045                 TObject3D(const TPoint3D &p):type(GEOMETRIC_TYPE_POINT) {
02046                         data.point=p;
02047                 }
02048                 /**
02049                   * Constructor from segment.
02050                   */
02051                 TObject3D(const TSegment3D &s):type(GEOMETRIC_TYPE_SEGMENT)     {
02052                         data.segment=s;
02053                 }
02054                 /**
02055                   * Constructor from line.
02056                   */
02057                 TObject3D(const TLine3D &r):type(GEOMETRIC_TYPE_LINE)   {
02058                         data.line=r;
02059                 }
02060                 /**
02061                   * Constructor from polygon.
02062                   */
02063                 TObject3D(const TPolygon3D &p):type(GEOMETRIC_TYPE_POLYGON)     {
02064                         data.polygon=new TPolygon3D(p);
02065                 }
02066                 /**
02067                   * Constructor from plane.
02068                   */
02069                 TObject3D(const TPlane &p):type(GEOMETRIC_TYPE_PLANE)   {
02070                         data.plane=p;
02071                 }
02072                 /**
02073                   * Empty constructor.
02074                   */
02075                 TObject3D():type(GEOMETRIC_TYPE_UNDEFINED)      {}
02076                 /**
02077                   * Destructor.
02078                   */
02079                 ~TObject3D()    {
02080                         destroy();
02081                 }
02082                 /**
02083                   * Checks whether content is a point.
02084                   */
02085                 inline bool isPoint() const     {
02086                         return type==GEOMETRIC_TYPE_POINT;
02087                 }
02088                 /**
02089                   * Checks whether content is a segment.
02090                   */
02091                 inline bool isSegment() const   {
02092                         return type==GEOMETRIC_TYPE_SEGMENT;
02093                 }
02094                 /**
02095                   * Checks whether content is a line.
02096                   */
02097                 inline bool isLine() const      {
02098                         return type==GEOMETRIC_TYPE_LINE;
02099                 }
02100                 /**
02101                   * Checks whether content is a polygon.
02102                   */
02103                 inline bool isPolygon() const   {
02104                         return type==GEOMETRIC_TYPE_POLYGON;
02105                 }
02106                 /**
02107                   * Checks whether content is a plane.
02108                   */
02109                 inline bool isPlane() const     {
02110                         return type==GEOMETRIC_TYPE_PLANE;
02111                 }
02112                 /**
02113                   * Gets object type.
02114                   */
02115                 inline unsigned char getType() const    {
02116                         return type;
02117                 }
02118                 /**
02119                   * Gets the content as a point, returning false if the type is not adequate.
02120                   */
02121                 inline bool getPoint(TPoint3D &p) const {
02122                         if (isPoint())  {
02123                                 p=data.point;
02124                                 return true;
02125                         }       else return false;
02126                 }
02127                 /**
02128                   * Gets the content as a segment, returning false if the type is not adequate.
02129                   */
02130                 inline bool getSegment(TSegment3D &s) const     {
02131                         if (isSegment())        {
02132                                 s=data.segment;
02133                                 return true;
02134                         }       else return false;
02135                 }
02136                 /**
02137                   * Gets the content as a line, returning false if the type is not adequate.
02138                   */
02139                 inline bool getLine(TLine3D &r) const   {
02140                         if (isLine())   {
02141                                 r=data.line;
02142                                 return true;
02143                         }       else return false;
02144                 }
02145                 /**
02146                   * Gets the content as a polygon, returning false if the type is not adequate.
02147                   */
02148                 inline bool getPolygon(TPolygon3D &p) const     {
02149                         if (isPolygon())        {
02150                                 p=*(data.polygon);
02151                                 return true;
02152                         }       else return false;
02153                 }
02154                 /**
02155                   * Gets the content as a plane, returning false if the type is not adequate.
02156                   */
02157                 inline bool getPlane(TPlane &p) const   {
02158                         if (isPlane())  {
02159                                 p=data.plane;
02160                                 return true;
02161                         }       else return false;
02162                 }
02163                 /**
02164                   * Assigns another object, creating a new pointer if needed.
02165                   */
02166                 void operator=(const TObject3D &obj)    {
02167                         if (this==&obj) return;
02168                         destroy();
02169                         switch (type=obj.type)  {
02170                                 case GEOMETRIC_TYPE_POINT:
02171                                         data.point=obj.data.point;
02172                                         break;
02173                                 case GEOMETRIC_TYPE_SEGMENT:
02174                                         data.segment=obj.data.segment;
02175                                         break;
02176                                 case GEOMETRIC_TYPE_LINE:
02177                                         data.line=obj.data.line;
02178                                         break;
02179                                 case GEOMETRIC_TYPE_POLYGON:
02180                                         data.polygon=new TPolygon3D(*(obj.data.polygon));
02181                                         break;
02182                                 case GEOMETRIC_TYPE_PLANE:
02183                                         data.plane=obj.data.plane;
02184                                         break;
02185                                 case GEOMETRIC_TYPE_UNDEFINED:
02186                                         break;
02187                                 default:
02188                                         THROW_EXCEPTION("Invalid TObject3D object");
02189                         }
02190                 }
02191                 /**
02192                   * Assigns a point to this object.
02193                   */
02194                 inline void operator=(const TPoint3D &p)        {
02195                         destroy();
02196                         type=GEOMETRIC_TYPE_POINT;
02197                         data.point=p;
02198                 }
02199                 /**
02200                   * Assigns a segment to this object.
02201                   */
02202                 inline void operator=(const TSegment3D &s)      {
02203                         destroy();
02204                         type=GEOMETRIC_TYPE_SEGMENT;
02205                         data.segment=s;
02206                 }
02207                 /**
02208                   * Assigns a line to this object.
02209                   */
02210                 inline void operator=(const TLine3D &l) {
02211                         destroy();
02212                         type=GEOMETRIC_TYPE_LINE;
02213                         data.line=l;
02214                 }
02215                 /**
02216                   * Assigns a polygon to this object.
02217                   */
02218                 inline void operator=(const TPolygon3D &p)      {
02219                         destroy();
02220                         type=GEOMETRIC_TYPE_POLYGON;
02221                         data.polygon=new TPolygon3D(p);
02222                 }
02223                 /**
02224                   * Assigns a plane to this object.
02225                   */
02226                 inline void operator=(const TPlane &p)  {
02227                         destroy();
02228                         type=GEOMETRIC_TYPE_PLANE;
02229                         data.plane=p;
02230                 }
02231                 /**
02232                   * Projects into 2D space.
02233                   * \throw std::logic_error if the 3D object loses its properties when projecting into 2D space (for example, it's a plane or a vertical line).
02234                   */
02235                 inline void generate2DObject(TObject2D &obj) const      {
02236                         switch (type)   {
02237                                 case GEOMETRIC_TYPE_POINT:
02238                                         obj=TPoint2D(data.point);
02239                                         break;
02240                                 case GEOMETRIC_TYPE_SEGMENT:
02241                                         obj=TSegment2D(data.segment);
02242                                         break;
02243                                 case GEOMETRIC_TYPE_LINE:
02244                                         obj=TLine2D(data.line);
02245                                         break;
02246                                 case GEOMETRIC_TYPE_POLYGON:
02247                                         obj=TPolygon2D(*(data.polygon));
02248                                         break;
02249                                 case GEOMETRIC_TYPE_PLANE:
02250                                         throw std::logic_error("Too many dimensions");
02251                                 default:
02252                                         obj=TObject2D();
02253                                         break;
02254                         }
02255                 }
02256                 /**
02257                   * Constructs from another object.
02258                   */
02259                 TObject3D(const TObject3D &obj):type(GEOMETRIC_TYPE_UNDEFINED)  {
02260                         operator=(obj);
02261                 }
02262                 /**
02263                   * Static method to retrieve every point included in a vector of objects.
02264                   */
02265                 static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts);
02266                 /**
02267                   * Static method to retrieve every segment included in a vector of objects.
02268                   */
02269                 static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms);
02270                 /**
02271                   * Static method to retrieve every line included in a vector of objects.
02272                   */
02273                 static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins);
02274                 /**
02275                   * Static method to retrieve every plane included in a vector of objects.
02276                   */
02277                 static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns);
02278                 /**
02279                   * Static method to retrieve every polygon included in a vector of objects.
02280                   */
02281                 static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys);
02282                 /**
02283                   * Static method to retrieve every point included in a vector of objects, returning the remaining objects in another argument.
02284                   */
02285                 static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts,std::vector<TObject3D> &remainder);
02286                 /**
02287                   * Static method to retrieve every segment included in a vector of objects, returning the remaining objects in another argument.
02288                   */
02289                 static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms,std::vector<TObject3D> &remainder);
02290                 /**
02291                   * Static method to retrieve every line included in a vector of objects, returning the remaining objects in another argument.
02292                   */
02293                 static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins,std::vector<TObject3D> &remainder);
02294                 /**
02295                   * Static method to retrieve every plane included in a vector of objects, returning the remaining objects in another argument.
02296                   */
02297                 static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns,std::vector<TObject3D> &remainder);
02298                 /**
02299                   * Static method to retrieve every polygon included in a vector of objects, returning the remaining objects in another argument.
02300                   */
02301                 static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder);
02302         };
02303 
02304 #endif
02305 
02306 
02307         //Streaming functions
02308         /**
02309           * TPoint2D binary input.
02310           */
02311         BASE_IMPEXP mrpt::utils::CStream& operator>>(mrpt::utils::CStream& in,mrpt::math::TPoint2D &o);
02312         /**
02313           * TPoint2D binary output.
02314           */
02315         BASE_IMPEXP mrpt::utils::CStream& operator<<(mrpt::utils::CStream& out,const mrpt::math::TPoint2D &o);
02316 
02317         /**
02318           * TPoint3D binary input.
02319           */
02320         BASE_IMPEXP mrpt::utils::CStream& operator>>(mrpt::utils::CStream& in,mrpt::math::TPoint3D &o);
02321         /**
02322           * TPoint3D binary output.
02323           */
02324         BASE_IMPEXP mrpt::utils::CStream& operator<<(mrpt::utils::CStream& out,const mrpt::math::TPoint3D &o);
02325 
02326         /**
02327           * TPose2D binary input.
02328           */
02329         BASE_IMPEXP mrpt::utils::CStream& operator>>(mrpt::utils::CStream& in,mrpt::math::TPose2D &o);
02330         /**
02331           * TPose2D binary output.
02332           */
02333         BASE_IMPEXP mrpt::utils::CStream& operator<<(mrpt::utils::CStream& out,const mrpt::math::TPose2D &o);
02334 
02335         /**
02336           * TPose3D binary input.
02337           */
02338         BASE_IMPEXP mrpt::utils::CStream& operator>>(mrpt::utils::CStream& in,mrpt::math::TPose3D &o);
02339         /**
02340           * TPose3D binary output.
02341           */
02342         BASE_IMPEXP mrpt::utils::CStream& operator<<(mrpt::utils::CStream& out,const mrpt::math::TPose3D &o);
02343 
02344         /**
02345           * TSegment2D binary input.
02346           */
02347         inline mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TSegment2D &s)     {
02348                 return in>>s.point1>>s.point2;
02349         }
02350         /**
02351           * TSegment2D binary output.
02352           */
02353         inline mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TSegment2D &s)      {
02354                 return out<<s.point1<<s.point2;
02355         }
02356 
02357         /**
02358           * TLine2D binary input.
02359           */
02360         inline mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TLine2D &l)        {
02361                 return in>>l.coefs[0]>>l.coefs[1]>>l.coefs[2];
02362         }
02363         /**
02364           * TLine2D binary output.
02365           */
02366         inline mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TLine2D &l) {
02367                 return out<<l.coefs[0]<<l.coefs[1]<<l.coefs[2];
02368         }
02369 
02370         /**
02371           * TObject2D binary input.
02372           */
02373         BASE_IMPEXP mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TObject2D &o);
02374         /**
02375           * TObject2D binary input.
02376           */
02377         BASE_IMPEXP mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TObject2D &o);
02378 
02379         /**
02380           * TSegment3D binary input.
02381           */
02382         inline mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TSegment3D &s)     {
02383                 return in>>s.point1>>s.point2;
02384         }
02385         /**
02386           * TSegment3D binary output.
02387           */
02388         inline mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TSegment3D &s)      {
02389                 return out<<s.point1<<s.point2;
02390         }
02391 
02392         /**
02393           * TLine3D binary input.
02394           */
02395         inline mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TLine3D &l)        {
02396                 return in>>l.pBase>>l.director[0]>>l.director[1]>>l.director[2];
02397         }
02398         /**
02399           * TLine3D binary output.
02400           */
02401         inline mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TLine3D &l) {
02402                 return out<<l.pBase<<l.director[0]<<l.director[1]<<l.director[2];
02403         }
02404 
02405         /**
02406           * TPlane binary input.
02407           */
02408         inline mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TPlane &p) {
02409                 return in>>p.coefs[0]>>p.coefs[1]>>p.coefs[2]>>p.coefs[3];
02410         }
02411         /**
02412           * TPlane binary output.
02413           */
02414         inline mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TPlane &p)  {
02415                 return out<<p.coefs[0]<<p.coefs[1]<<p.coefs[2]<<p.coefs[3];
02416         }
02417 
02418         /**
02419           * TObject3D binary input.
02420           */
02421         BASE_IMPEXP mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TObject3D &o);
02422         /**
02423           * TObject3D binary output.
02424           */
02425         BASE_IMPEXP mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TObject3D &o);
02426 
02427         }       //end of namespace math
02428 
02429         namespace utils
02430         {
02431                 using namespace ::mrpt::math;
02432 
02433                 // Specialization must occur in the same namespace
02434                 MRPT_DECLARE_TTYPENAME(TPoint2D)
02435                 MRPT_DECLARE_TTYPENAME(TPoint3D)
02436                 MRPT_DECLARE_TTYPENAME(TPose2D)
02437                 MRPT_DECLARE_TTYPENAME(TPose3D)
02438                 MRPT_DECLARE_TTYPENAME(TSegment2D)
02439                 MRPT_DECLARE_TTYPENAME(TLine2D)
02440                 MRPT_DECLARE_TTYPENAME(TPolygon2D)
02441                 MRPT_DECLARE_TTYPENAME(TObject2D)
02442                 MRPT_DECLARE_TTYPENAME(TSegment3D)
02443                 MRPT_DECLARE_TTYPENAME(TLine3D)
02444                 MRPT_DECLARE_TTYPENAME(TPlane)
02445                 MRPT_DECLARE_TTYPENAME(TPolygon3D)
02446                 MRPT_DECLARE_TTYPENAME(TObject3D)
02447 
02448         } // end of namespace utils
02449 
02450 }       //end of namespace
02451 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011