vector2.h

Go to the documentation of this file.

 
#ifndef VECTOR2_H
#define VECTOR2_H
 
namespace argos {
   class CRotationMatrix2;
}
 
#include <argos3/core/utility/math/general.h>
#include <argos3/core/utility/math/angles.h>
#include <argos3/core/utility/math/matrix/matrix.h>
#include <argos3/core/utility/string_utilities.h>
#include <array>
#include <iostream>
#include <cmath>
 
namespace argos {
 
   class CVector2 {
   
   friend class CRotationMatrix2;
   friend class CTransformationMatrix2;
   friend class CPose2;
 
   public:
 
      static const CVector2 X;
 
      static const CVector2 Y;
 
      static const CVector2 ZERO;
 
      CVector2() :
         m_fX(0.0),
         m_fY(0.0) {
      }
 
      CVector2(Real f_x,
               Real f_y) :
         m_fX(f_x),
         m_fY(f_y) {
      }
 
      CVector2(const std::array<Real, 2>& arr_coordinates) {
         Set(arr_coordinates);
      }
 
      CVector2(Real f_length,
               const CRadians& f_angle) :
         m_fX(Cos(f_angle) * f_length),
         m_fY(Sin(f_angle) * f_length) {
      }
 
      inline Real GetX() const {
         return m_fX;
      }
 
      inline void SetX(Real f_x) {
         m_fX = f_x;
      }
 
      inline Real GetY() const {
         return m_fY;
      }
 
      inline void SetY(Real f_y) {
         m_fY = f_y;
      }
 
      inline void Set(Real f_x, Real f_y) {
         m_fX = f_x;
         m_fY = f_y;
      }
 
      inline void Set(const std::array<Real, 2>& arr_coordinates) {
         m_fX = arr_coordinates[0];
         m_fY = arr_coordinates[1];
      }
 
      inline void FromPolarCoordinates(Real f_length,
                                       const CRadians& f_angle) {
         m_fX = Cos(f_angle) * f_length;
         m_fY = Sin(f_angle) * f_length;
      }
 
      inline Real SquareLength() const {
         return Square(m_fX) + Square(m_fY);
      }
 
      inline Real Length() const {
         return Sqrt(SquareLength());
      }
 
      inline CVector2& Normalize() {
         *this /= Length();
         return *this;
      }
 
      inline CRadians Angle() const {
         return ATan2(m_fY, m_fX);
      }
 
      inline CVector2& Rotate(const CRadians& c_angle) {
         Real fSin, fCos;
#ifdef ARGOS_SINCOS
         SinCos(c_angle, fSin, fCos);
#else
         fSin = Sin(c_angle);
         fCos = Cos(c_angle);
#endif
         Real fX = m_fX * fCos - m_fY * fSin;
         Real fY = m_fX * fSin + m_fY * fCos;
         m_fX = fX;
         m_fY = fY;
         return *this;
      }
 
      inline Real DotProduct(const CVector2& c_vector2) const {
         return m_fX * c_vector2.m_fX + m_fY * c_vector2.m_fY;
      }
 
      inline Real CrossProduct(const CVector2& c_vector2) const {
         return m_fX * c_vector2.m_fY - m_fY * c_vector2.m_fX;
      }
 
      inline CVector2& Scale(Real f_scale_x,
                             Real f_scale_y) {
         m_fX *= f_scale_x;
         m_fY *= f_scale_y;
         return *this;
      }
 
      inline CVector2& Perpendicularize() {
         Real fNewX = -m_fY;
         m_fY = m_fX;
         m_fX = fNewX;
         return *this;
      }
 
      inline CVector2& Absolute() {
         m_fX = Abs(m_fX);
         m_fY = Abs(m_fY);
         return *this;
      }
 
      inline bool operator==(const CVector2& c_vector2) const {
         return (m_fX == c_vector2.m_fX && m_fY == c_vector2.m_fY);
      }
 
      inline bool operator!=(const CVector2& c_vector2) const {
         return (m_fX != c_vector2.m_fX || m_fY != c_vector2.m_fY);
      }
 
      inline CVector2& operator+=(const CVector2& c_vector2) {
         m_fX += c_vector2.m_fX;
         m_fY += c_vector2.m_fY;
         return *this;
      }
 
      inline CVector2& operator-=(const CVector2& c_vector2) {
         m_fX -= c_vector2.m_fX;
         m_fY -= c_vector2.m_fY;
         return *this;
      }
 
      inline CVector2& operator*=(Real f_value) {
         m_fX *= f_value;
         m_fY *= f_value;
         return *this;
      }
 
      inline CVector2& operator/=(Real f_value) {
         m_fX /= f_value;
         m_fY /= f_value;
         return *this;
      }
 
      inline CVector2 operator+(const CVector2& c_vector2) const {
         CVector2 cResult(*this);
         cResult += c_vector2;
         return cResult;
      }
 
      inline CVector2 operator-(const CVector2& c_vector2) const {
         CVector2 cResult(*this);
         cResult -= c_vector2;
         return cResult;
      }
 
      inline CVector2 operator*(Real f_value) const {
         CVector2 cResult(*this);
         cResult *= f_value;
         return cResult;
      }
 
      inline CVector2 operator/(Real f_value) const {
         CVector2 cResult(*this);
         cResult /= f_value;
         return cResult;
      }
 
      operator CMatrix<1,2>() const {
         CMatrix<1,2> cMatrix;
         cMatrix(0,0) = m_fX;
         cMatrix(0,1) = m_fY;
         return cMatrix;
      }
 
      operator CMatrix<2,1>() const {
         CMatrix<2,1> cMatrix;
         cMatrix(0,0) = m_fX;
         cMatrix(1,0) = m_fY;
         return cMatrix;
      }
 
      inline friend CVector2 operator*(Real f_value,
                                       const CVector2& c_vector2) {
         return c_vector2 * f_value;
      }
 
      inline CVector2 operator-() const {
         return CVector2(-m_fX, -m_fY);
      }
 
      inline friend std::ostream& operator<<(std::ostream& c_os,
                                             const CVector2& c_vector2) {
         c_os << c_vector2.m_fX << ","
              << c_vector2.m_fY;
         return c_os;
      }
 
      inline friend std::istream& operator>>(std::istream& c_is,
                                             CVector2& c_vector2) {
         Real fValues[2];
         ParseValues<Real> (c_is, 2, fValues, ',');
         c_vector2.Set(fValues[0], fValues[1]);
         return c_is;
      }
 
   private:
 
      Real m_fX;
 
      Real m_fY;
 
   };
 
   /****************************************/
   /****************************************/
 
   inline Real SquareDistance(const CVector2& c_v1, const CVector2& c_v2) {
      return (c_v1 - c_v2).SquareLength();
   }
 
   inline Real Distance(const CVector2& c_v1, const CVector2& c_v2) {
      return (c_v1 - c_v2).Length();
   }
 
   /****************************************/
   /****************************************/
 
}
 
#endif