ARGoS  3
A parallel, multi-engine simulator for swarm robotics
core/utility/math/general.h
Go to the documentation of this file.
00001 
00009 #ifndef GENERAL_H
00010 #define GENERAL_H
00011 
00012 #include <argos3/core/utility/datatypes/datatypes.h>
00013 #include <vector>
00014 #include <utility>
00015 
00016 
00017 namespace argos {
00018     /****************************************/
00019     /****************************************/
00020 
00025     template<typename T> T Abs(const T& t_v) {
00026         if (t_v > T(0)) return t_v;
00027         if (t_v < T(0)) return -t_v;
00028         return T(0);
00029     }
00030 
00037     inline SInt32 Abs(SInt32 t_v) {
00038         if (t_v > 0) return t_v;
00039         if (t_v < 0) return -t_v;
00040         return 0;
00041     }
00042 
00048     inline Real Abs(Real t_v) {
00049         if (t_v > 0.0f) return t_v;
00050         if (t_v < 0.0f) return -t_v;
00051         return 0.0f;
00052     }
00053 
00054     /****************************************/
00055     /****************************************/
00056 
00057 #ifdef ARGOS_DOUBLE_PRECISION
00058 #define Log  ::log
00059 #define Sqrt ::sqrt
00060 #define Exp ::exp
00061 #else
00062 #define Log ::logf
00063 #define Sqrt ::sqrtf
00064 #define Exp ::expf
00065 #endif
00066 
00067     /****************************************/
00068     /****************************************/
00069 
00075     template<typename T> T Min(const T& t_v1, const T& t_v2) {
00076         return t_v1 < t_v2 ? t_v1 : t_v2;
00077     }
00078 
00084     template<typename T> T& Min(T& t_v1, T& t_v2) {
00085         return t_v1 < t_v2 ? t_v1 : t_v2;
00086     }
00087 
00093     template<typename T> T Max(const T& t_v1, const T& t_v2) {
00094         return t_v1 > t_v2 ? t_v1 : t_v2;
00095     }
00096 
00102     template<typename T> T& Max(T& t_v1, T& t_v2) {
00103         return t_v1 > t_v2 ? t_v1 : t_v2;
00104     }
00105 
00106     /****************************************/
00107     /****************************************/
00108 
00113     template<typename T> SInt32 Sign(const T& t_v) {
00114         if (t_v > T(0)) return 1;
00115         if (t_v < T(0)) return -1;
00116         return 0;
00117     }
00118 
00119     /****************************************/
00120     /****************************************/
00121 
00126     template<typename T> T Square(const T& t_v) {
00127         return t_v * t_v;
00128     }
00129 
00130     /****************************************/
00131     /****************************************/
00132 
00138     inline SInt32 Floor(Real f_value) {
00139         SInt32 nI = static_cast<SInt32> (f_value);
00140         if (f_value >= 0.0f) return nI;
00141         return nI - 1;
00142     }
00143 
00149     inline SInt32 Ceil(Real f_value) {
00150         SInt32 nI = static_cast<SInt32> (f_value);
00151         if (nI < f_value) return nI + 1;
00152         return nI;
00153     }
00154 
00160     inline SInt32 Round(Real f_value) {
00161         if (f_value > 0.0f) return Floor(f_value + 0.5f);
00162         return Ceil(f_value - 0.5f);
00163     }
00164 
00170     inline SInt32 RoundClosestToZero(Real f_value) {
00171         if (f_value > 0.0f) return Floor(f_value);
00172         return Ceil(f_value);
00173     }
00174 
00175     /****************************************/
00176     /****************************************/
00177 
00192     inline bool DoubleEqAbsolute(Real f_value1, Real f_value2, Real f_epsilon) {
00193         return Abs<Real > (f_value1 - f_value2) <= f_epsilon * Max<Real > (1.0f, Max<Real > (Abs<Real > (f_value1), Abs<Real > (f_value2)));
00194     }
00195 
00204     inline bool DoubleEq(Real f_value1, Real f_value2) {
00205         return Abs<Real > (f_value1 - f_value2) <= 0.0001f * Max<Real > (1.0f, Max<Real > (Abs<Real > (f_value1), Abs<Real > (f_value2)));
00206     }
00207 
00208     /****************************************/
00209     /****************************************/
00210 
00217     inline Real Interpolate(Real f_x, const std::vector<std::pair<Real, Real> >& c_points) {
00218         std::pair<Real, Real> cP0 = c_points.at(0);
00219         std::pair<Real, Real> cP1;
00220         for (UInt32 i = 1; i < c_points.size(); ++i) {
00221             cP1 = c_points.at(i);
00222             if (cP1.first >= f_x) {
00223                 break;
00224             } else if (i < c_points.size() - 1) {
00225                 cP0 = cP1;
00226             }
00227         }
00228         return (f_x * (cP0.second - cP1.second)) / (cP0.first - cP1.first)+(-cP1.first * cP0.second + cP0.first * cP1.second) / (cP0.first - cP1.first);;
00229     }
00230 }
00231 
00232 #endif