Helpers.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2012-2016 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#ifndef _GAZEBO_MATH_FUNCTIONS_HH_
#define _GAZEBO_MATH_FUNCTIONS_HH_

#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/round.hpp>
#include <algorithm>
#include <cmath>
#include <limits>
#include <string>
#include <iostream>
#include <vector>

#define GZ_DBL_MAX std::numeric_limits<double>::max()

#define GZ_DBL_MIN std::numeric_limits<double>::min()

#define GZ_DBL_INF std::numeric_limits<double>::infinity()

#define GZ_FLT_MAX std::numeric_limits<float>::max()

#define GZ_FLT_MIN std::numeric_limits<float>::min()

#define GZ_UINT32_MAX std::numeric_limits<uint32_t>::max()

#define GZ_UINT32_MIN std::numeric_limits<uint32_t>::min()

#define GZ_INT32_MAX std::numeric_limits<int32_t>::max()

#define GZ_INT32_MIN std::numeric_limits<int32_t>::min()


namespace gazebo
{
  namespace math
  {

    static const double NAN_D = std::numeric_limits<double>::quiet_NaN();

    static const int NAN_I = std::numeric_limits<int>::quiet_NaN();

    template<typename T>
    inline T clamp(T _v, T _min, T _max)
    {
      return std::max(std::min(_v, _max), _min);
    }

    inline bool isnan(float _v)
    {
      return (boost::math::isnan)(_v);
    }

    inline bool isnan(double _v)
    {
      return (boost::math::isnan)(_v);
    }

    inline float fixnan(float _v)
    {
      return isnan(_v) || std::isinf(_v) ? 0.0f : _v;
    }

    inline double fixnan(double _v)
    {
      return isnan(_v) || std::isinf(_v) ? 0.0 : _v;
    }

    template<typename T>
    inline T mean(const std::vector<T> &_values)
    {
      T sum = 0;
      for (unsigned int i = 0; i < _values.size(); ++i)
        sum += _values[i];
      return sum / _values.size();
    }

    template<typename T>
    inline T variance(const std::vector<T> &_values)
    {
      T avg = mean<T>(_values);

      T sum = 0;
      for (unsigned int i = 0; i < _values.size(); ++i)
        sum += (_values[i] - avg) * (_values[i] - avg);
      return sum / _values.size();
    }

    template<typename T>
    inline T max(const std::vector<T> &_values)
    {
      T max = std::numeric_limits<T>::min();
      for (unsigned int i = 0; i < _values.size(); ++i)
        if (_values[i] > max)
          max = _values[i];
      return max;
    }

    template<typename T>
    inline T min(const std::vector<T> &_values)
    {
      T min = std::numeric_limits<T>::max();
      for (unsigned int i = 0; i < _values.size(); ++i)
        if (_values[i] < min)
          min = _values[i];
      return min;
    }

    template<typename T>
    inline bool equal(const T &_a, const T &_b,
                      const T &_epsilon = 1e-6)
    {
      return std::fabs(_a - _b) <= _epsilon;
    }

    template<typename T>
    inline T precision(const T &_a, const unsigned int &_precision)
    {
      if (!std::isinf(_a))
      {
        return boost::math::round(
          _a * pow(10, _precision)) / pow(10, _precision);
      }
      else
      {
        return _a;
      }
    }

    inline bool isPowerOfTwo(unsigned int _x)
    {
      return ((_x != 0) && ((_x & (~_x + 1)) == _x));
    }

    inline unsigned int roundUpPowerOfTwo(unsigned int _x)
    {
      if (_x == 0)
        return 1;

      if (isPowerOfTwo(_x))
        return _x;

      while (_x & (_x - 1))
        _x = _x & (_x - 1);

      _x = _x << 1;

      return _x;
    }

    inline int parseInt(const std::string& _input)
    {
      const char *p = _input.c_str();
      if (!*p || *p == '?')
        return NAN_I;

      int s = 1;
      while (*p == ' ')
        p++;

      if (*p == '-')
      {
        s = -1;
        p++;
      }

      double acc = 0;
      while (*p >= '0' && *p <= '9')
        acc = acc * 10 + *p++ - '0';

      if (*p)
      {
        std::cerr << "Invalid int numeric format[" << _input << "]\n";
        return 0.0;
      }

      return s * acc;
    }

    inline double parseFloat(const std::string& _input)
    {
      const char *p = _input.c_str();
      if (!*p || *p == '?')
        return NAN_D;
      int s = 1;
      while (*p == ' ')
        p++;

      if (*p == '-')
      {
        s = -1;
        p++;
      }

      double acc = 0;
      while (*p >= '0' && *p <= '9')
        acc = acc * 10 + *p++ - '0';

      if (*p == '.')
      {
        double k = 0.1;
        p++;
        while (*p >= '0' && *p <= '9')
        {
          acc += (*p++ - '0') * k;
          k *= 0.1;
        }
      }
      if (*p == 'e')
      {
        int es = 1;
        int f = 0;
        p++;
        if (*p == '-')
        {
          es = -1;
          p++;
        }
        else if (*p == '+')
        {
          es = 1;
          p++;
        }
        while (*p >= '0' && *p <= '9')
          f = f * 10 + *p++ - '0';

        acc *= pow(10, f*es);
      }

      if (*p)
      {
        std::cerr << "Invalid double numeric format[" << _input << "]\n";
        return 0.0;
      }
      return s * acc;
    }
  }
}
#endif