Helpers.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 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 #ifndef Q_MOC_RUN
21 #include <boost/math/special_functions/fpclassify.hpp>
22 #include <boost/math/special_functions/round.hpp>
23 #endif
24 #include <algorithm>
25 #include <cmath>
26 #include <limits>
27 #include <string>
28 #include <iostream>
29 #include <vector>
30 
32 #define GZ_DBL_MAX std::numeric_limits<double>::max()
33 
35 #define GZ_DBL_MIN std::numeric_limits<double>::min()
36 
38 #define GZ_DBL_INF std::numeric_limits<double>::infinity()
39 
41 #define GZ_FLT_MAX std::numeric_limits<float>::max()
42 
44 #define GZ_FLT_MIN std::numeric_limits<float>::min()
45 
47 #define GZ_UINT32_MAX std::numeric_limits<uint32_t>::max()
48 
50 #define GZ_UINT32_MIN std::numeric_limits<uint32_t>::min()
51 
53 #define GZ_INT32_MAX std::numeric_limits<int32_t>::max()
54 
56 #define GZ_INT32_MIN std::numeric_limits<int32_t>::min()
57 
58 
59 namespace gazebo
60 {
61  namespace math
62  {
67 
69  static const double NAN_D = std::numeric_limits<double>::quiet_NaN();
70 
72  static const int NAN_I = std::numeric_limits<int>::quiet_NaN();
73 
78  template<typename T>
79  inline T clamp(T _v, T _min, T _max)
80  {
81  return std::max(std::min(_v, _max), _min);
82  }
83 
87  inline bool isnan(float _v)
88  {
89  return (boost::math::isnan)(_v);
90  }
91 
95  inline bool isnan(double _v)
96  {
97  return (boost::math::isnan)(_v);
98  }
99 
103  inline float fixnan(float _v)
104  {
105  return isnan(_v) || std::isinf(_v) ? 0.0f : _v;
106  }
107 
111  inline double fixnan(double _v)
112  {
113  return isnan(_v) || std::isinf(_v) ? 0.0 : _v;
114  }
115 
119  template<typename T>
120  inline T mean(const std::vector<T> &_values)
121  {
122  T sum = 0;
123  for (unsigned int i = 0; i < _values.size(); ++i)
124  sum += _values[i];
125  return sum / _values.size();
126  }
127 
131  template<typename T>
132  inline T variance(const std::vector<T> &_values)
133  {
134  T avg = mean<T>(_values);
135 
136  T sum = 0;
137  for (unsigned int i = 0; i < _values.size(); ++i)
138  sum += (_values[i] - avg) * (_values[i] - avg);
139  return sum / _values.size();
140  }
141 
145  template<typename T>
146  inline T max(const std::vector<T> &_values)
147  {
149  for (unsigned int i = 0; i < _values.size(); ++i)
150  if (_values[i] > max)
151  max = _values[i];
152  return max;
153  }
154 
158  template<typename T>
159  inline T min(const std::vector<T> &_values)
160  {
162  for (unsigned int i = 0; i < _values.size(); ++i)
163  if (_values[i] < min)
164  min = _values[i];
165  return min;
166  }
167 
172  template<typename T>
173  inline bool equal(const T &_a, const T &_b,
174  const T &_epsilon = 1e-6)
175  {
176  return std::fabs(_a - _b) <= _epsilon;
177  }
178 
183  template<typename T>
184  inline T precision(const T &_a, const unsigned int &_precision)
185  {
186  if (!std::isinf(_a))
187  {
188  return boost::math::round(
189  _a * pow(10, _precision)) / pow(10, _precision);
190  }
191  else
192  {
193  return _a;
194  }
195  }
196 
200  inline bool isPowerOfTwo(unsigned int _x)
201  {
202  return ((_x != 0) && ((_x & (~_x + 1)) == _x));
203  }
204 
210  inline unsigned int roundUpPowerOfTwo(unsigned int _x)
211  {
212  if (_x == 0)
213  return 1;
214 
215  if (isPowerOfTwo(_x))
216  return _x;
217 
218  while (_x & (_x - 1))
219  _x = _x & (_x - 1);
220 
221  _x = _x << 1;
222 
223  return _x;
224  }
225 
229  inline int parseInt(const std::string& _input)
230  {
231  const char *p = _input.c_str();
232  if (!*p || *p == '?')
233  return NAN_I;
234 
235  int s = 1;
236  while (*p == ' ')
237  p++;
238 
239  if (*p == '-')
240  {
241  s = -1;
242  p++;
243  }
244 
245  double acc = 0;
246  while (*p >= '0' && *p <= '9')
247  acc = acc * 10 + *p++ - '0';
248 
249  if (*p)
250  {
251  std::cerr << "Invalid int numeric format[" << _input << "]\n";
252  return 0.0;
253  }
254 
255  return s * acc;
256  }
257 
262  inline double parseFloat(const std::string& _input)
263  {
264  const char *p = _input.c_str();
265  if (!*p || *p == '?')
266  return NAN_D;
267  int s = 1;
268  while (*p == ' ')
269  p++;
270 
271  if (*p == '-')
272  {
273  s = -1;
274  p++;
275  }
276 
277  double acc = 0;
278  while (*p >= '0' && *p <= '9')
279  acc = acc * 10 + *p++ - '0';
280 
281  if (*p == '.')
282  {
283  double k = 0.1;
284  p++;
285  while (*p >= '0' && *p <= '9')
286  {
287  acc += (*p++ - '0') * k;
288  k *= 0.1;
289  }
290  }
291  if (*p == 'e')
292  {
293  int es = 1;
294  int f = 0;
295  p++;
296  if (*p == '-')
297  {
298  es = -1;
299  p++;
300  }
301  else if (*p == '+')
302  {
303  es = 1;
304  p++;
305  }
306  while (*p >= '0' && *p <= '9')
307  f = f * 10 + *p++ - '0';
308 
309  acc *= pow(10, f*es);
310  }
311 
312  if (*p)
313  {
314  std::cerr << "Invalid double numeric format[" << _input << "]\n";
315  return 0.0;
316  }
317  return s * acc;
318  }
320  }
321 }
322 #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:210
Forward declarations for the common classes.
Definition: Animation.hh:33
int parseInt(const std::string &_input)
parse string into an integer
Definition: Helpers.hh:229
static const int NAN_I
Returns the representation of a quiet not a number (NAN)
Definition: Helpers.hh:72
double parseFloat(const std::string &_input)
parse string into float
Definition: Helpers.hh:262
bool isPowerOfTwo(unsigned int _x)
is this a power of 2?
Definition: Helpers.hh:200
T min(const std::vector< T > &_values)
get the minimum value of vector of values
Definition: Helpers.hh:159
static const double NAN_D
Returns the representation of a quiet not a number (NAN)
Definition: Helpers.hh:69
bool isnan(float _v)
check if a float is NaN
Definition: Helpers.hh:87
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:79
T variance(const std::vector< T > &_values)
get variance of vector of values
Definition: Helpers.hh:132
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:173
T mean(const std::vector< T > &_values)
get mean of vector of values
Definition: Helpers.hh:120
float fixnan(float _v)
Fix a nan value.
Definition: Helpers.hh:103
bool isnan(double _v)
check if a double is NaN
Definition: Helpers.hh:95
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:184
T max(const std::vector< T > &_values)
get the maximum value of vector of values
Definition: Helpers.hh:146