pose2.h

Go to the documentation of this file.

 
#ifndef POSE2_H
#define POSE2_H
 
#include <argos3/core/utility/math/vector3.h>
#include <argos3/core/utility/math/quaternion.h>
namespace argos
{
   class CPose2 {
   
   public:
      ~CPose2() {
      }
 
      CPose2() : 
         m_cPosition(0,0),
         m_cOrientation(0) {
      }
 
      CPose2(const CPose2& c_pose) : 
         m_cPosition(c_pose.m_cPosition),
         m_cOrientation(c_pose.m_cOrientation) {
      }
 
      CPose2(const CVector2& c_position, const CRadians& c_orientation) : 
         m_cPosition(c_position),
         m_cOrientation(c_orientation) {
      }
 
      CPose2(const Real f_x, const Real f_y, const Real f_th) : 
         m_cPosition(f_x, f_y),
         m_cOrientation(f_th) {
      }
 
      inline const CVector2& GetPosition() const
      { return m_cPosition;}
 
      inline const CRadians& GetOrientation() const
      { return m_cOrientation;}
 
      inline void Rotate(const CRadians& c_offset) {
         m_cOrientation += c_offset;
      }
 
      inline void Translate(const CVector2& c_offset) {
         m_cPosition = (*this)*c_offset;
      }
 
      inline bool operator==(const CPose2& c_pose) const {
         if(m_cPosition.m_fX != c_pose.m_cPosition.m_fX)
            return false;
         if(m_cPosition.m_fY != c_pose.m_cPosition.m_fY)
            return false;
         if(m_cOrientation != c_pose.m_cOrientation)
            return false;
         return true;
      }
 
      inline bool operator!=(const CPose2& c_pose) const {
         if(m_cPosition.m_fX == c_pose.m_cPosition.m_fX)
            return false;
         if(m_cPosition.m_fY == c_pose.m_cPosition.m_fY)
            return false;
         if(m_cOrientation == c_pose.m_cOrientation)
            return false;
         return true;
      }
 
      inline friend std::ostream& operator<<(std::ostream& c_os,
                                             const CPose2& c_pose) {
         c_os << "Position("
              << c_pose.m_cPosition.GetX() << ","
              << c_pose.m_cPosition.GetY() << ")\n"
              << "Orientation("
              << c_pose.m_cOrientation.GetValue()
              << " radians"
              << " -> "
              << c_pose.m_cOrientation.GetValue() * CRadians::RADIANS_TO_DEGREES
              << " degrees)";
         return c_os;
      }
 
   public:
      CPose2(const CVector3& c_position, const CQuaternion& c_orientation);
 
      CPose2 Inverse() const;
 
      CPose2 operator*(const CPose2& c_pose) const;
 
      CVector2 operator*(const CVector2& c_vec) const;
 
      CPose2& operator*=(const CPose2& c_pose);
 
   private:
      CVector2 m_cPosition;
      CRadians m_cOrientation;
   };
 
} // namespace argos
#endif