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