angles.h
Go to the documentation of this file.
1 
16 #ifndef ANGLES_H
17 #define ANGLES_H
18 
19 namespace argos {
20  class CRadians;
21  class CDegrees;
22 }
23 
24 #include <argos3/core/utility/datatypes/datatypes.h>
25 #include <argos3/core/utility/math/general.h>
26 #include <argos3/core/utility/math/range.h>
27 #include <cmath>
28 
32 #define ARGOS_PI 3.14159265358979323846264338327950288
33 
34 namespace argos {
35 
36  /****************************************/
37  /****************************************/
38 
42  class CRadians {
43 
44  public:
45 
49  static const CRadians PI;
50 
54  static const CRadians TWO_PI;
55 
59  static const CRadians PI_OVER_TWO;
60 
64  static const CRadians PI_OVER_THREE;
65 
69  static const CRadians PI_OVER_FOUR;
70 
74  static const CRadians PI_OVER_SIX;
75 
79  static const CRadians ZERO;
80 
86  m_fValue(0.0) {
87  }
88 
94  explicit CRadians(Real f_value) :
95  m_fValue(f_value) {
96  }
97 
103  inline void FromValueInDegrees(Real f_value) {
104  m_fValue = f_value / RADIANS_TO_DEGREES;
105  }
106 
111  inline Real GetValue() const {
112  return m_fValue;
113  }
114 
119  inline Real GetAbsoluteValue() const {
120  return Abs(m_fValue);
121  }
122 
127  inline void SetValue(Real f_value) {
128  m_fValue = f_value;
129  }
130 
138  SIGNED_RANGE.WrapValue(*this);
139  return *this;
140  }
141 
149  UNSIGNED_RANGE.WrapValue(*this);
150  return *this;
151  }
152 
153  inline CRadians& Negate() {
154  m_fValue = -m_fValue;
155  return *this;
156  }
157 
158  inline CRadians& operator+() {
159  return *this;
160  }
161 
162  inline CRadians operator-() const {
163  return CRadians(-m_fValue);
164  }
165 
166  inline CRadians& operator+=(const CRadians& c_radians) {
167  m_fValue += c_radians.m_fValue;
168  return *this;
169  }
170 
171  inline CRadians& operator-=(const CRadians& c_radians) {
172  m_fValue -= c_radians.m_fValue;
173  return *this;
174  }
175 
176  inline CRadians& operator*=(Real f_value) {
177  m_fValue *= f_value;
178  return *this;
179  }
180 
181  inline CRadians& operator/=(Real f_value) {
182  m_fValue /= f_value;
183  return *this;
184  }
185 
186  inline CRadians operator+(const CRadians& c_radians) const {
187  CRadians cResult(*this);
188  cResult += c_radians;
189  return cResult;
190  }
191 
192  inline CRadians operator-(const CRadians& c_radians) const {
193  CRadians cResult(*this);
194  cResult -= c_radians;
195  return cResult;
196  }
197 
198  inline CRadians operator*(Real f_value) const {
199  CRadians cResult(*this);
200  cResult *= f_value;
201  return cResult;
202  }
203 
204  inline friend CRadians operator*(Real f_value,
205  const CRadians& c_radians) {
206  CRadians cResult(c_radians);
207  cResult *= f_value;
208  return cResult;
209  }
210 
211  inline Real operator/(const CRadians& c_radians) const {
212  return m_fValue / c_radians.m_fValue;
213  }
214 
215  inline CRadians operator/(Real f_value) const {
216  CRadians cResult(*this);
217  cResult /= f_value;
218  return cResult;
219  }
220 
221  inline bool operator<(const CRadians& c_radians) const {
222  return m_fValue < c_radians.m_fValue;
223  }
224 
225  inline bool operator<=(const CRadians& c_radians) const {
226  return m_fValue <= c_radians.m_fValue;
227  }
228 
229  inline bool operator>(const CRadians& c_radians) const {
230  return m_fValue > c_radians.m_fValue;
231  }
232 
233  inline bool operator>=(const CRadians& c_radians) const {
234  return m_fValue >= c_radians.m_fValue;
235  }
236 
237  inline bool operator==(const CRadians& c_radians) const {
238  return m_fValue == c_radians.m_fValue;
239  }
240 
241  inline bool operator!=(const CRadians& c_radians) const {
242  return m_fValue != c_radians.m_fValue;
243  }
244 
249  friend CDegrees ToDegrees(const CRadians& c_radians);
250 
251  friend CRadians NormalizedDifference(const CRadians& c_rad1,
252  const CRadians& c_rad2);
253 
254  inline friend std::ostream& operator<<(std::ostream& c_os,
255  const CRadians& c_radians) {
256  c_os << "CRadians("
257  << c_radians.m_fValue
258  << " -> "
259  << c_radians.m_fValue * RADIANS_TO_DEGREES
260  << " degrees"
261  << ")";
262  return c_os;
263  }
264 
265  inline friend std::istream& operator>>(std::istream& is,
266  CRadians& c_radians) {
267  is >> c_radians.m_fValue;
268  return is;
269  }
270 
271  public:
272 
275  static const Real RADIANS_TO_DEGREES;
277  private:
278 
279  Real m_fValue;
280  };
281 
282  /****************************************/
283  /****************************************/
284 
288  class CDegrees {
289 
290  public:
291 
297  m_fValue(0.0) {
298  }
299 
305  explicit CDegrees(Real f_value) :
306  m_fValue(f_value) {
307  }
308 
314  inline void FromValueInRadians(Real f_value) {
315  m_fValue = f_value / DEGREES_TO_RADIANS;
316  }
317 
322  inline Real GetValue() const {
323  return m_fValue;
324  }
325 
330  inline Real GetAbsoluteValue() const {
331  return Abs(m_fValue);
332  }
333 
338  inline void SetValue(Real f_value) {
339  m_fValue = f_value;
340  }
341 
348  SIGNED_RANGE.WrapValue(*this);
349  return (*this);
350  }
351 
358  UNSIGNED_RANGE.WrapValue(*this);
359  return (*this);
360  }
361 
362  inline CDegrees& operator+() {
363  return *this;
364  }
365 
366  inline CDegrees operator-() const {
367  return CDegrees(-m_fValue);
368  }
369 
370  inline CDegrees& operator+=(const CDegrees& c_degrees) {
371  m_fValue += c_degrees.m_fValue;
372  return *this;
373  }
374 
375  inline CDegrees& operator-=(const CDegrees& c_degrees) {
376  m_fValue -= c_degrees.m_fValue;
377  return *this;
378  }
379 
380  inline CDegrees& operator*=(Real f_value) {
381  m_fValue *= f_value;
382  return *this;
383  }
384 
385  inline CDegrees& operator/=(Real f_value) {
386  m_fValue /= f_value;
387  return *this;
388  }
389 
390  inline CDegrees operator+(const CDegrees& c_degrees) const {
391  CDegrees cResult(*this);
392  cResult += c_degrees;
393  return cResult;
394  }
395 
396  inline CDegrees operator-(const CDegrees& c_degrees) const {
397  CDegrees cResult(*this);
398  cResult -= c_degrees;
399  return cResult;
400  }
401 
402  inline CDegrees operator*(Real f_value) const {
403  CDegrees cResult(*this);
404  cResult *= f_value;
405  return cResult;
406  }
407 
408  inline friend CDegrees operator*(Real f_value,
409  const CDegrees& c_degrees) {
410  CDegrees cResult(c_degrees);
411  cResult *= f_value;
412  return cResult;
413  }
414 
415  inline Real operator/(const CDegrees& c_degrees) const {
416  return m_fValue / c_degrees.m_fValue;
417  }
418 
419  inline CDegrees operator/(Real f_value) const {
420  CDegrees cResult(*this);
421  cResult /= f_value;
422  return cResult;
423  }
424 
425  inline bool operator<(const CDegrees& c_degrees) const {
426  return m_fValue < c_degrees.m_fValue;
427  }
428 
429  inline bool operator<=(const CDegrees& c_degrees) const {
430  return m_fValue <= c_degrees.m_fValue;
431  }
432 
433  inline bool operator>(const CDegrees& c_degrees) const {
434  return m_fValue > c_degrees.m_fValue;
435  }
436 
437  inline bool operator>=(const CDegrees& c_degrees) const {
438  return m_fValue >= c_degrees.m_fValue;
439  }
440 
441  inline bool operator==(const CDegrees& c_degrees) const {
442  return m_fValue == c_degrees.m_fValue;
443  }
444 
445  inline bool operator!=(const CDegrees& c_degrees) const {
446  return m_fValue != c_degrees.m_fValue;
447  }
448 
453  friend CRadians ToRadians(const CDegrees& c_degrees);
454 
455  friend CDegrees NormalizedDifference(const CDegrees& c_angle1,
456  const CDegrees& c_angle2);
457 
458  inline friend std::ostream& operator<<(std::ostream& c_os,
459  const CDegrees& c_degrees) {
460  c_os << "CDegrees("
461  << c_degrees.m_fValue
462  << ")";
463  return c_os;
464  }
465 
466  inline friend std::istream& operator>>(std::istream& is,
467  CDegrees& c_degrees) {
468  is >> c_degrees.m_fValue;
469  return is;
470  }
471 
472  private:
473 
474  Real m_fValue;
475  static const CRange<CDegrees> SIGNED_RANGE;
476  static const CRange<CDegrees> UNSIGNED_RANGE;
477  static const Real DEGREES_TO_RADIANS;
479  };
480 
481  /****************************************/
482  /****************************************/
483 
489  inline CDegrees ToDegrees(const CRadians& c_radians) {
490  return CDegrees(c_radians.m_fValue * CRadians::RADIANS_TO_DEGREES);
491  }
492 
498  inline CRadians ToRadians(const CDegrees& c_degrees) {
499  return CRadians(c_degrees.m_fValue * CDegrees::DEGREES_TO_RADIANS);
500  }
501 
510  inline CRadians NormalizedDifference(const CRadians& c_angle1,
511  const CRadians& c_angle2) {
512  CRadians cResult;
513  cResult.m_fValue = Mod(c_angle1.m_fValue - c_angle2.m_fValue + CRadians::PI.m_fValue,
514  CRadians::TWO_PI.m_fValue);
515  if(cResult.m_fValue < 0.0f) cResult.m_fValue += CRadians::TWO_PI.m_fValue;
516  cResult.m_fValue -= CRadians::PI.m_fValue;
517  return cResult;
518  }
519 
528  inline CDegrees NormalizedDifference(const CDegrees& c_angle1,
529  const CDegrees& c_angle2) {
530  CDegrees cResult;
531  cResult.m_fValue = Mod(c_angle1.m_fValue - c_angle2.m_fValue + 180.0f, 360.0f);
532  if(cResult.m_fValue < 0.0f) cResult.m_fValue += 360.0f;
533  cResult.m_fValue -= 180.0f;
534  return cResult;
535  }
536 
537  /****************************************/
538  /****************************************/
539 
540 #undef ARGOS_SINCOS
541 #ifdef ARGOS_USE_DOUBLE
542 # ifndef __APPLE__
543 # define ARGOS_SINCOS ::sincos
544 # else
545 # define ARGOS_SINCOS ::__sincos
546 # endif
547 # define ARGOS_SIN ::sin
548 # define ARGOS_ASIN ::asin
549 # define ARGOS_COS ::cos
550 # define ARGOS_ACOS ::acos
551 # define ARGOS_TAN ::tan
552 # define ARGOS_ATAN2 ::atan2
553 #else
554 # ifndef __APPLE__
555 # define ARGOS_SINCOS ::sincosf
556 # else
557 # define ARGOS_SINCOS ::__sincosf
558 # endif
559 # define ARGOS_SIN ::sinf
560 # define ARGOS_ASIN ::asinf
561 # define ARGOS_COS ::cosf
562 # define ARGOS_ACOS ::acosf
563 # define ARGOS_TAN ::tanf
564 # define ARGOS_ATAN2 ::atan2f
565 #endif
566 
567 #ifdef ARGOS_SINCOS
568 
574  inline void SinCos(const CRadians& c_radians,
575  Real& f_sin,
576  Real& f_cos) {
577  ARGOS_SINCOS(c_radians.GetValue(), &f_sin, &f_cos);
578  }
579 #endif
580 
586  inline Real Sin(const CRadians& c_radians) {
587  return ARGOS_SIN(c_radians.GetValue());
588  }
589 
595  inline Real Cos(const CRadians& c_radians) {
596  return ARGOS_COS(c_radians.GetValue());
597  }
598 
604  inline Real Tan(const CRadians& c_radians) {
605  return ARGOS_TAN(c_radians.GetValue());
606  }
607 
613  inline CRadians ASin(Real f_value) {
614  return CRadians(ARGOS_ASIN(f_value));
615  }
616 
622  inline CRadians ACos(Real f_value) {
623  return CRadians(ARGOS_ACOS(f_value));
624  }
625 
633  inline CRadians ATan2(const Real f_y, const Real f_x) {
634  return CRadians(ARGOS_ATAN2(f_y, f_x));
635  }
636 
637  /****************************************/
638  /****************************************/
639 
640 }
641 
642 #endif
friend std::istream & operator>>(std::istream &is, CDegrees &c_degrees)
Definition: angles.h:466
CRadians & operator+()
Definition: angles.h:158
bool operator!=(const CRadians &c_radians) const
Definition: angles.h:241
Real operator/(const CDegrees &c_degrees) const
Definition: angles.h:415
void SetValue(Real f_value)
Sets the value in degrees.
Definition: angles.h:338
static const CRange< CRadians > UNSIGNED_RANGE
The unsigned normalization range [0:TWO_PI].
Definition: angles.h:274
CRadians & operator/=(Real f_value)
Definition: angles.h:181
CDegrees ToDegrees(const CRadians &c_radians)
Converts CRadians to CDegrees.
Definition: angles.h:489
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
CRadians ATan2(const Real f_y, const Real f_x)
Computes the arctangent of the passed values.
Definition: angles.h:633
CRadians & SignedNormalize()
Normalizes the value in the range [-PI:PI].
Definition: angles.h:137
static const CRange< CRadians > SIGNED_RANGE
The signed normalization range [-PI:PI].
Definition: angles.h:273
friend CDegrees operator*(Real f_value, const CDegrees &c_degrees)
Definition: angles.h:408
CDegrees operator-() const
Definition: angles.h:366
friend std::ostream & operator<<(std::ostream &c_os, const CDegrees &c_degrees)
Definition: angles.h:458
bool operator<(const CDegrees &c_degrees) const
Definition: angles.h:425
CRadians operator/(Real f_value) const
Definition: angles.h:215
bool operator<=(const CRadians &c_radians) const
Definition: angles.h:225
T Abs(const T &t_v)
Returns the absolute value of the passed argument.
Definition: general.h:25
static const CRadians TWO_PI
Set to PI * 2.
Definition: angles.h:54
bool operator<=(const CDegrees &c_degrees) const
Definition: angles.h:429
Real GetValue() const
Returns the value in degrees.
Definition: angles.h:322
Real operator/(const CRadians &c_radians) const
Definition: angles.h:211
CDegrees()
Class constructor It initializes m_fValue to 0 degrees.
Definition: angles.h:296
void SetValue(Real f_value)
Sets the value in radians.
Definition: angles.h:127
Real Cos(const CRadians &c_radians)
Computes the cosine of the passed value in radians.
Definition: angles.h:595
CRadians ASin(Real f_value)
Computes the arcsine of the passed value.
Definition: angles.h:613
#define ARGOS_ATAN2
Definition: angles.h:564
#define ARGOS_ASIN
Definition: angles.h:560
Real GetAbsoluteValue() const
Returns the absolute value in radians.
Definition: angles.h:119
bool operator<(const CRadians &c_radians) const
Definition: angles.h:221
bool operator==(const CRadians &c_radians) const
Definition: angles.h:237
CRadians(Real f_value)
Class constructor It initializes m_fValue to the passed value.
Definition: angles.h:94
It defines the basic type CDegrees, used to store an angle value in degrees.
Definition: angles.h:288
bool operator>=(const CRadians &c_radians) const
Definition: angles.h:233
friend CRadians NormalizedDifference(const CRadians &c_rad1, const CRadians &c_rad2)
Calculates the normalized difference between the given angles.
Definition: angles.h:510
CDegrees & operator+=(const CDegrees &c_degrees)
Definition: angles.h:370
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
#define ARGOS_SIN
Definition: angles.h:559
CRadians operator-() const
Definition: angles.h:162
void FromValueInRadians(Real f_value)
Sets the value from a value in radians It sets m_fValue (which is in degrees) converting from the pas...
Definition: angles.h:314
CRadians & UnsignedNormalize()
Normalizes the value in the range [0:TWO_PI].
Definition: angles.h:148
bool operator!=(const CDegrees &c_degrees) const
Definition: angles.h:445
CRadians operator-(const CRadians &c_radians) const
Definition: angles.h:192
CDegrees(Real f_value)
Class constructor It initializes m_fValue to the passed value.
Definition: angles.h:305
friend CDegrees ToDegrees(const CRadians &c_radians)
Converts this object to CDegrees.
Definition: angles.h:489
friend std::istream & operator>>(std::istream &is, CRadians &c_radians)
Definition: angles.h:265
CDegrees operator-(const CDegrees &c_degrees) const
Definition: angles.h:396
static const CRadians PI_OVER_FOUR
Set to PI / 4.
Definition: angles.h:69
CDegrees operator+(const CDegrees &c_degrees) const
Definition: angles.h:390
CRadians operator+(const CRadians &c_radians) const
Definition: angles.h:186
friend CRadians operator*(Real f_value, const CRadians &c_radians)
Definition: angles.h:204
void FromValueInDegrees(Real f_value)
Sets the value from a value in degrees It sets m_fValue (which is in radians) converting from the pas...
Definition: angles.h:103
CRadians & operator+=(const CRadians &c_radians)
Definition: angles.h:166
friend CRadians ToRadians(const CDegrees &c_degrees)
Converts this object to CRadians.
Definition: angles.h:498
friend CDegrees NormalizedDifference(const CDegrees &c_angle1, const CDegrees &c_angle2)
Calculates the normalized difference between the given angles.
Definition: angles.h:528
CDegrees & UnsignedNormalize()
Normalizes the value in the range [0:360].
Definition: angles.h:357
#define ARGOS_COS
Definition: angles.h:561
CRadians & operator-=(const CRadians &c_radians)
Definition: angles.h:171
CRadians operator*(Real f_value) const
Definition: angles.h:198
static const Real RADIANS_TO_DEGREES
Constant to convert from radians to degrees.
Definition: angles.h:275
Real Sin(const CRadians &c_radians)
Computes the sine of the passed value in radians.
Definition: angles.h:586
#define ARGOS_ACOS
Definition: angles.h:562
static const CRadians PI
The PI constant.
Definition: angles.h:49
static const CRadians PI_OVER_SIX
Set to PI / 6.
Definition: angles.h:74
Real GetAbsoluteValue() const
Returns the absolute value in degrees.
Definition: angles.h:330
Real Tan(const CRadians &c_radians)
Computes the tangent of the passed value in radians.
Definition: angles.h:604
void SinCos(const CRadians &c_radians, Real &f_sin, Real &f_cos)
Computes the sine and cosine of the passed value in radians.
Definition: angles.h:574
friend std::ostream & operator<<(std::ostream &c_os, const CRadians &c_radians)
Definition: angles.h:254
CRadians & operator*=(Real f_value)
Definition: angles.h:176
static const CRadians ZERO
Set to zero radians.
Definition: angles.h:79
CRadians NormalizedDifference(const CRadians &c_angle1, const CRadians &c_angle2)
Calculates the normalized difference between the given angles.
Definition: angles.h:510
bool operator>=(const CDegrees &c_degrees) const
Definition: angles.h:437
bool operator>(const CRadians &c_radians) const
Definition: angles.h:229
Real GetValue() const
Returns the value in radians.
Definition: angles.h:111
bool operator==(const CDegrees &c_degrees) const
Definition: angles.h:441
#define ARGOS_TAN
Definition: angles.h:563
CDegrees operator/(Real f_value) const
Definition: angles.h:419
CRadians ToRadians(const CDegrees &c_degrees)
Converts CDegrees to CRadians.
Definition: angles.h:498
CDegrees & operator/=(Real f_value)
Definition: angles.h:385
CDegrees & operator*=(Real f_value)
Definition: angles.h:380
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
CDegrees & SignedNormalize()
Normalizes the value in the range [-180:180].
Definition: angles.h:347
CDegrees & operator-=(const CDegrees &c_degrees)
Definition: angles.h:375
CRadians ACos(Real f_value)
Computes the arccosine of the passed value.
Definition: angles.h:622
#define ARGOS_SINCOS
Definition: angles.h:555
CDegrees operator*(Real f_value) const
Definition: angles.h:402
CRadians & Negate()
Definition: angles.h:153
static const CRadians PI_OVER_THREE
Set to PI / 3.
Definition: angles.h:64
CDegrees & operator+()
Definition: angles.h:362
bool operator>(const CDegrees &c_degrees) const
Definition: angles.h:433
static const CRadians PI_OVER_TWO
Set to PI / 2.
Definition: angles.h:59
#define Mod
Definition: general.h:66
CRadians()
Class constructor It initializes m_fValue to 0 radians.
Definition: angles.h:85