17 #ifndef _GAZEBO_MATH_FUNCTIONS_HH_
18 #define _GAZEBO_MATH_FUNCTIONS_HH_
20 #include <boost/math/special_functions/fpclassify.hpp>
28 #define GZ_DBL_MAX std::numeric_limits<double>::max()
29 #define GZ_DBL_MIN std::numeric_limits<double>::min()
31 #define GZ_FLT_MAX std::numeric_limits<float>::max()
32 #define GZ_FLT_MIN std::numeric_limits<float>::min()
44 static const double NAN_D = std::numeric_limits<double>::quiet_NaN();
47 static const int NAN_I = std::numeric_limits<int>::quiet_NaN();
54 inline T
clamp(T _v, T _min, T _max)
79 inline T
mean(
const std::vector<T> &_values)
82 for (
unsigned int i = 0; i < _values.size(); ++i)
84 return sum / _values.size();
91 inline T
variance(
const std::vector<T> &_values)
93 T avg = mean<T>(_values);
96 for (
unsigned int i = 0; i < _values.size(); ++i)
97 sum += (_values[i] - avg) * (_values[i] - avg);
98 return sum / _values.size();
105 inline T
max(
const std::vector<T> &_values)
108 for (
unsigned int i = 0; i < _values.size(); ++i)
109 if (_values[i] > max)
118 inline T
min(
const std::vector<T> &_values)
121 for (
unsigned int i = 0; i < _values.size(); ++i)
122 if (_values[i] < min)
132 inline bool equal(
const T &_a,
const T &_b,
133 const T &_epsilon = 1e-6)
135 return std::fabs(_a - _b) <= _epsilon;
143 inline T
precision(
const T &_a,
const unsigned int &_precision)
145 return round(_a * pow(10, _precision)) / pow(10, _precision);
153 return ((_x != 0) && ((_x & (~_x + 1)) == _x));
161 const char *p = _input.c_str();
162 if (!*p || *p ==
'?')
176 while (*p >=
'0' && *p <=
'9')
177 acc = acc * 10 + *p++ -
'0';
181 std::cerr <<
"Invalid int numeric format[" << _input <<
"]\n";
194 const char *p = _input.c_str();
195 if (!*p || *p ==
'?')
208 while (*p >=
'0' && *p <=
'9')
209 acc = acc * 10 + *p++ -
'0';
215 while (*p >=
'0' && *p <=
'9')
217 acc += (*p++ -
'0') * k;
236 while (*p >=
'0' && *p <=
'9')
237 f = f * 10 + *p++ -
'0';
239 acc *= pow(10, f*es);
244 std::cerr <<
"Invalid double numeric format[" << _input <<
"]\n";