angles.h

Go to the documentation of this file.

 
#ifndef ANGLES_H
#define ANGLES_H
 
namespace argos {
   class CRadians;
   class CDegrees;
}
 
#include <argos3/core/utility/datatypes/datatypes.h>
#include <argos3/core/utility/math/general.h>
#include <argos3/core/utility/math/range.h>
#include <cmath>
 
#define ARGOS_PI 3.14159265358979323846264338327950288
 
namespace argos {
 
   /****************************************/
   /****************************************/
 
   class CRadians {
 
   public:
 
      static const CRadians PI;
 
      static const CRadians TWO_PI;
 
      static const CRadians PI_OVER_TWO;
 
      static const CRadians PI_OVER_THREE;
 
      static const CRadians PI_OVER_FOUR;
 
      static const CRadians PI_OVER_SIX;
 
      static const CRadians ZERO;
 
      CRadians() :
         m_fValue(0.0) {
      }
 
      explicit CRadians(Real f_value) :
         m_fValue(f_value) {
      }
 
      inline void FromValueInDegrees(Real f_value) {
         m_fValue = f_value / RADIANS_TO_DEGREES;
      }
 
      inline Real GetValue() const {
         return m_fValue;
      }
 
      inline Real GetAbsoluteValue() const {
         return Abs(m_fValue);
      }
 
      inline void SetValue(Real f_value) {
         m_fValue = f_value;
      }
 
      inline CRadians& SignedNormalize() {
         SIGNED_RANGE.WrapValue(*this);
         return *this;
      }
 
      inline CRadians& UnsignedNormalize() {
         UNSIGNED_RANGE.WrapValue(*this);
         return *this;
      }
 
      inline CRadians& Negate() {
         m_fValue = -m_fValue;
         return *this;
      }
 
      inline CRadians& operator+() {
         return *this;
      }
 
      inline CRadians operator-() const {
         return CRadians(-m_fValue);
      }
 
      inline CRadians& operator+=(const CRadians& c_radians) {
         m_fValue += c_radians.m_fValue;
         return *this;
      }
 
      inline CRadians& operator-=(const CRadians& c_radians) {
         m_fValue -= c_radians.m_fValue;
         return *this;
      }
 
      inline CRadians& operator*=(Real f_value) {
         m_fValue *= f_value;
         return *this;
      }
 
      inline CRadians& operator/=(Real f_value) {
         m_fValue /= f_value;
         return *this;
      }
 
      inline CRadians operator+(const CRadians& c_radians) const {
         CRadians cResult(*this);
         cResult += c_radians;
         return cResult;
      }
 
      inline CRadians operator-(const CRadians& c_radians) const {
         CRadians cResult(*this);
         cResult -= c_radians;
         return cResult;
      }
 
      inline CRadians operator*(Real f_value) const {
         CRadians cResult(*this);
         cResult *= f_value;
         return cResult;
      }
 
      inline friend CRadians operator*(Real f_value,
                                       const CRadians& c_radians) {
         CRadians cResult(c_radians);
         cResult *= f_value;
         return cResult;
      }
 
      inline Real operator/(const CRadians& c_radians) const {
         return m_fValue / c_radians.m_fValue;
      }
 
      inline CRadians operator/(Real f_value) const {
         CRadians cResult(*this);
         cResult /= f_value;
         return cResult;
      }
 
      inline bool operator<(const CRadians& c_radians) const {
         return m_fValue < c_radians.m_fValue;
      }
 
      inline bool operator<=(const CRadians& c_radians) const {
         return m_fValue <= c_radians.m_fValue;
      }
 
      inline bool operator>(const CRadians& c_radians) const {
         return m_fValue > c_radians.m_fValue;
      }
 
      inline bool operator>=(const CRadians& c_radians) const {
         return m_fValue >= c_radians.m_fValue;
      }
 
      inline bool operator==(const CRadians& c_radians) const {
         return m_fValue == c_radians.m_fValue;
      }
 
      inline bool operator!=(const CRadians& c_radians) const {
         return m_fValue != c_radians.m_fValue;
      }
 
      friend CDegrees ToDegrees(const CRadians& c_radians);
 
      friend CRadians NormalizedDifference(const CRadians& c_rad1,
                                           const CRadians& c_rad2);
 
