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