ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
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