ARGoS  3
A parallel, multi-engine simulator for swarm robotics
core/utility/math/angles.h
Go to the documentation of this file.
00001 
00016 #ifndef ANGLES_H
00017 #define ANGLES_H
00018 
00019 namespace argos {
00020    class CRadians;
00021    class CDegrees;
00022 }
00023 
00024 #include <argos3/core/utility/datatypes/datatypes.h>
00025 #include <argos3/core/utility/math/general.h>
00026 #include <argos3/core/utility/math/range.h>
00027 #include <cmath>
00028 
00032 #define ARGOS_PI 3.14159265358979323846264338327950288
00033 
00034 namespace argos {
00035 
00036    /****************************************/
00037    /****************************************/
00038 
00042    class CRadians {
00043 
00044    public:
00045 
00049       static const CRadians PI;
00050 
00054       static const CRadians TWO_PI;
00055 
00059       static const CRadians PI_OVER_TWO;
00060 
00064       static const CRadians PI_OVER_THREE;
00065 
00069       static const CRadians PI_OVER_FOUR;
00070 
00074       static const CRadians PI_OVER_SIX;
00075 
00079       static const CRadians ZERO;
00080 
00085       CRadians() :
00086          m_fValue(0.0) {
00087       }
00088 
00094       explicit CRadians(Real f_value) :
00095          m_fValue(f_value) {
00096       }
00097 
00103       inline void FromValueInDegrees(Real f_value) {
00104          m_fValue = f_value / RADIANS_TO_DEGREES;
00105       }
00106 
00112       inline void FromValueInAseba(SInt16 n_value) {
00113          ASEBA_RANGE.MapValueIntoRange(*this, n_value, SIGNED_RANGE);
00114       }
00115 
00120       inline Real GetValue() const {
00121          return m_fValue;
00122       }
00123 
00128       inline Real GetAbsoluteValue() const {
00129          return Abs(m_fValue);
00130       }
00131 
00136       inline void SetValue(Real f_value) {
00137          m_fValue = f_value;
00138       }
00139 
00146       inline CRadians& SignedNormalize() {
00147          SIGNED_RANGE.WrapValue(*this);
00148          return *this;
00149       }
00150 
00157       inline CRadians& UnsignedNormalize() {
00158          UNSIGNED_RANGE.WrapValue(*this);
00159          return *this;
00160       }
00161 
00162       inline CRadians& Negate() {
00163          m_fValue = -m_fValue;
00164          return *this;
00165       }
00166 
00167       inline CRadians& operator+() {
00168          return *this;
00169       }
00170 
00171       inline CRadians operator-() const {
00172          return CRadians(-m_fValue);
00173       }
00174 
00175       inline CRadians& operator+=(const CRadians& c_radians) {
00176          m_fValue += c_radians.m_fValue;
00177          return *this;
00178       }
00179 
00180       inline CRadians& operator-=(const CRadians& c_radians) {
00181          m_fValue -= c_radians.m_fValue;
00182          return *this;
00183       }
00184 
00185       inline CRadians& operator*=(Real f_value) {
00186          m_fValue *= f_value;
00187          return *this;
00188       }
00189 
00190       inline CRadians& operator/=(Real f_value) {
00191          m_fValue /= f_value;
00192          return *this;
00193       }
00194 
00195       inline CRadians operator+(const CRadians& c_radians) const {
00196          CRadians cResult(*this);
00197          cResult += c_radians;
00198          return cResult;
00199       }
00200 
00201       inline CRadians operator-(const CRadians& c_radians) const {
00202          CRadians cResult(*this);
00203          cResult -= c_radians;
00204          return cResult;
00205       }
00206 
00207       inline CRadians operator*(Real f_value) const {
00208          CRadians cResult(*this);
00209          cResult *= f_value;
00210          return cResult;
00211       }
00212 
00213       inline friend CRadians operator*(Real f_value,
00214                                        const CRadians& c_radians) {
00215          CRadians cResult(c_radians);
00216          cResult *= f_value;
00217          return cResult;
00218       }
00219 
00220       inline Real operator/(const CRadians& c_radians) const {
00221          return m_fValue / c_radians.m_fValue;
00222       }
00223 
00224       inline CRadians operator/(Real f_value) const {
00225          CRadians cResult(*this);
00226          cResult /= f_value;
00227          return cResult;
00228       }
00229 
00230       inline bool operator<(const CRadians& c_radians) const {
00231          return m_fValue < c_radians.m_fValue;
00232       }
00233 
00234       inline bool operator<=(const CRadians& c_radians) const {
00235          return m_fValue <= c_radians.m_fValue;
00236       }
00237 
00238       inline bool operator>(const CRadians& c_radians) const {
00239          return m_fValue > c_radians.m_fValue;
00240       }
00241 
00242       inline bool operator>=(const CRadians& c_radians) const {
00243          return m_fValue >= c_radians.m_fValue;
00244       }
00245 
00246       inline bool operator==(const CRadians& c_radians) const {
00247          return m_fValue == c_radians.m_fValue;
00248       }
00249 
00250       inline bool operator!=(const CRadians& c_radians) const {
00251          return m_fValue != c_radians.m_fValue;
00252       }
00253 
00258       friend CDegrees ToDegrees(const CRadians& c_radians);
00259 
00260       inline friend std::ostream& operator<<(std::ostream& c_os,
00261                                              const CRadians& c_radians) {
00262          c_os << "CRadians("
00263               << c_radians.m_fValue
00264               << " -> "
00265               << c_radians.m_fValue * RADIANS_TO_DEGREES
00266               << " degrees"
00267               << ")";
00268          return c_os;
00269       }
00270 
00271       inline friend std::istream& operator>>(std::istream& is,
00272                                              CRadians& c_radians) {
00273          is >> c_radians.m_fValue;
00274          return is;
00275       }
00276 
00277    public:
00278 
00279       static const CRange<CRadians> SIGNED_RANGE; 
00280       static const CRange<CRadians> UNSIGNED_RANGE; 
00281       static const CRange<SInt32> ASEBA_RANGE; 
00282       static const Real RADIANS_TO_DEGREES; 
00284    private:
00285 
00286       Real m_fValue;            
00287    };
00288 
00289    /****************************************/
00290    /****************************************/
00291 
00295    class CDegrees {
00296 
00297    public:
00298 
00303       CDegrees() :
00304          m_fValue(0.0) {
00305       }
00306 
00312       explicit CDegrees(Real f_value) :
00313          m_fValue(f_value) {
00314       }
00315 
00321       inline void FromValueInRadians(Real f_value) {
00322          m_fValue = f_value / DEGREES_TO_RADIANS;
00323       }
00324 
00330       inline void FromValueInAseba(SInt16 n_value) {
00331          ASEBA_RANGE.MapValueIntoRange(*this, n_value, SIGNED_RANGE);
00332       }
00333 
00338       inline Real GetValue() const {
00339          return m_fValue;
00340       }
00341 
00346       inline void SetValue(Real f_value) {
00347          m_fValue = f_value;
00348       }
00349 
00355       CDegrees& SignedNormalize() {
00356          SIGNED_RANGE.WrapValue(*this);
00357          return (*this);
00358       }
00359 
00365       CDegrees& UnsignedNormalize() {
00366          UNSIGNED_RANGE.WrapValue(*this);
00367          return (*this);
00368       }
00369 
00370       inline CDegrees& operator+() {
00371          return *this;
00372       }
00373 
00374       inline CDegrees operator-() const {
00375          return CDegrees(-m_fValue);
00376       }
00377 
00378       inline CDegrees& operator+=(const CDegrees& c_degrees) {
00379          m_fValue += c_degrees.m_fValue;
00380          return *this;
00381       }
00382 
00383       inline CDegrees& operator-=(const CDegrees& c_degrees) {
00384          m_fValue -= c_degrees.m_fValue;
00385          return *this;
00386       }
00387 
00388       inline CDegrees& operator*=(Real f_value) {
00389          m_fValue *= f_value;
00390          return *this;
00391       }
00392 
00393       inline CDegrees& operator/=(Real f_value) {
00394          m_fValue /= f_value;
00395          return *this;
00396       }
00397 
00398       inline CDegrees operator+(const CDegrees& c_degrees) const {
00399          CDegrees cResult(*this);
00400          cResult += c_degrees;
00401          return cResult;
00402       }
00403 
00404       inline CDegrees operator-(const CDegrees& c_degrees) const {
00405          CDegrees cResult(*this);
00406          cResult -= c_degrees;
00407          return cResult;
00408       }
00409 
00410       inline CDegrees operator*(Real f_value) const {
00411          CDegrees cResult(*this);
00412          cResult *= f_value;
00413          return cResult;
00414       }
00415 
00416       inline friend CDegrees operator*(Real f_value,
00417                                        const CDegrees& c_degrees) {
00418          CDegrees cResult(c_degrees);
00419          cResult *= f_value;
00420          return cResult;
00421       }
00422 
00423       inline Real operator/(const CDegrees& c_degrees) const {
00424          return m_fValue / c_degrees.m_fValue;
00425       }
00426 
00427       inline CDegrees operator/(Real f_value) const {
00428          CDegrees cResult(*this);
00429          cResult /= f_value;
00430          return cResult;
00431       }
00432 
00433       inline bool operator<(const CDegrees& c_degrees) const {
00434          return m_fValue < c_degrees.m_fValue;
00435       }
00436 
00437       inline bool operator<=(const CDegrees& c_degrees) const {
00438          return m_fValue <= c_degrees.m_fValue;
00439       }
00440 
00441       inline bool operator>(const CDegrees& c_degrees) const {
00442          return m_fValue > c_degrees.m_fValue;
00443       }
00444 
00445       inline bool operator>=(const CDegrees& c_degrees) const {
00446          return m_fValue >= c_degrees.m_fValue;
00447       }
00448 
00449       inline bool operator==(const CDegrees& c_degrees) const {
00450          return m_fValue == c_degrees.m_fValue;
00451       }
00452 
00453       inline bool operator!=(const CDegrees& c_degrees) const {
00454          return m_fValue != c_degrees.m_fValue;
00455       }
00456 
00461       friend CRadians ToRadians(const CDegrees& c_degrees);
00462 
00463       inline friend std::ostream& operator<<(std::ostream& c_os,
00464                                              const CDegrees& c_degrees) {
00465          c_os << "CDegrees("
00466               << c_degrees.m_fValue
00467               << ")";
00468          return c_os;
00469       }
00470 
00471       inline friend std::istream& operator>>(std::istream& is,
00472                                              CDegrees& c_degrees) {
00473          is >> c_degrees.m_fValue;
00474          return is;
00475       }
00476 
00477    private:
00478 
00479       Real m_fValue; 
00480       static const CRange<CDegrees> SIGNED_RANGE; 
00481       static const CRange<CDegrees> UNSIGNED_RANGE; 
00482       static const CRange<SInt16> ASEBA_RANGE; 
00483       static const Real DEGREES_TO_RADIANS; 
00485    };
00486 
00487    /****************************************/
00488    /****************************************/
00489 
00495    inline CDegrees ToDegrees(const CRadians& c_radians) {
00496       return CDegrees(c_radians.m_fValue * CRadians::RADIANS_TO_DEGREES);
00497    }
00498 
00504    inline CRadians ToRadians(const CDegrees& c_degrees) {
00505       return CRadians(c_degrees.m_fValue * CDegrees::DEGREES_TO_RADIANS);
00506    }
00507    
00508    /****************************************/
00509    /****************************************/
00510 
00511 #undef ARGOS_SINCOS
00512 #ifdef ARGOS_USE_DOUBLE
00513 #  ifdef _GNU_SOURCE
00514 #    define ARGOS_SINCOS ::sincos
00515 #  endif
00516 #  define ARGOS_SIN    ::sin
00517 #  define ARGOS_ASIN   ::asin
00518 #  define ARGOS_COS    ::cos
00519 #  define ARGOS_ACOS   ::acos
00520 #  define ARGOS_TAN    ::tan
00521 #  define ARGOS_ATAN2  ::atan2
00522 #else
00523 #  ifdef _GNU_SOURCE
00524 #    define ARGOS_SINCOS ::sincosf
00525 #  endif
00526 #  define ARGOS_SIN    ::sinf
00527 #  define ARGOS_ASIN   ::asinf
00528 #  define ARGOS_COS    ::cosf
00529 #  define ARGOS_ACOS   ::acosf
00530 #  define ARGOS_TAN    ::tanf
00531 #  define ARGOS_ATAN2  ::atan2f
00532 #endif
00533 
00534 #ifdef ARGOS_SINCOS
00535 
00541    inline void SinCos(const CRadians& c_radians,
00542                       Real& f_sin,
00543                       Real& f_cos) {
00544       ARGOS_SINCOS(c_radians.GetValue(), &f_sin, &f_cos);
00545    }
00546 #endif
00547 
00553    inline Real Sin(const CRadians& c_radians) {
00554       return ARGOS_SIN(c_radians.GetValue());
00555    }
00556 
00562    inline Real Cos(const CRadians& c_radians) {
00563       return ARGOS_COS(c_radians.GetValue());
00564    }
00565 
00571    inline Real Tan(const CRadians& c_radians) {
00572       return ARGOS_TAN(c_radians.GetValue());
00573    }
00574 
00580    inline CRadians ASin(Real f_value) {
00581       return CRadians(ARGOS_ASIN(f_value));
00582    }
00583 
00589    inline CRadians ACos(Real f_value) {
00590       return CRadians(ARGOS_ACOS(f_value));
00591    }
00592 
00600    inline CRadians ATan2(const Real f_y, const Real f_x) {
00601       return CRadians(ARGOS_ATAN2(f_y, f_x));
00602    }
00603 
00604    /****************************************/
00605    /****************************************/
00606 
00607 }
00608 
00609 #endif