      inline friend std::ostream& operator<<(std::ostream& c_os,
                                             const CRadians& c_radians) {
         c_os << "CRadians("
              << c_radians.m_fValue
              << " -> "
              << c_radians.m_fValue * RADIANS_TO_DEGREES
              << " degrees"
              << ")";
         return c_os;
      }
 
      inline friend std::istream& operator>>(std::istream& is,
                                             CRadians& c_radians) {
         is >> c_radians.m_fValue;
         return is;
      }
 
   public:
 
      static const CRange<CRadians> SIGNED_RANGE; 
      static const CRange<CRadians> UNSIGNED_RANGE; 
      static const Real RADIANS_TO_DEGREES; 
   private:
 
      Real m_fValue;            
   };
 
   /****************************************/
   /****************************************/
 
   class CDegrees {
 
   public:
 
      CDegrees() :
         m_fValue(0.0) {
      }
 
      explicit CDegrees(Real f_value) :
         m_fValue(f_value) {
      }
 
      inline void FromValueInRadians(Real f_value) {
         m_fValue = f_value / DEGREES_TO_RADIANS;
      }
 
      inline Real GetValue() const {
         return m_fValue;
      }
 
      inline Real GetAbsoluteValue() const {
       return Abs(m_fValue);
      }
 
      inline void SetValue(Real f_value) {
         m_fValue = f_value;
      }
 
      CDegrees& SignedNormalize() {
         SIGNED_RANGE.WrapValue(*this);
         return (*this);
      }
 
      CDegrees& UnsignedNormalize() {
         UNSIGNED_RANGE.WrapValue(*this);
         return (*this);
      }
 
      inline CDegrees& operator+() {
         return *this;
      }
 
      inline CDegrees operator-() const {
         return CDegrees(-m_fValue);
      }
 
      inline CDegrees& operator+=(const CDegrees& c_degrees) {
         m_fValue += c_degrees.m_fValue;
         return *this;
      }
 
      inline CDegrees& operator-=(const CDegrees& c_degrees) {
         m_fValue -= c_degrees.m_fValue;
         return *this;
      }
 
      inline CDegrees& operator*=(Real f_value) {
         m_fValue *= f_value;
         return *this;
      }
 
      inline CDegrees& operator/=(Real f_value) {
         m_fValue /= f_value;
         return *this;
      }
 
      inline CDegrees operator+(const CDegrees& c_degrees) const {
         CDegrees cResult(*this);
         cResult += c_degrees;
         return cResult;
      }
 
      inline CDegrees operator-(const CDegrees& c_degrees) const {
         CDegrees cResult(*this);
         cResult -= c_degrees;
         return cResult;
      }
 
      inline CDegrees operator*(Real f_value) const {
         CDegrees cResult(*this);
         cResult *= f_value;
         return cResult;
      }
 
      inline friend CDegrees operator*(Real f_value,
                                       const CDegrees& c_degrees) {
         CDegrees cResult(c_degrees);
         cResult *= f_value;
         return cResult;
      }
 
      inline Real operator/(const CDegrees& c_degrees) const {
         return m_fValue / c_degrees.m_fValue;
      }
 
      inline CDegrees operator/(Real f_value) const {
         CDegrees cResult(*this);
         cResult /= f_value;
         return cResult;
      }
 
      inline bool operator<(const CDegrees& c_degrees) const {
         return m_fValue < c_degrees.m_fValue;
      }
 
      inline bool operator<=(const CDegrees& c_degrees) const {
         return m_fValue <= c_degrees.m_fValue;
      }
 
      inline bool operator>(const CDegrees& c_degrees) const {
         return m_fValue > c_degrees.m_fValue;
      }
 
      inline bool operator>=(const CDegrees& c_degrees) const {
         return m_fValue >= c_degrees.m_fValue;
      }
 
      inline bool operator==(const CDegrees& c_degrees) const {
         return m_fValue == c_degrees.m_fValue;
      }
 
      inline bool operator!=(const CDegrees& c_degrees) const {
         return m_fValue != c_degrees.m_fValue;
      }
 
      friend CRadians ToRadians(const CDegrees& c_degrees);
 
      friend CDegrees NormalizedDifference(const CDegrees& c_angle1,
                                           const CDegrees& c_angle2);
 
