Helpers.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2016 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef _GAZEBO_MATH_FUNCTIONS_HH_
18 #define _GAZEBO_MATH_FUNCTIONS_HH_
19 
20 #include <boost/math/special_functions/fpclassify.hpp>
21 #include <boost/math/special_functions/round.hpp>
22 #include <algorithm>
23 #include <cmath>
24 #include <limits>
25 #include <string>
26 #include <iostream>
27 #include <vector>
28 
30 #define GZ_DBL_MAX std::numeric_limits<double>::max()
31 
33 #define GZ_DBL_MIN std::numeric_limits<double>::min()
34 
36 #define GZ_DBL_INF std::numeric_limits<double>::infinity()
37 
39 #define GZ_FLT_MAX std::numeric_limits<float>::max()
40 
42 #define GZ_FLT_MIN std::numeric_limits<float>::min()
43 
45 #define GZ_UINT32_MAX std::numeric_limits<uint32_t>::max()
46 
48 #define GZ_UINT32_MIN std::numeric_limits<uint32_t>::min()
49 
51 #define GZ_INT32_MAX std::numeric_limits<int32_t>::max()
52 
54 #define GZ_INT32_MIN std::numeric_limits<int32_t>::min()
55 
56 
57 namespace gazebo
58 {
59  namespace math
60  {
65 
67  static const double NAN_D = std::numeric_limits<double>::quiet_NaN();
68 
70  static const int NAN_I = std::numeric_limits<int>::quiet_NaN();
71 
76  template<typename T>
77  inline T clamp(T _v, T _min, T _max)
78  {
79  return std::max(std::min(_v, _max), _min);
80  }
81 
85  inline bool isnan(float _v)
86  {
87  return (boost::math::isnan)(_v);
88  }
89 
93  inline bool isnan(double _v)
94  {
95  return (boost::math::isnan)(_v);
96  }
97 
101  inline float fixnan(float _v)
102  {
103  return isnan(_v) || std::isinf(_v) ? 0.0f : _v;
104  }
105 
109  inline double fixnan(double _v)
110  {
111  return isnan(_v) || std::isinf(_v) ? 0.0 : _v;
112  }
113 
117  template<typename T>
118  inline T mean(const std::vector<T> &_values)
119  {
120  T sum = 0;
121  for (unsigned int i = 0; i < _values.size(); ++i)
122  sum += _values[i];
123  return sum / _values.size();
124  }
125 
129  template<typename T>
130  inline T variance(const std::vector<T> &_values)
131  {
132  T avg = mean<T>(_values);
133 
134  T sum = 0;
135  for (unsigned int i = 0; i < _values.size(); ++i)
136  sum += (_values[i] - avg) * (_values[i] - avg);
137  return sum / _values.size();
138  }
139 
143  template<typename T>
144  inline T max(const std::vector<T> &_values)
145  {
147  for (unsigned int i = 0; i < _values.size(); ++i)
148  if (_values[i] > max)
149  max = _values[i];
150  return max;
151  }
152 
156  template<typename T>
157  inline T min(const std::vector<T> &_values)
158  {
160  for (unsigned int i = 0; i < _values.size(); ++i)
161  if (_values[i] < min)
162  min = _values[i];
163  return min;
164  }
165 
170  template<typename T>
171  inline bool equal(const T &_a, const T &_b,
172  const T &_epsilon = 1e-6)
173  {
174  return std::fabs(_a - _b) <= _epsilon;
175  }
176 
181  template<typename T>
182  inline T precision(const T &_a, const unsigned int &_precision)
183  {
184  if (!std::isinf(_a))
185  {
186  return boost::math::round(
187  _a * pow(10, _precision)) / pow(10, _precision);
188  }
189  else
190  {
191  return _a;
192  }
193  }
194 
198  inline bool isPowerOfTwo(unsigned int _x)
199  {
200  return ((_x != 0) && ((_x & (~_x + 1)) == _x));
201  }
202 
208  inline unsigned int roundUpPowerOfTwo(unsigned int _x)
209  {
210  if (_x == 0)
211  return 1;
212 
213  if (isPowerOfTwo(_x))
214  return _x;
215 
216  while (_x & (_x - 1))
217  _x = _x & (_x - 1);
218 
219  _x = _x << 1;
220 
221  return _x;
222  }
223 
227  inline int parseInt(const std::string& _input)
228  {
229  const char *p = _input.c_str();
230  if (!*p || *p == '?')
231  return NAN_I;
232 
233  int s = 1;
234  while (*p == ' ')
235  p++;
236 
237  if (*p == '-')
238  {
239  s = -1;
240  p++;
241  }
242 
243  double acc = 0;
244  while (*p >= '0' && *p <= '9')
245  acc = acc * 10 + *p++ - '0';
246 
247  if (*p)
248  {
249  std::cerr << "Invalid int numeric format[" << _input << "]\n";
250  return 0.0;
251  }
252 
253  return s * acc;
254  }
255 
260  inline double parseFloat(const std::string& _input)
261  {
262  const char *p = _input.c_str();
263  if (!*p || *p == '?')
264  return NAN_D;
265  int s = 1;
266  while (*p == ' ')
267  p++;
268 
269  if (*p == '-')
270  {
271  s = -1;
272  p++;
273  }
274 
275  double acc = 0;
276  while (*p >= '0' && *p <= '9')
277  acc = acc * 10 + *p++ - '0';
278 
279  if (*p == '.')
280  {
281  double k = 0.1;
282  p++;
283  while (*p >= '0' && *p <= '9')
284  {
285  acc += (*p++ - '0') * k;
286  k *= 0.1;
287  }
288  }
289  if (*p == 'e')
290  {
291  int es = 1;
292  int f = 0;
293  p++;
294  if (*p == '-')
295  {
296  es = -1;
297  p++;
298  }
299  else if (*p == '+')
300  {
301  es = 1;
302  p++;
303  }
304  while (*p >= '0' && *p <= '9')
305  f = f * 10 + *p++ - '0';
306 
307  acc *= pow(10, f*es);
308  }
309 
310  if (*p)
311  {
312  std::cerr << "Invalid double numeric format[" << _input << "]\n";
313  return 0.0;
314  }
315  return s * acc;
316  }
318  }
319 }
320 #endif
unsigned int roundUpPowerOfTwo(unsigned int _x)
Get the smallest power of two that is greater or equal to a given value.
Definition: Helpers.hh:208
Forward declarations for the common classes.
Definition: Animation.hh:33
int parseInt(const std::string &_input)
parse string into an integer
Definition: Helpers.hh:227
static const int NAN_I
Returns the representation of a quiet not a number (NAN)
Definition: Helpers.hh:70
double parseFloat(const std::string &_input)
parse string into float
Definition: Helpers.hh:260
bool isPowerOfTwo(unsigned int _x)
is this a power of 2?
Definition: Helpers.hh:198
T min(const std::vector< T > &_values)
get the minimum value of vector of values
Definition: Helpers.hh:157
static const double NAN_D
Returns the representation of a quiet not a number (NAN)
Definition: Helpers.hh:67
bool isnan(float _v)
check if a float is NaN
Definition: Helpers.hh:85
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:77
T variance(const std::vector< T > &_values)
get variance of vector of values
Definition: Helpers.hh:130
bool equal(const T &_a, const T &_b, const T &_epsilon=1e-6)
check if two values are equal, within a tolerance
Definition: Helpers.hh:171
T mean(const std::vector< T > &_values)
get mean of vector of values
Definition: Helpers.hh:118
float fixnan(float _v)
Fix a nan value.
Definition: Helpers.hh:101
bool isnan(double _v)
check if a double is NaN
Definition: Helpers.hh:93
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:182
T max(const std::vector< T > &_values)
get the maximum value of vector of values
Definition: Helpers.hh:144