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 #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 #include "gazebo/util/system.hh"
29 
31 #define GZ_DBL_MAX gazebo::math::MAX_D
32 
34 #define GZ_DBL_MIN gazebo::math::MIN_D
35 
37 #define GZ_DBL_INF gazebo::math::INF_D
38 
40 #define GZ_FLT_MAX gazebo::math::MAX_F
41 
43 #define GZ_FLT_MIN gazebo::math::MIN_F
44 
46 #define GZ_UINT32_MAX gazebo::math::MAX_UI32
47 
49 #define GZ_UINT32_MIN gazebo::math::MIN_UI32
50 
52 #define GZ_INT32_MAX gazebo::math::MAX_I32
53 
55 #define GZ_INT32_MIN gazebo::math::MIN_I32
56 
57 
58 namespace gazebo
59 {
60  namespace math
61  {
66 
68  static const double
70  MAX_D = std::numeric_limits<double>::max();
71 
73  static const double
75  MIN_D = std::numeric_limits<double>::min();
76 
78  static const double
80  INF_D = std::numeric_limits<double>::infinity();
81 
83  static const double
85  NAN_D = std::numeric_limits<double>::quiet_NaN();
86 
88  static const float
90  MAX_F = std::numeric_limits<float>::max();
91 
93  static const float
95  MIN_F = std::numeric_limits<float>::min();
96 
98  static const uint32_t
100  MAX_UI32 = std::numeric_limits<uint32_t>::max();
101 
103  static const uint32_t
104  GAZEBO_DEPRECATED(8.0)
105  MIN_UI32 = std::numeric_limits<uint32_t>::min();
106 
108  static const int32_t
109  GAZEBO_DEPRECATED(8.0)
110  MAX_I32 = std::numeric_limits<int32_t>::max();
111 
113  static const int32_t
114  GAZEBO_DEPRECATED(8.0)
115  MIN_I32 = std::numeric_limits<int32_t>::min();
116 
118  static const int
119  GAZEBO_DEPRECATED(8.0)
120  NAN_I = std::numeric_limits<int>::quiet_NaN();
121 
127  template<typename T>
128  inline T
129  GAZEBO_DEPRECATED(8.0)
130  clamp(T _v, T _min, T _max)
131  {
132  return std::max(std::min(_v, _max), _min);
133  }
134 
139  inline bool
141  isnan(float _v)
142  {
143  return (boost::math::isnan)(_v);
144  }
145 
150  inline bool
151  GAZEBO_DEPRECATED(8.0)
152  isnan(double _v)
153  {
154  return (boost::math::isnan)(_v);
155  }
156 
157 #ifndef _WIN32
158 #pragma GCC diagnostic push
159 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
160 #endif
161  inline float
166  GAZEBO_DEPRECATED(8.0)
167  fixnan(float _v)
168  {
169  return isnan(_v) || std::isinf(_v) ? 0.0f : _v;
170  }
171 
176  inline double
177  GAZEBO_DEPRECATED(8.0)
178  fixnan(double _v)
179  {
180  return isnan(_v) || std::isinf(_v) ? 0.0 : _v;
181  }
182 #ifndef _WIN32
183 #pragma GCC diagnostic pop
184 #endif
185 
190  template<typename T>
191  inline T
192  GAZEBO_DEPRECATED(8.0)
193  mean(const std::vector<T> &_values)
194  {
195  T sum = 0;
196  for (unsigned int i = 0; i < _values.size(); ++i)
197  sum += _values[i];
198  return sum / _values.size();
199  }
200 
205  template<typename T>
206  inline T
207  GAZEBO_DEPRECATED(8.0)
208  variance(const std::vector<T> &_values)
209  {
210  T avg = mean<T>(_values);
211 
212  T sum = 0;
213  for (unsigned int i = 0; i < _values.size(); ++i)
214  sum += (_values[i] - avg) * (_values[i] - avg);
215  return sum / _values.size();
216  }
217 
222  template<typename T>
223  inline T
224  GAZEBO_DEPRECATED(8.0)
225  max(const std::vector<T> &_values)
226  {
227  T max = std::numeric_limits<T>::min();
228  for (unsigned int i = 0; i < _values.size(); ++i)
229  if (_values[i] > max)
230  max = _values[i];
231  return max;
232  }
233 
238  template<typename T>
239  inline T
240  GAZEBO_DEPRECATED(8.0)
241  min(const std::vector<T> &_values)
242  {
243  T min = std::numeric_limits<T>::max();
244  for (unsigned int i = 0; i < _values.size(); ++i)
245  if (_values[i] < min)
246  min = _values[i];
247  return min;
248  }
249 
255  template<typename T>
256  inline bool
257  GAZEBO_DEPRECATED(8.0)
258  equal(const T &_a, const T &_b, const T &_epsilon = 1e-6)
259  {
260  return std::fabs(_a - _b) <= _epsilon;
261  }
262 
268  template<typename T>
269  inline T
270  GAZEBO_DEPRECATED(8.0)
271  precision(const T &_a, const unsigned int &_precision)
272  {
273  if (!std::isinf(_a))
274  {
275  return boost::math::round(
276  _a * pow(10, _precision)) / pow(10, _precision);
277  }
278  else
279  {
280  return _a;
281  }
282  }
283 
288  inline bool
289  GAZEBO_DEPRECATED(8.0)
290  isPowerOfTwo(unsigned int _x)
291  {
292  return ((_x != 0) && ((_x & (~_x + 1)) == _x));
293  }
294 
301  inline unsigned int
302  GAZEBO_DEPRECATED(8.0)
303  roundUpPowerOfTwo(unsigned int _x)
304  {
305  if (_x == 0)
306  return 1;
307 
308 #ifndef _WIN32
309 #pragma GCC diagnostic push
310 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
311 #endif
312  if (isPowerOfTwo(_x))
313  return _x;
314 #ifndef _WIN32
315 #pragma GCC diagnostic pop
316 #endif
317 
318  while (_x & (_x - 1))
319  _x = _x & (_x - 1);
320 
321  _x = _x << 1;
322 
323  return _x;
324  }
325 
330  inline int
331  GAZEBO_DEPRECATED(8.0)
332  parseInt(const std::string& _input)
333  {
334  const char *p = _input.c_str();
335 #ifndef _WIN32
336 #pragma GCC diagnostic push
337 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
338 #endif
339  if (!*p || *p == '?')
340  return NAN_I;
341 #ifndef _WIN32
342 #pragma GCC diagnostic pop
343 #endif
344 
345  int s = 1;
346  while (*p == ' ')
347  p++;
348 
349  if (*p == '-')
350  {
351  s = -1;
352  p++;
353  }
354 
355  int acc = 0;
356  while (*p >= '0' && *p <= '9')
357  acc = acc * 10 + *p++ - '0';
358 
359  if (*p)
360  {
361  std::cerr << "Invalid int numeric format[" << _input << "]\n";
362  return 0.0;
363  }
364 
365  return s * acc;
366  }
367 
373  inline double
374  GAZEBO_DEPRECATED(8.0)
375  parseFloat(const std::string& _input)
376  {
377  const char *p = _input.c_str();
378 #ifndef _WIN32
379 #pragma GCC diagnostic push
380 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
381 #endif
382  if (!*p || *p == '?')
383  return NAN_D;
384 #ifndef _WIN32
385 #pragma GCC diagnostic pop
386 #endif
387  int s = 1;
388  while (*p == ' ')
389  p++;
390 
391  if (*p == '-')
392  {
393  s = -1;
394  p++;
395  }
396 
397  double acc = 0;
398  while (*p >= '0' && *p <= '9')
399  acc = acc * 10 + *p++ - '0';
400 
401  if (*p == '.')
402  {
403  double k = 0.1;
404  p++;
405  while (*p >= '0' && *p <= '9')
406  {
407  acc += (*p++ - '0') * k;
408  k *= 0.1;
409  }
410  }
411  if (*p == 'e')
412  {
413  int es = 1;
414  int f = 0;
415  p++;
416  if (*p == '-')
417  {
418  es = -1;
419  p++;
420  }
421  else if (*p == '+')
422  {
423  es = 1;
424  p++;
425  }
426  while (*p >= '0' && *p <= '9')
427  f = f * 10 + *p++ - '0';
428 
429  acc *= pow(10, f*es);
430  }
431 
432  if (*p)
433  {
434  std::cerr << "Invalid double numeric format[" << _input << "]\n";
435  return 0.0;
436  }
437  return s * acc;
438  }
440  }
441 }
442 #endif
T T _min
Definition: Helpers.hh:130
T T T _max
Definition: Helpers.hh:131
Forward declarations for the common classes.
Definition: Animation.hh:33
static const double GAZEBO_DEPRECATED(8.0) MAX_D
Double maximum value. This value will be similar to 1.79769e+308.
Definition: Helpers.hh:140
bool const T & _b
Definition: Helpers.hh:258