All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Helpers.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 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_FLT_MAX std::numeric_limits<float>::max()
37 
39 #define GZ_FLT_MIN std::numeric_limits<float>::min()
40 
42 #define GZ_UINT32_MAX std::numeric_limits<uint32_t>::max()
43 
45 #define GZ_UINT32_MIN std::numeric_limits<uint32_t>::min()
46 
48 #define GZ_INT32_MAX std::numeric_limits<int32_t>::max()
49 
51 #define GZ_INT32_MIN std::numeric_limits<int32_t>::min()
52 
53 
54 namespace gazebo
55 {
56  namespace math
57  {
62 
64  static const double NAN_D = std::numeric_limits<double>::quiet_NaN();
65 
67  static const int NAN_I = std::numeric_limits<int>::quiet_NaN();
68 
73  template<typename T>
74  inline T clamp(T _v, T _min, T _max)
75  {
76  return std::max(std::min(_v, _max), _min);
77  }
78 
82  inline bool isnan(float _v)
83  {
84  return (boost::math::isnan)(_v);
85  }
86 
90  inline bool isnan(double _v)
91  {
92  return (boost::math::isnan)(_v);
93  }
94 
98  inline float fixnan(float _v)
99  {
100  return isnan(_v) || std::isinf(_v) ? 0.0f : _v;
101  }
102 
106  inline double fixnan(double _v)
107  {
108  return isnan(_v) || std::isinf(_v) ? 0.0 : _v;
109  }
110 
114  template<typename T>
115  inline T mean(const std::vector<T> &_values)
116  {
117  T sum = 0;
118  for (unsigned int i = 0; i < _values.size(); ++i)
119  sum += _values[i];
120  return sum / _values.size();
121  }
122 
126  template<typename T>
127  inline T variance(const std::vector<T> &_values)
128  {
129  T avg = mean<T>(_values);
130 
131  T sum = 0;
132  for (unsigned int i = 0; i < _values.size(); ++i)
133  sum += (_values[i] - avg) * (_values[i] - avg);
134  return sum / _values.size();
135  }
136 
140  template<typename T>
141  inline T max(const std::vector<T> &_values)
142  {
144  for (unsigned int i = 0; i < _values.size(); ++i)
145  if (_values[i] > max)
146  max = _values[i];
147  return max;
148  }
149 
153  template<typename T>
154  inline T min(const std::vector<T> &_values)
155  {
157  for (unsigned int i = 0; i < _values.size(); ++i)
158  if (_values[i] < min)
159  min = _values[i];
160  return min;
161  }
162 
167  template<typename T>
168  inline bool equal(const T &_a, const T &_b,
169  const T &_epsilon = 1e-6)
170  {
171  return std::fabs(_a - _b) <= _epsilon;
172  }
173 
178  template<typename T>
179  inline T precision(const T &_a, const unsigned int &_precision)
180  {
181  return boost::math::round(_a * pow(10, _precision)) / pow(10, _precision);
182  }
183 
187  inline bool isPowerOfTwo(unsigned int _x)
188  {
189  return ((_x != 0) && ((_x & (~_x + 1)) == _x));
190  }
191 
197  inline unsigned int roundUpPowerOfTwo(unsigned int _x)
198  {
199  if (_x == 0)
200  return 1;
201 
202  if (isPowerOfTwo(_x))
203  return _x;
204 
205  while (_x & (_x - 1))
206  _x = _x & (_x - 1);
207 
208  _x = _x << 1;
209 
210  return _x;
211  }
212 
216  inline int parseInt(const std::string& _input)
217  {
218  const char *p = _input.c_str();
219  if (!*p || *p == '?')
220  return NAN_I;
221 
222  int s = 1;
223  while (*p == ' ')
224  p++;
225 
226  if (*p == '-')
227  {
228  s = -1;
229  p++;
230  }
231 
232  double acc = 0;
233  while (*p >= '0' && *p <= '9')
234  acc = acc * 10 + *p++ - '0';
235 
236  if (*p)
237  {
238  std::cerr << "Invalid int numeric format[" << _input << "]\n";
239  return 0.0;
240  }
241 
242  return s * acc;
243  }
244 
249  inline double parseFloat(const std::string& _input)
250  {
251  const char *p = _input.c_str();
252  if (!*p || *p == '?')
253  return NAN_D;
254  int s = 1;
255  while (*p == ' ')
256  p++;
257 
258  if (*p == '-')
259  {
260  s = -1;
261  p++;
262  }
263 
264  double acc = 0;
265  while (*p >= '0' && *p <= '9')
266  acc = acc * 10 + *p++ - '0';
267 
268  if (*p == '.')
269  {
270  double k = 0.1;
271  p++;
272  while (*p >= '0' && *p <= '9')
273  {
274  acc += (*p++ - '0') * k;
275  k *= 0.1;
276  }
277  }
278  if (*p == 'e')
279  {
280  int es = 1;
281  int f = 0;
282  p++;
283  if (*p == '-')
284  {
285  es = -1;
286  p++;
287  }
288  else if (*p == '+')
289  {
290  es = 1;
291  p++;
292  }
293  while (*p >= '0' && *p <= '9')
294  f = f * 10 + *p++ - '0';
295 
296  acc *= pow(10, f*es);
297  }
298 
299  if (*p)
300  {
301  std::cerr << "Invalid double numeric format[" << _input << "]\n";
302  return 0.0;
303  }
304  return s * acc;
305  }
307  }
308 }
309 #endif