      inline friend std::ostream& operator<<(std::ostream& c_os,
                                             const CDegrees& c_degrees) {
         c_os << "CDegrees("
              << c_degrees.m_fValue
              << ")";
         return c_os;
      }
 
      inline friend std::istream& operator>>(std::istream& is,
                                             CDegrees& c_degrees) {
         is >> c_degrees.m_fValue;
         return is;
      }
 
   private:
 
      Real m_fValue; 
      static const CRange<CDegrees> SIGNED_RANGE; 
      static const CRange<CDegrees> UNSIGNED_RANGE; 
      static const Real DEGREES_TO_RADIANS; 
   };
 
   /****************************************/
   /****************************************/
 
   inline CDegrees ToDegrees(const CRadians& c_radians) {
      return CDegrees(c_radians.m_fValue * CRadians::RADIANS_TO_DEGREES);
   }
 
   inline CRadians ToRadians(const CDegrees& c_degrees) {
      return CRadians(c_degrees.m_fValue * CDegrees::DEGREES_TO_RADIANS);
   }
 
   inline CRadians NormalizedDifference(const CRadians& c_angle1,
                                        const CRadians& c_angle2) {
      CRadians cResult;
      cResult.m_fValue = Mod(c_angle1.m_fValue - c_angle2.m_fValue + CRadians::PI.m_fValue,
                             CRadians::TWO_PI.m_fValue);
      if(cResult.m_fValue < 0.0f) cResult.m_fValue += CRadians::TWO_PI.m_fValue;
      cResult.m_fValue -= CRadians::PI.m_fValue;
      return cResult;
   }
   
   inline CDegrees NormalizedDifference(const CDegrees& c_angle1,
                                        const CDegrees& c_angle2) {
      CDegrees cResult;
      cResult.m_fValue = Mod(c_angle1.m_fValue - c_angle2.m_fValue + 180.0f, 360.0f);
      if(cResult.m_fValue < 0.0f) cResult.m_fValue += 360.0f;
      cResult.m_fValue -= 180.0f;
      return cResult;
   }
   
   /****************************************/
   /****************************************/
 
#undef ARGOS_SINCOS
#ifdef ARGOS_USE_DOUBLE
#  ifndef __APPLE__
#    define ARGOS_SINCOS ::sincos
#  else
#    define ARGOS_SINCOS ::__sincos
#  endif
#  define ARGOS_SIN    ::sin
#  define ARGOS_ASIN   ::asin
#  define ARGOS_COS    ::cos
#  define ARGOS_ACOS   ::acos
#  define ARGOS_TAN    ::tan
#  define ARGOS_ATAN2  ::atan2
#else
#  ifndef __APPLE__
#    define ARGOS_SINCOS ::sincosf
#  else
#    define ARGOS_SINCOS ::__sincosf
#  endif
#  define ARGOS_SIN    ::sinf
#  define ARGOS_ASIN   ::asinf
#  define ARGOS_COS    ::cosf
#  define ARGOS_ACOS   ::acosf
#  define ARGOS_TAN    ::tanf
#  define ARGOS_ATAN2  ::atan2f
#endif
 
#ifdef ARGOS_SINCOS
   inline void SinCos(const CRadians& c_radians,
                      Real& f_sin,
                      Real& f_cos) {
      ARGOS_SINCOS(c_radians.GetValue(), &f_sin, &f_cos);
   }
#endif
 
   inline Real Sin(const CRadians& c_radians) {
      return ARGOS_SIN(c_radians.GetValue());
   }
 
   inline Real Cos(const CRadians& c_radians) {
      return ARGOS_COS(c_radians.GetValue());
   }
 
   inline Real Tan(const CRadians& c_radians) {
      return ARGOS_TAN(c_radians.GetValue());
   }
 
   inline CRadians ASin(Real f_value) {
      return CRadians(ARGOS_ASIN(f_value));
   }
 
   inline CRadians ACos(Real f_value) {
      return CRadians(ARGOS_ACOS(f_value));
   }
 
   inline CRadians ATan2(const Real f_y, const Real f_x) {
      return CRadians(ARGOS_ATAN2(f_y, f_x));
   }
 
   /****************************************/
   /****************************************/
 
}
 
#endif