Go to the documentation of this file.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 CPOINT_H
00029 #define CPOINT_H
00030
00031 #include <mrpt/poses/CPoseOrPoint.h>
00032
00033 namespace mrpt
00034 {
00035 namespace poses
00036 {
00037
00038
00039
00040
00041
00042 template <class DERIVEDCLASS>
00043 class CPoint : public CPoseOrPoint<DERIVEDCLASS>
00044 {
00045 public:
00046
00047
00048
00049
00050
00051
00052 template <class OTHERCLASS>
00053 inline void AddComponents(const OTHERCLASS &b)
00054 {
00055 const int dims = std::min( size_t(DERIVEDCLASS::static_size), size_t(OTHERCLASS::is3DPoseOrPoint() ? 3:2));
00056 for (int i=0;i<dims;i++)
00057 static_cast<DERIVEDCLASS*>(this)->m_coords[i]+= static_cast<const OTHERCLASS*>(&b)->m_coords[i];
00058 }
00059
00060
00061 inline void operator *=(const double s)
00062 {
00063 for (int i=0;i<DERIVEDCLASS::static_size;i++)
00064 static_cast<DERIVEDCLASS*>(this)->m_coords[i] *= s;
00065 }
00066
00067
00068 inline void getAsVector(vector_double &v) const
00069 {
00070 v.resize(DERIVEDCLASS::static_size);
00071 for (int i=0;i<DERIVEDCLASS::static_size;i++)
00072 v[i] = static_cast<const DERIVEDCLASS*>(this)->m_coords[i];
00073 }
00074
00075 inline vector_double getAsVector() const { vector_double v; getAsVector(v); return v; }
00076
00077
00078
00079
00080 void getHomogeneousMatrix(CMatrixDouble44 & out_HM ) const
00081 {
00082 out_HM.unit(4,1.0);
00083 out_HM.get_unsafe(0,3)= static_cast<const DERIVEDCLASS*>(this)->x();
00084 out_HM.get_unsafe(1,3)= static_cast<const DERIVEDCLASS*>(this)->y();
00085 if (DERIVEDCLASS::is3DPoseOrPoint())
00086 out_HM.get_unsafe(2,3)= static_cast<const DERIVEDCLASS*>(this)->m_coords[2];
00087 }
00088
00089
00090
00091
00092 void asString(std::string &s) const
00093 {
00094 s = (DERIVEDCLASS::is3DPoseOrPoint()) ?
00095 mrpt::format("[%f %f]", static_cast<const DERIVEDCLASS*>(this)->x(), static_cast<const DERIVEDCLASS*>(this)->y()) :
00096 mrpt::format("[%f %f %f]",static_cast<const DERIVEDCLASS*>(this)->x(), static_cast<const DERIVEDCLASS*>(this)->y(), static_cast<const DERIVEDCLASS*>(this)->m_coords[2]);
00097 }
00098 inline std::string asString() const { std::string s; asString(s); return s; }
00099
00100
00101
00102
00103
00104 void fromString(const std::string &s)
00105 {
00106 CMatrixDouble m;
00107 if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
00108 ASSERT_EQUAL_(mrpt::math::size(m,1),1)
00109 ASSERT_EQUAL_(mrpt::math::size(m,2),DERIVEDCLASS::static_size)
00110 for (int i=0;i<DERIVEDCLASS::static_size;i++)
00111 static_cast<DERIVEDCLASS*>(this)->m_coords[i] = m.get_unsafe(0,i);
00112 }
00113
00114 inline const double &operator[](unsigned int i) const { return static_cast<const DERIVEDCLASS*>(this)->m_coords[i]; }
00115 inline double &operator[](unsigned int i) { return static_cast<DERIVEDCLASS*>(this)->m_coords[i]; }
00116
00117
00118
00119 };
00120
00121
00122 template <class DERIVEDCLASS>
00123 std::ostream &operator << (std::ostream& o, const CPoint<DERIVEDCLASS>& p)
00124 {
00125 o << "(" << p[0] << "," << p[1];
00126 if (p.is3DPoseOrPoint()) o << "," << p[2];
00127 o <<")";
00128 return o;
00129 }
00130
00131
00132 template <class DERIVEDCLASS>
00133 bool operator < (const CPoint<DERIVEDCLASS> &a, const CPoint<DERIVEDCLASS> &b)
00134 {
00135 if (a.x()<b.x()) return true;
00136 else
00137 {
00138 if (!a.is3DPoseOrPoint())
00139 return a.y()<b.y();
00140 else if (a.y()<b.y())
00141 return true;
00142 else return a[2]<b[2];
00143 }
00144 }
00145
00146 template <class DERIVEDCLASS>
00147 bool operator==(const CPoint<DERIVEDCLASS> &p1,const CPoint<DERIVEDCLASS> &p2)
00148 {
00149 for (int i=0;i<DERIVEDCLASS::static_size;i++)
00150 if (p1[i]!=p2[i]) return false;
00151 return true;
00152 }
00153
00154 template <class DERIVEDCLASS>
00155 bool operator!=(const CPoint<DERIVEDCLASS> &p1,const CPoint<DERIVEDCLASS> &p2)
00156 {
00157 for (int i=0;i<DERIVEDCLASS::static_size;i++)
00158 if (p1[i]!=p2[i]) return true;
00159 return false;
00160 }
00161
00162
00163
00164 }
00165 }
00166
00167 #endif