Position3D.hpp
Go to the documentation of this file.
00001 //
00002 // Position3D.hpp
00003 //
00004 
00005 #ifndef POSITION3D_HPP
00006 #define POSITION3D_HPP
00007 
00008 #include "../BasicDatatypes.hpp"
00009 #include "Point3D.hpp"
00010 #include "../tools/errorhandler.hpp"
00011 
00012 namespace datatypes
00013 {
00014         
00015 class Point2D;
00016 //class Point3D;
00017 
00018 class Matrix;
00019 
00020         
00021 //
00022 // ***********************************************
00023 //
00024 class Vector
00025 {
00026 public:
00027         Vector(UINT16 numOfElements);
00028         ~Vector();
00029         
00030         inline const double operator[](const UINT16 elementNr) const;
00031         inline double& operator[](const UINT16 elementNr);
00032         inline Vector& operator=(const Vector& in);
00033 //      inline const Vector operator*(Matrix& matrix);
00034         
00035 private:
00036         UINT16 m_numOfElements;
00037         double* m_elements;
00038 };
00039 
00040 // Keine Pruefung des Index wg. Laufzeit
00041 inline Vector& Vector::operator=(const Vector& in)
00042 {
00043         if (in.m_numOfElements != m_numOfElements)
00044         {
00045                 dieWithError("Vector::operator=: Vector size does not match!");
00046                 return (*this);
00047         }
00048         
00049         for (UINT16 i = 0; i<m_numOfElements; i++)
00050         {
00051                 m_elements[i] = in[i];
00052         }
00053         return (*this);
00054 }
00055 
00056 // Keine Pruefung des Index wg. Laufzeit
00057 inline const double Vector::operator[](const UINT16 elementNr) const
00058 {
00059         return (m_elements[elementNr]);
00060 }
00061 
00062 // Fuer v[p] = x
00063 inline double& Vector::operator[](const UINT16 elementNr)
00064 {
00065         return (m_elements[elementNr]);
00066 }
00067 
00068 class Matrix
00069 {
00070 public:
00071         Matrix(UINT16 numOfRows, UINT16 numOfColumns);
00072         ~Matrix();
00073         
00074         inline const double operator()(const UINT16 rowNr, const UINT16 columnNr) const;
00075         inline double& operator()(const UINT16 rowNr, const UINT16 columnNr);
00076         inline const Vector operator*(Vector& vector);
00077 
00078 private:
00079         UINT16 m_numOfRows;
00080         UINT16 m_numOfColumns;
00081         double** m_elements;
00082 };
00083 
00084 // Fuer x = m(r,c);
00085 // Keine Pruefung des Index wg. Laufzeit
00086 inline const double Matrix::operator()(const UINT16 rowNr, const UINT16 columnNr) const
00087 {
00088         return (m_elements[rowNr][columnNr]);
00089 }
00090 
00091 // Fuer m(r,c) = x;
00092 // Keine Pruefung des Index wg. Laufzeit
00093 inline double& Matrix::operator()(const UINT16 rowNr, const UINT16 columnNr)
00094 {
00095         return (m_elements[rowNr][columnNr]);
00096 }
00097 
00098 // Fuer v2 = m * v1;
00099 // Vektor-Laenge und Spaltenanzahl der Matrix muessen gleich sein!
00100 inline const Vector Matrix::operator*(Vector& vector)
00101 {
00102         UINT16 row, col;
00103         double sum;
00104         Vector result(m_numOfRows);
00105         
00106         for (row = 0; row < m_numOfRows; row++)
00107         {
00108                 sum = 0.0;
00109                 for (col=0; col < m_numOfColumns; col++)
00110                 {
00111                         sum += m_elements[row][col] * vector[col];
00112                 }
00113                 result[row] = sum;
00114         }
00115         
00116         return result;
00117 }
00118 
00119 
00120 // Fuer v2 = v1 * m;
00121 /*
00122 inline const Vector Vector::operator*(Matrix& matrix)
00123 {
00124         UINT16 row, col;
00125         double sum;
00126         Vector result(m_numOfElements);
00127         
00128         for (row = 0; row < m_numOfElements; row++)
00129         {
00130                 sum = 0.0;
00131                 for (col=0; col < m_numOfElements; col++)
00132                 {
00133                         sum += matrix(row, col) * m_elements[col];
00134                 }
00135                 result[row] = sum;
00136         }
00137         
00138         return result;
00139 }
00140 */
00141 
00142 //
00143 // *************************************************
00144 //
00145 
00147 
00149 class Position3D : public BasicData
00150 {
00151         typedef double value_type;
00152         
00153 public:
00155 
00158         Position3D();
00159 
00161 
00166         Position3D(value_type yaw, value_type pitch, value_type roll,
00167                                          value_type x, value_type y, value_type z);
00168 
00170 
00179         Position3D(value_type yaw, value_type pitch, value_type roll,
00180                                          const Point3D& point);
00181 
00182         // Estimate the memory usage of this object
00183         inline virtual const UINT32 getUsedMemory() const {return sizeof(*this);};
00184         
00185         // Die eigene Position3D wird als Laserscanner-Mountingposition interpretiert
00186         bool transformToVehicle(Point3D* pt);   // double *dXPos, double *dYPos, double *dZPos, const CRect& r)
00187         
00188         
00190         bool operator==(const Position3D& other) const;
00191 
00193         value_type getYawAngle() const { return m_yawAngle; }
00195         value_type getPitchAngle() const { return m_pitchAngle; }
00197         value_type getRollAngle() const { return m_rollAngle; }
00198 
00200         value_type getX() const { return m_point.getX(); }
00202         value_type getY() const { return m_point.getY(); }
00204         value_type getZ() const { return m_point.getZ(); }
00205 
00207         Position3D toPosition3D() const;
00208 
00210         Point3D toPoint3D() const;
00211 
00213         Point2D toPoint2D() const;
00214 
00216         void setX(value_type x) { m_point.setX(x); }
00218         void setY(value_type y) { m_point.setY(y); }
00220         void setZ(value_type z) { m_point.setZ(z); }
00222         void setYawAngle(value_type angle) { m_yawAngle = angle; }
00224         void setPitchAngle(value_type angle) { m_pitchAngle = angle; }
00226         void setRollAngle(value_type angle) { m_rollAngle = angle; }
00227 
00229 
00237         Position3D& set(value_type yaw, value_type pitch, value_type roll,
00238                                                   value_type x, value_type y, value_type z);
00239 
00241         void normalizeAngles();
00242 
00243         // For debugging output: Conversion to string.
00244         std::string toString() const;
00245 
00246 private:
00247         // Orientierung im 3D-Raum
00249         value_type m_yawAngle;
00251         value_type m_pitchAngle;
00253         value_type m_rollAngle;
00254 
00255         Point3D m_point;        // Koordinaten im 3D-Raum
00256 };
00257 
00258 }       // namespace datatypes
00259 
00260 
00261 #endif // POSITION3D_HPP


libsick_ldmrs
Author(s): SICK AG , Martin G√ľnther , Jochen Sprickerhof
autogenerated on Wed Jun 14 2017 04:04:50