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 #include <cmath>
21 #include <algorithm>
22 #include <limits>
23 #include <string>
24 #include <iostream>
25 #include <vector>
26 #include <tuple>
27 #include <cstdint>
28 
30 #define IGN_DBL_MAX std::numeric_limits<double>::max()
31 
33 #define IGN_DBL_MIN std::numeric_limits<double>::min()
34 
36 #define IGN_DBL_LOW std::numeric_limits<double>::lowest()
37 
39 #define IGN_DBL_INF std::numeric_limits<double>::infinity()
40 
42 #define IGN_FLT_MAX std::numeric_limits<float>::max()
43 
45 #define IGN_FLT_MIN std::numeric_limits<float>::min()
46 
48 #define IGN_FLT_LOW std::numeric_limits<float>::lowest()
49 
51 #define IGN_FLT_INF std::numeric_limits<float>::infinity()
52 
54 #define IGN_UINT16_MAX std::numeric_limits<uint16_t>::max()
55 
57 #define IGN_UINT16_MIN std::numeric_limits<uint16_t>::min()
58 
61 #define IGN_UINT16_LOW std::numeric_limits<uint16_t>::lowest()
62 
64 #define IGN_UINT16_INF std::numeric_limits<uint16_t>::infinity()
65 
67 #define IGN_INT16_MAX std::numeric_limits<int16_t>::max()
68 
70 #define IGN_INT16_MIN std::numeric_limits<int16_t>::min()
71 
74 #define IGN_INT16_LOW std::numeric_limits<int16_t>::lowest()
75 
77 #define IGN_INT16_INF std::numeric_limits<int16_t>::infinity()
78 
80 #define IGN_UINT32_MAX std::numeric_limits<uint32_t>::max()
81 
83 #define IGN_UINT32_MIN std::numeric_limits<uint32_t>::min()
84 
87 #define IGN_UINT32_LOW std::numeric_limits<uint32_t>::lowest()
88 
90 #define IGN_UINT32_INF std::numeric_limits<uint32_t>::infinity()
91 
93 #define IGN_INT32_MAX std::numeric_limits<int32_t>::max()
94 
96 #define IGN_INT32_MIN std::numeric_limits<int32_t>::min()
97 
100 #define IGN_INT32_LOW std::numeric_limits<int32_t>::lowest()
101 
103 #define IGN_INT32_INF std::numeric_limits<int32_t>::infinity()
104 
106 #define IGN_UINT64_MAX std::numeric_limits<uint64_t>::max()
107 
109 #define IGN_UINT64_MIN std::numeric_limits<uint64_t>::min()
110 
113 #define IGN_UINT64_LOW std::numeric_limits<uint64_t>::lowest()
114 
116 #define IGN_UINT64_INF std::numeric_limits<uint64_t>::infinity()
117 
119 #define IGN_INT64_MAX std::numeric_limits<int64_t>::max()
120 
122 #define IGN_INT64_MIN std::numeric_limits<int64_t>::min()
123 
126 #define IGN_INT64_LOW std::numeric_limits<int64_t>::lowest()
127 
129 #define IGN_INT64_INF std::numeric_limits<int64_t>::infinity()
130 
133 #ifdef M_PI
134 #define IGN_PI M_PI
135 #define IGN_PI_2 M_PI_2
136 #define IGN_PI_4 M_PI_4
137 #define IGN_SQRT2 M_SQRT2
138 #else
139 #define IGN_PI 3.14159265358979323846
140 #define IGN_PI_2 1.57079632679489661923
141 #define IGN_PI_4 0.78539816339744830962
142 #define IGN_SQRT2 1.41421356237309504880
143 #endif
144 
148 #if defined __FLT_EVAL_METHOD__ && __FLT_EVAL_METHOD__ == 2
149 #define IGN_FP_VOLATILE volatile
150 #else
151 #define IGN_FP_VOLATILE
152 #endif
153 
156 #define IGN_SPHERE_VOLUME(_radius) (4.0*IGN_PI*std::pow(_radius, 3)/3.0)
157 
161 #define IGN_CYLINDER_VOLUME(_r, _l) (_l * IGN_PI * std::pow(_r, 2))
162 
167 #define IGN_BOX_VOLUME(_x, _y, _z) (_x *_y * _z)
168 
171 #define IGN_BOX_VOLUME_V(_v) (_v.X() *_v.Y() * _v.Z())
172 
181 #if defined _WIN32 || defined __CYGWIN__
182  #ifdef BUILDING_DLL
183  #ifdef __GNUC__
184  #define IGNITION_VISIBLE __attribute__ ((dllexport))
185  #else
186  #define IGNITION_VISIBLE __declspec(dllexport)
187  #endif
188  #else
189  #ifdef __GNUC__
190  #define IGNITION_VISIBLE __attribute__ ((dllimport))
191  #else
192  #define IGNITION_VISIBLE __declspec(dllimport)
193  #endif
194  #endif
195  #define IGNITION_HIDDEN
196 #else
197  #if __GNUC__ >= 4
198  #define IGNITION_VISIBLE __attribute__ ((visibility ("default")))
199  #define IGNITION_HIDDEN __attribute__ ((visibility ("hidden")))
200  #else
201  #define IGNITION_VISIBLE
202  #define IGNITION_HIDDEN
203  #endif
204 #endif
205 
206 namespace ignition
207 {
209  namespace math
210  {
212  static const double NAN_D = std::numeric_limits<double>::quiet_NaN();
213 
215  static const float NAN_F = std::numeric_limits<float>::quiet_NaN();
216 
218  static const int NAN_I = std::numeric_limits<int>::quiet_NaN();
219 
224  template<typename T>
225  inline T clamp(T _v, T _min, T _max)
226  {
227  return std::max(std::min(_v, _max), _min);
228  }
229 
233  inline bool isnan(float _v)
234  {
235  return (std::isnan)(_v);
236  }
237 
241  inline bool isnan(double _v)
242  {
243  return (std::isnan)(_v);
244  }
245 
249  inline float fixnan(float _v)
250  {
251  return isnan(_v) || std::isinf(_v) ? 0.0f : _v;
252  }
253 
257  inline double fixnan(double _v)
258  {
259  return isnan(_v) || std::isinf(_v) ? 0.0 : _v;
260  }
261 
265  inline bool isEven(const int _v)
266  {
267  return !(_v % 2);
268  }
269 
273  inline bool isEven(const unsigned int _v)
274  {
275  return !(_v % 2);
276  }
277 
281  inline bool isOdd(const int _v)
282  {
283  return (_v % 2) != 0;
284  }
285 
289  inline bool isOdd(const unsigned int _v)
290  {
291  return (_v % 2) != 0;
292  }
293 
297  template<typename T>
298  inline T mean(const std::vector<T> &_values)
299  {
300  T sum = 0;
301  for (unsigned int i = 0; i < _values.size(); ++i)
302  sum += _values[i];
303  return sum / _values.size();
304  }
305 
309  template<typename T>
310  inline T variance(const std::vector<T> &_values)
311  {
312  T avg = mean<T>(_values);
313 
314  T sum = 0;
315  for (unsigned int i = 0; i < _values.size(); ++i)
316  sum += (_values[i] - avg) * (_values[i] - avg);
317  return sum / _values.size();
318  }
319 
323  template<typename T>
324  inline T max(const std::vector<T> &_values)
325  {
327  for (unsigned int i = 0; i < _values.size(); ++i)
328  if (_values[i] > max)
329  max = _values[i];
330  return max;
331  }
332 
336  template<typename T>
337  inline T min(const std::vector<T> &_values)
338  {
340  for (unsigned int i = 0; i < _values.size(); ++i)
341  if (_values[i] < min)
342  min = _values[i];
343  return min;
344  }
345 
350  template<typename T>
351  inline bool equal(const T &_a, const T &_b,
352  const T &_epsilon = 1e-6)
353  {
354  IGN_FP_VOLATILE T diff = std::abs(_a - _b);
355  return diff <= _epsilon;
356  }
357 
362  template<typename T>
363  inline T precision(const T &_a, const unsigned int &_precision)
364  {
365  auto p = std::pow(10, _precision);
366  return static_cast<T>(std::round(_a * p) / p);
367  }
368 
372  template<typename T>
373  inline void sort2(T &_a, T &_b)
374  {
375  using std::swap;
376  if (_b < _a)
377  swap(_a, _b);
378  }
379 
384  template<typename T>
385  inline void sort3(T &_a, T &_b, T &_c)
386  {
387  // _a <= _b
388  sort2(_a, _b);
389  // _a <= _c, _b <= _c
390  sort2(_b, _c);
391  // _a <= _b <= _c
392  sort2(_a, _b);
393  }
394 
398  inline bool isPowerOfTwo(unsigned int _x)
399  {
400  return ((_x != 0) && ((_x & (~_x + 1)) == _x));
401  }
402 
408  inline unsigned int roundUpPowerOfTwo(unsigned int _x)
409  {
410  if (_x == 0)
411  return 1;
412 
413  if (isPowerOfTwo(_x))
414  return _x;
415 
416  while (_x & (_x - 1))
417  _x = _x & (_x - 1);
418 
419  _x = _x << 1;
420 
421  return _x;
422  }
423 
427  inline int parseInt(const std::string &_input)
428  {
429  const char *p = _input.c_str();
430  if (!*p || *p == '?')
431  return NAN_I;
432 
433  int s = 1;
434  while (*p == ' ')
435  p++;
436 
437  if (*p == '-')
438  {
439  s = -1;
440  p++;
441  }
442 
443  int acc = 0;
444  while (*p >= '0' && *p <= '9')
445  acc = acc * 10 + *p++ - '0';
446 
447  if (*p)
448  {
449  std::cerr << "Invalid int numeric format[" << _input << "]\n";
450  return NAN_I;
451  }
452 
453  return s * acc;
454  }
455 
460  inline double parseFloat(const std::string &_input)
461  {
462  const char *p = _input.c_str();
463  if (!*p || *p == '?')
464  return NAN_D;
465  int s = 1;
466  while (*p == ' ')
467  p++;
468 
469  if (*p == '-')
470  {
471  s = -1;
472  p++;
473  }
474 
475  double acc = 0;
476  while (*p >= '0' && *p <= '9')
477  acc = acc * 10 + *p++ - '0';
478 
479  if (*p == '.')
480  {
481  double k = 0.1;
482  p++;
483  while (*p >= '0' && *p <= '9')
484  {
485  acc += (*p++ - '0') * k;
486  k *= 0.1;
487  }
488  }
489  if (*p == 'e')
490  {
491  int es = 1;
492  int f = 0;
493  p++;
494  if (*p == '-')
495  {
496  es = -1;
497  p++;
498  }
499  else if (*p == '+')
500  {
501  es = 1;
502  p++;
503  }
504  while (*p >= '0' && *p <= '9')
505  f = f * 10 + *p++ - '0';
506 
507  acc *= pow(10, f*es);
508  }
509 
510  if (*p)
511  {
512  std::cerr << "Invalid double numeric format[" << _input << "]\n";
513  return NAN_D;
514  }
515  return s * acc;
516  }
517 
518 
519  // Degrade precision on Windows, which cannot handle 'long double'
520  // values properly. See the implementation of Unpair.
521  // 32 bit ARM processors also define 'long double' to be the same
522  // size as 'double', and must also be degraded
523 #if defined _MSC_VER || defined __arm__
524  using PairInput = uint16_t;
525  using PairOutput = uint32_t;
526 #else
527  using PairInput = uint32_t;
528  using PairOutput = uint64_t;
529 #endif
530 
540  PairOutput IGNITION_VISIBLE Pair(const PairInput _a, const PairInput _b);
541 
553  std::tuple<PairInput, PairInput> IGNITION_VISIBLE Unpair(
554  const PairOutput _key);
555  }
556 }
557 
558 #endif
static const float NAN_F
Returns the representation of a quiet not a number (NAN)
Definition: Helpers.hh:215
std::tuple< PairInput, PairInput > Unpair(const PairOutput _key)
The reverse of the Pair function.
#define IGNITION_VISIBLE
Use to represent "symbol visible" if supported.
Definition: Helpers.hh:201
static const double NAN_D
Returns the representation of a quiet not a number (NAN)
Definition: Helpers.hh:212
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:363
T mean(const std::vector< T > &_values)
get mean of vector of values
Definition: Helpers.hh:298
bool isnan(float _v)
check if a float is NaN
Definition: Helpers.hh:233
unsigned int roundUpPowerOfTwo(unsigned int _x)
Get the smallest power of two that is greater or equal to a given value.
Definition: Helpers.hh:408
T max(const std::vector< T > &_values)
get the maximum value of vector of values
Definition: Helpers.hh:324
uint64_t PairOutput
Definition: Helpers.hh:528
bool isOdd(const int _v)
Check if parameter is odd.
Definition: Helpers.hh:281
T variance(const std::vector< T > &_values)
get variance of vector of values
Definition: Helpers.hh:310
#define IGN_FP_VOLATILE
Define IGN_FP_VOLATILE for FP equality comparisons Use volatile parameters when checking floating poi...
Definition: Helpers.hh:151
bool isPowerOfTwo(unsigned int _x)
Is this a power of 2?
Definition: Helpers.hh:398
PairOutput Pair(const PairInput _a, const PairInput _b)
A pairing function that maps two values to a unique third value.
uint32_t PairInput
Definition: Helpers.hh:527
bool isEven(const int _v)
Check if parameter is even.
Definition: Helpers.hh:265
static const int NAN_I
Returns the representation of a quiet not a number (NAN)
Definition: Helpers.hh:218
double parseFloat(const std::string &_input)
parse string into float
Definition: Helpers.hh:460
void sort2(T &_a, T &_b)
Sort two numbers, such that _a <= _b.
Definition: Helpers.hh:373
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:351
float fixnan(float _v)
Fix a nan value.
Definition: Helpers.hh:249
int parseInt(const std::string &_input)
parse string into an integer
Definition: Helpers.hh:427
T min(const std::vector< T > &_values)
get the minimum value of vector of values
Definition: Helpers.hh:337
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:225
bool isnan(double _v)
check if a double is NaN
Definition: Helpers.hh:241
void sort3(T &_a, T &_b, T &_c)
Sort three numbers, such that _a <= _b <= _c.
Definition: Helpers.hh:385