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 2012 Nate Koenig
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 <algorithm>
22 #include <cmath>
23 #include <limits>
24 #include <string>
25 #include <iostream>
26 #include <vector>
27 
28 #define GZ_DBL_MAX std::numeric_limits<double>::max()
29 #define GZ_DBL_MIN std::numeric_limits<double>::min()
30 
31 #define GZ_FLT_MAX std::numeric_limits<float>::max()
32 #define GZ_FLT_MIN std::numeric_limits<float>::min()
33 
34 namespace gazebo
35 {
36  namespace math
37  {
42 
44  static const double NAN_D = std::numeric_limits<double>::quiet_NaN();
45 
47  static const int NAN_I = std::numeric_limits<int>::quiet_NaN();
48 
53  template<typename T>
54  inline T clamp(T _v, T _min, T _max)
55  {
56  return std::max(std::min(_v, _max), _min);
57  }
58 
62  inline bool isnan(float _v)
63  {
64  return (boost::math::isnan)(_v);
65  }
66 
70  inline bool isnan(double _v)
71  {
72  return (boost::math::isnan)(_v);
73  }
74 
78  template<typename T>
79  inline T mean(const std::vector<T> &_values)
80  {
81  T sum = 0;
82  for (unsigned int i = 0; i < _values.size(); ++i)
83  sum += _values[i];
84  return sum / _values.size();
85  }
86 
90  template<typename T>
91  inline T variance(const std::vector<T> &_values)
92  {
93  T avg = mean<T>(_values);
94 
95  T sum = 0;
96  for (unsigned int i = 0; i < _values.size(); ++i)
97  sum += (_values[i] - avg) * (_values[i] - avg);
98  return sum / _values.size();
99  }
100 
104  template<typename T>
105  inline T max(const std::vector<T> &_values)
106  {
108  for (unsigned int i = 0; i < _values.size(); ++i)
109  if (_values[i] > max)
110  max = _values[i];
111  return max;
112  }
113 
117  template<typename T>
118  inline T min(const std::vector<T> &_values)
119  {
121  for (unsigned int i = 0; i < _values.size(); ++i)
122  if (_values[i] < min)
123  min = _values[i];
124  return min;
125  }
126 
131  template<typename T>
132  inline bool equal(const T &_a, const T &_b,
133  const T &_epsilon = 1e-6)
134  {
135  return std::fabs(_a - _b) <= _epsilon;
136  }
137 
142  template<typename T>
143  inline T precision(const T &_a, const unsigned int &_precision)
144  {
145  return round(_a * pow(10, _precision)) / pow(10, _precision);
146  }
147 
151  inline bool isPowerOfTwo(unsigned int _x)
152  {
153  return ((_x != 0) && ((_x & (~_x + 1)) == _x));
154  }
155 
159  inline int parseInt(const std::string& _input)
160  {
161  const char *p = _input.c_str();
162  if (!*p || *p == '?')
163  return NAN_I;
164 
165  int s = 1;
166  while (*p == ' ')
167  p++;
168 
169  if (*p == '-')
170  {
171  s = -1;
172  p++;
173  }
174 
175  double acc = 0;
176  while (*p >= '0' && *p <= '9')
177  acc = acc * 10 + *p++ - '0';
178 
179  if (*p)
180  {
181  std::cerr << "Invalid int numeric format[" << _input << "]\n";
182  return 0.0;
183  }
184 
185  return s * acc;
186  }
187 
192  inline double parseFloat(const std::string& _input)
193  {
194  const char *p = _input.c_str();
195  if (!*p || *p == '?')
196  return NAN_D;
197  int s = 1;
198  while (*p == ' ')
199  p++;
200 
201  if (*p == '-')
202  {
203  s = -1;
204  p++;
205  }
206 
207  double acc = 0;
208  while (*p >= '0' && *p <= '9')
209  acc = acc * 10 + *p++ - '0';
210 
211  if (*p == '.')
212  {
213  double k = 0.1;
214  p++;
215  while (*p >= '0' && *p <= '9')
216  {
217  acc += (*p++ - '0') * k;
218  k *= 0.1;
219  }
220  }
221  if (*p == 'e')
222  {
223  int es = 1;
224  int f = 0;
225  p++;
226  if (*p == '-')
227  {
228  es = -1;
229  p++;
230  }
231  else if (*p == '+')
232  {
233  es = 1;
234  p++;
235  }
236  while (*p >= '0' && *p <= '9')
237  f = f * 10 + *p++ - '0';
238 
239  acc *= pow(10, f*es);
240  }
241 
242  if (*p)
243  {
244  std::cerr << "Invalid double numeric format[" << _input << "]\n";
245  return 0.0;
246  }
247  return s * acc;
248  }
250  }
251 }
252 #endif