00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef CPOSEORPOINT_H
00029 #define CPOSEORPOINT_H
00030
00031 #include <mrpt/utils/CSerializable.h>
00032 #include <mrpt/math/CMatrixD.h>
00033
00034 namespace mrpt
00035 {
00038 namespace poses
00039 {
00040 class CPoint2D;
00041 class CPoint3D;
00042 class CPose2D;
00043 class CPose3D;
00044
00045
00046 DEFINE_SERIALIZABLE_PRE( CPoseOrPoint )
00047
00048
00560 class MRPTDLLIMPEXP CPoseOrPoint : public mrpt::utils::CSerializable
00561 {
00562
00563 DEFINE_VIRTUAL_SERIALIZABLE( CPoseOrPoint )
00564
00565 protected:
00569 bool is3D;
00570
00571 public:
00572 CPoseOrPoint() : is3D(), x(),y(),z()
00573 { }
00574
00577 double x,y,z;
00578
00581 double distanceTo(const CPoseOrPoint &b) const
00582 {
00583 if (is3D || b.is3D)
00584 return sqrt((x-b.x)*(x-b.x)+(y-b.y)*(y-b.y)+(z-b.z)*(z-b.z));
00585 else return sqrt((x-b.x)*(x-b.x)+(y-b.y)*(y-b.y));
00586 }
00587
00590 double sqrDistanceTo(const CPoseOrPoint &b) const
00591 {
00592 if (is3D || b.is3D)
00593 return (x-b.x)*(x-b.x)+(y-b.y)*(y-b.y)+(z-b.z)*(z-b.z);
00594 else return (x-b.x)*(x-b.x)+(y-b.y)*(y-b.y);
00595 }
00596
00597
00600 double norm() const;
00601
00604 virtual void operator *=(const double &s) = 0;
00605
00611 virtual const math::CMatrixDouble& getHomogeneousMatrix() const =0;
00612
00618 virtual void getHomogeneousMatrix(const math::CMatrixDouble *& ptrHM ) const =0;
00619
00625 void getInverseHomogeneousMatrix( math::CMatrixDouble &out_HM ) const;
00626
00630 double distance2DTo( double ax, double ay ) const { return sqrt( (ax-x)*(ax-x)+(ay-y)*(ay-y) ); }
00631
00635 double distance3DTo( double ax, double ay, double az ) const { return sqrt( (ax-x)*(ax-x)+(ay-y)*(ay-y)+(az-z)*(az-z) ); }
00636
00640 double distance2DToSquare( double ax, double ay ) const { return (ax-x)*(ax-x)+(ay-y)*(ay-y); }
00641
00645 double distance3DToSquare( double ax, double ay, double az ) const { return (ax-x)*(ax-x)+(ay-y)*(ay-y)+(az-z)*(az-z); }
00646
00647 };
00648
00649
00650 }
00651 }
00652
00653 #endif