All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros 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 _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 
93 namespace ignition
94 {
96  namespace math
97  {
99  static const double NAN_D = std::numeric_limits<double>::quiet_NaN();
100 
102  static const float NAN_F = std::numeric_limits<float>::quiet_NaN();
103 
105  static const int NAN_I = std::numeric_limits<int>::quiet_NaN();
106 
111  template<typename T>
112  inline T clamp(T _v, T _min, T _max)
113  {
114  return std::max(std::min(_v, _max), _min);
115  }
116 
120  inline bool isnan(float _v)
121  {
122  return (std::isnan)(_v);
123  }
124 
128  inline bool isnan(double _v)
129  {
130  return (std::isnan)(_v);
131  }
132 
136  inline float fixnan(float _v)
137  {
138  return isnan(_v) || std::isinf(_v) ? 0.0f : _v;
139  }
140 
144  inline double fixnan(double _v)
145  {
146  return isnan(_v) || std::isinf(_v) ? 0.0 : _v;
147  }
148 
152  inline bool isEven(const int _v)
153  {
154  return !(_v % 2);
155  }
156 
160  inline bool isEven(const unsigned int _v)
161  {
162  return !(_v % 2);
163  }
164 
168  inline bool isOdd(const int _v)
169  {
170  return (_v % 2) != 0;
171  }
172 
176  inline bool isOdd(const unsigned int _v)
177  {
178  return (_v % 2) != 0;
179  }
180 
184  template<typename T>
185  inline T mean(const std::vector<T> &_values)
186  {
187  T sum = 0;
188  for (unsigned int i = 0; i < _values.size(); ++i)
189  sum += _values[i];
190  return sum / _values.size();
191  }
192 
196  template<typename T>
197  inline T variance(const std::vector<T> &_values)
198  {
199  T avg = mean<T>(_values);
200 
201  T sum = 0;
202  for (unsigned int i = 0; i < _values.size(); ++i)
203  sum += (_values[i] - avg) * (_values[i] - avg);
204  return sum / _values.size();
205  }
206 
210  template<typename T>
211  inline T max(const std::vector<T> &_values)
212  {
214  for (unsigned int i = 0; i < _values.size(); ++i)
215  if (_values[i] > max)
216  max = _values[i];
217  return max;
218  }
219 
223  template<typename T>
224  inline T min(const std::vector<T> &_values)
225  {
227  for (unsigned int i = 0; i < _values.size(); ++i)
228  if (_values[i] < min)
229  min = _values[i];
230  return min;
231  }
232 
237  template<typename T>
238  inline bool equal(const T &_a, const T &_b,
239  const T &_epsilon = 1e-6)
240  {
241  IGN_FP_VOLATILE T diff = std::abs(_a - _b);
242  return diff <= _epsilon;
243  }
244 
249  template<typename T>
250  inline T precision(const T &_a, const unsigned int &_precision)
251  {
252  return std::round(_a * pow(10, _precision)) / pow(10, _precision);
253  }
254 
258  inline bool isPowerOfTwo(unsigned int _x)
259  {
260  return ((_x != 0) && ((_x & (~_x + 1)) == _x));
261  }
262 
268  inline unsigned int roundUpPowerOfTwo(unsigned int _x)
269  {
270  if (_x == 0)
271  return 1;
272 
273  if (isPowerOfTwo(_x))
274  return _x;
275 
276  while (_x & (_x - 1))
277  _x = _x & (_x - 1);
278 
279  _x = _x << 1;
280 
281  return _x;
282  }
283 
287  inline int parseInt(const std::string &_input)
288  {
289  const char *p = _input.c_str();
290  if (!*p || *p == '?')
291  return NAN_I;
292 
293  int s = 1;
294  while (*p == ' ')
295  p++;
296 
297  if (*p == '-')
298  {
299  s = -1;
300  p++;
301  }
302 
303  double acc = 0;
304  while (*p >= '0' && *p <= '9')
305  acc = acc * 10 + *p++ - '0';
306 
307  if (*p)
308  {
309  std::cerr << "Invalid int numeric format[" << _input << "]\n";
310  return NAN_I;
311  }
312 
313  return static_cast<int>(s * acc);
314  }
315 
320  inline double parseFloat(const std::string &_input)
321  {
322  const char *p = _input.c_str();
323  if (!*p || *p == '?')
324  return NAN_D;
325  int s = 1;
326  while (*p == ' ')
327  p++;
328 
329  if (*p == '-')
330  {
331  s = -1;
332  p++;
333  }
334 
335  double acc = 0;
336  while (*p >= '0' && *p <= '9')
337  acc = acc * 10 + *p++ - '0';
338 
339  if (*p == '.')
340  {
341  double k = 0.1;
342  p++;
343  while (*p >= '0' && *p <= '9')
344  {
345  acc += (*p++ - '0') * k;
346  k *= 0.1;
347  }
348  }
349  if (*p == 'e')
350  {
351  int es = 1;
352  int f = 0;
353  p++;
354  if (*p == '-')
355  {
356  es = -1;
357  p++;
358  }
359  else if (*p == '+')
360  {
361  es = 1;
362  p++;
363  }
364  while (*p >= '0' && *p <= '9')
365  f = f * 10 + *p++ - '0';
366 
367  acc *= pow(10, f*es);
368  }
369 
370  if (*p)
371  {
372  std::cerr << "Invalid double numeric format[" << _input << "]\n";
373  return NAN_D;
374  }
375  return s * acc;
376  }
377  }
378 }
379 
388 #if defined _WIN32 || defined __CYGWIN__
389  #ifdef BUILDING_DLL
390  #ifdef __GNUC__
391  #define IGNITION_VISIBLE __attribute__ ((dllexport))
392  #else
393  #define IGNITION_VISIBLE __declspec(dllexport)
394  #endif
395  #else
396  #ifdef __GNUC__
397  #define IGNITION_VISIBLE __attribute__ ((dllimport))
398  #else
399  #define IGNITION_VISIBLE __declspec(dllimport)
400  #endif
401  #endif
402  #define IGNITION_HIDDEN
403 #else
404  #if __GNUC__ >= 4
405  #define IGNITION_VISIBLE __attribute__ ((visibility ("default")))
406  #define IGNITION_HIDDEN __attribute__ ((visibility ("hidden")))
407  #else
408  #define IGNITION_VISIBLE
409  #define IGNITION_HIDDEN
410  #endif
411 #endif
412 
413 #endif
static const float NAN_F
Returns the representation of a quiet not a number (NAN)
Definition: Helpers.hh:102
static const double NAN_D
Returns the representation of a quiet not a number (NAN)
Definition: Helpers.hh:99
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:250
T mean(const std::vector< T > &_values)
get mean of vector of values
Definition: Helpers.hh:185
bool isnan(float _v)
check if a float is NaN
Definition: Helpers.hh:120
unsigned int roundUpPowerOfTwo(unsigned int _x)
Get the smallest power of two that is greater or equal to a given value.
Definition: Helpers.hh:268
T max(const std::vector< T > &_values)
get the maximum value of vector of values
Definition: Helpers.hh:211
bool isOdd(const int _v)
Check if parameter is odd.
Definition: Helpers.hh:168
T variance(const std::vector< T > &_values)
get variance of vector of values
Definition: Helpers.hh:197
#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:258
bool isEven(const int _v)
Check if parameter is even.
Definition: Helpers.hh:152
static const int NAN_I
Returns the representation of a quiet not a number (NAN)
Definition: Helpers.hh:105
double parseFloat(const std::string &_input)
parse string into float
Definition: Helpers.hh:320
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:238
float fixnan(float _v)
Fix a nan value.
Definition: Helpers.hh:136
int parseInt(const std::string &_input)
parse string into an integer
Definition: Helpers.hh:287
T min(const std::vector< T > &_values)
get the minimum value of vector of values
Definition: Helpers.hh:224
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition: Helpers.hh:112