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 _IGNITION_MATH_FUNCTIONS_HH_
18 #define _IGNITION_MATH_FUNCTIONS_HH_
19 
20 #ifndef _USE_MATH_DEFINES
21 # define _USE_MATH_DEFINES
22 #endif
23 
24 #include <cmath>
25 #include <algorithm>
26 #include <limits>
27 #include <string>
28 #include <iostream>
29 #include <vector>
30 
32 #define IGN_DBL_MAX std::numeric_limits<double>::max()
33 
35 #define IGN_DBL_MIN std::numeric_limits<double>::min()
36 
38 #define IGN_DBL_LOW std::numeric_limits<double>::lowest()
39 
41 #define IGN_DBL_INF std::numeric_limits<double>::infinity()
42 
44 #define IGN_FLT_MAX std::numeric_limits<float>::max()
45 
47 #define IGN_FLT_MIN std::numeric_limits<float>::min()
48 
50 #define IGN_FLT_LOW std::numeric_limits<float>::lowest()
51 
53 #define IGN_UINT32_MAX std::numeric_limits<uint32_t>::max()
54 
56 #define IGN_UINT32_MIN std::numeric_limits<uint32_t>::min()
57 
60 #define IGN_UINT32_LOW std::numeric_limits<uint32_t>::lowest()
61 
63 #define IGN_INT32_MAX std::numeric_limits<int32_t>::max()
64 
66 #define IGN_INT32_MIN std::numeric_limits<int32_t>::min()
67 
70 #define IGN_INT32_LOW std::numeric_limits<int32_t>::lowest()
71 
74 #ifdef M_PI
75 #define IGN_PI M_PI
76 #define IGN_PI_2 M_PI_2
77 #define IGN_PI_4 M_PI_4
78 #else
79 #define IGN_PI 3.14159265358979323846
80 #define IGN_PI_2 1.57079632679489661923
81 #define IGN_PI_4 0.78539816339744830962
82 #endif
83 
87 #if defined __FLT_EVAL_METHOD__ && __FLT_EVAL_METHOD__ == 2
88 #define IGN_FP_VOLATILE volatile
89 #else
90 #define IGN_FP_VOLATILE
91 #endif
92 
95 #define IGN_SPHERE_VOLUME(_radius) (4.0*IGN_PI*std::pow(_radius, 3)/3.0)
96 
100 #define IGN_CYLINDER_VOLUME(_r, _l) (_l * IGN_PI * std::pow(_r, 2))
101 
106 #define IGN_BOX_VOLUME(_x, _y, _z) (_x *_y * _z)
107 
110 #define IGN_BOX_VOLUME_V(_v) (_v.X() *_v.Y() * _v.Z())
111 
112 namespace ignition
113 {
115  namespace math
116  {
118  static const double NAN_D = std::numeric_limits<double>::quiet_NaN();
119 
121  static const float NAN_F = std::numeric_limits<float>::quiet_NaN();
122 
124  static const int NAN_I = std::numeric_limits<int>::quiet_NaN();
125 
130  template<typename T>
131  inline T clamp(T _v, T _min, T _max)
132  {
133  return std::max(std::min(_v, _max), _min);
134  }
135 
139  inline bool isnan(float _v)
140  {
141  return (std::isnan)(_v);
142  }
143 
147  inline bool isnan(double _v)
148  {
149  return (std::isnan)(_v);
150  }
151 
155  inline float fixnan(float _v)
156  {
157  return isnan(_v) || std::isinf(_v) ? 0.0f : _v;
158  }
159 
163  inline double fixnan(double _v)
164  {
165  return isnan(_v) || std::isinf(_v) ? 0.0 : _v;
166  }
167 
171  inline bool isEven(const int _v)
172  {
173  return !(_v % 2);
174  }
175 
179  inline bool isEven(const unsigned int _v)
180  {
181  return !(_v % 2);
182  }
183 
187  inline bool isOdd(const int _v)
188  {
189  return (_v % 2) != 0;
190  }
191 
195  inline bool isOdd(const unsigned int _v)
196  {
197  return (_v % 2) != 0;
198  }
199 
203  template<typename T>
204  inline T mean(const std::vector<T> &_values)
205  {
206  T sum = 0;
207  for (unsigned int i = 0; i < _values.size(); ++i)
208  sum += _values[i];
209  return sum / _values.size();
210  }
211 
215  template<typename T>
216  inline T variance(const std::vector<T> &_values)
217  {
218  T avg = mean<T>(_values);
219 
220  T sum = 0;
221  for (unsigned int i = 0; i < _values.size(); ++i)
222  sum += (_values[i] - avg) * (_values[i] - avg);
223  return sum / _values.size();
224  }
225 
229  template<typename T>
230  inline T max(const std::vector<T> &_values)
231  {
233  for (unsigned int i = 0; i < _values.size(); ++i)
234  if (_values[i] > max)
235  max = _values[i];
236  return max;
237  }
238 
242  template<typename T>
243  inline T min(const std::vector<T> &_values)
244  {
246  for (unsigned int i = 0; i < _values.size(); ++i)
247  if (_values[i] < min)
248  min = _values[i];
249  return min;
250  }
251 
256  template<typename T>
257  inline bool equal(const T &_a, const T &_b,
258  const T &_epsilon = 1e-6)
259  {
260  IGN_FP_VOLATILE T diff = std::abs(_a - _b);
261  return diff <= _epsilon;
262  }
263 
268  template<typename T>
269  inline T precision(const T &_a, const unsigned int &_precision)
270  {
271  return std::round(_a * std::pow(10, _precision))
272  / std::pow(10, _precision);
273  }
274 
278  template<typename T>
279  inline void sort2(T &_a, T &_b)
280  {
281  using std::swap;
282  if (_b < _a)
283  swap(_a, _b);
284  }
285 
290  template<typename T>
291  inline void sort3(T &_a, T &_b, T &_c)
292  {
293  // _a <= _b
294  sort2(_a, _b);
295  // _a <= _c, _b <= _c
296  sort2(_b, _c);
297  // _a <= _b <= _c
298  sort2(_a, _b);
299  }
300 
304  inline bool isPowerOfTwo(unsigned int _x)
305  {
306  return ((_x != 0) && ((_x & (~_x + 1)) == _x));
307  }
308 
314  inline unsigned int roundUpPowerOfTwo(unsigned int _x)
315  {
316  if (_x == 0)
317  return 1;
318 
319  if (isPowerOfTwo(_x))
320  return _x;
321 
322  while (_x & (_x - 1))
323  _x = _x & (_x - 1);
324 
325  _x = _x << 1;
326 
327  return _x;
328  }
329 
333  inline int parseInt(const std::string &_input)
334  {
335  const char *p = _input.c_str();
336  if (!*p || *p == '?')
337  return NAN_I;
338 
339  int s = 1;
340  while (*p == ' ')
341  p++;
342 
343  if (*p == '-')
344  {
345  s = -1;
346  p++;
347  }
348 
349  double acc = 0;
350  while (*p >= '0' && *p <= '9')
351  acc = acc * 10 + *p++ - '0';
352 
353  if (*p)
354  {
355  std::cerr << "Invalid int numeric format[" << _input << "]\n";
356  return NAN_I;
357  }
358 
359  return static_cast<int>(s * acc);
360  }
361 
366  inline double parseFloat(const std::string &_input)
367  {
368  const char *p = _input.c_str();
369  if (!*p || *p == '?')
370  return NAN_D;
371  int s = 1;
372  while (*p == ' ')
373  p++;
374 
375  if (*p == '-')
376  {
377  s = -1;
378  p++;
379  }
380 
381  double acc = 0;
382  while (*p >= '0' && *p <= '9')
383  acc = acc * 10 + *p++ - '0';
384 
385  if (*p == '.')
386  {
387  double k = 0.1;
388  p++;
389  while (*p >= '0' && *p <= '9')
390  {
391  acc += (*p++ - '0') * k;
392  k *= 0.1;
393  }
394  }
395  if (*p == 'e')
396  {
397  int es = 1;
398  int f = 0;
399  p++;
400  if (*p == '-')
401  {
402  es = -1;
403  p++;
404  }
405  else if (*p == '+')
406  {
407  es = 1;
408  p++;
409  }
410  while (*p >= '0' && *p <= '9')
411  f = f * 10 + *p++ - '0';
412 
413  acc *= pow(10, f*es);
414  }
415 
416  if (*p)
417  {
418  std::cerr << "Invalid double numeric format[" << _input << "]\n";
419  return NAN_D;
420  }
421  return s * acc;
422  }
423  }
424 }
425 
434 #if defined _WIN32 || defined __CYGWIN__
435  #ifdef BUILDING_DLL
436  #ifdef __GNUC__
437  #define IGNITION_VISIBLE __attribute__ ((dllexport))
438  #else
439  #define IGNITION_VISIBLE __declspec(dllexport)
440  #endif
441  #else
442  #ifdef __GNUC__
443  #define IGNITION_VISIBLE __attribute__ ((dllimport))
444  #else
445  #define IGNITION_VISIBLE __declspec(dllimport)
446  #endif
447  #endif
448  #define IGNITION_HIDDEN
449 #else
450  #if __GNUC__ >= 4
451  #define IGNITION_VISIBLE __attribute__ ((visibility ("default")))
452  #define IGNITION_HIDDEN __attribute__ ((visibility ("hidden")))
453  #else
454  #define IGNITION_VISIBLE
455  #define IGNITION_HIDDEN
456  #endif
457 #endif
458 
459 #endif
static const float NAN_F
Returns the representation of a quiet not a number (NAN)
Definition: Helpers.hh:121
static const double NAN_D
Returns the representation of a quiet not a number (NAN)
Definition: Helpers.hh:118
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:269
T mean(const std::vector< T > &_values)
get mean of vector of values
Definition: Helpers.hh:204
bool isnan(float _v)
check if a float is NaN
Definition: Helpers.hh:139
unsigned int roundUpPowerOfTwo(unsigned int _x)
Get the smallest power of two that is greater or equal to a given value.
Definition: Helpers.hh:314
T max(const std::vector< T > &_values)
get the maximum value of vector of values
Definition: Helpers.hh:230
bool isOdd(const int _v)
Check if parameter is odd.
Definition: Helpers.hh:187
T variance(const std::vector< T > &_values)
get variance of vector of values
Definition: Helpers.hh:216
#define IGN_FP_VOLATILE
Define IGN_FP_VOLATILE for FP equality comparisons Use volatile parameters when checking floating poi...
Definition: Helpers.hh:90
bool isPowerOfTwo(unsigned int _x)
Is this a power of 2?
Definition: Helpers.hh:304
bool isEven(const int _v)
Check if parameter is even.
Definition: Helpers.hh:171
static const int NAN_I
Returns the representation of a quiet not a number (NAN)
Definition: Helpers.hh:124
double parseFloat(const std::string &_input)
parse string into float
Definition: Helpers.hh:366
void sort2(T &_a, T &_b)
Sort two numbers, such that _a <= _b.
Definition: Helpers.hh:279
Definition: AffineException.hh:30
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:257
float fixnan(float _v)
Fix a nan value.
Definition: Helpers.hh:155
int parseInt(const std::string &_input)
parse string into an integer
Definition: Helpers.hh:333
T min(const std::vector< T > &_values)
get the minimum value of vector of values
Definition: Helpers.hh:243
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:131
bool isnan(double _v)
check if a double is NaN
Definition: Helpers.hh:147
void sort3(T &_a, T &_b, T &_c)
Sort three numbers, such that _a <= _b <= _c.
Definition: Helpers.hh:291