Helpers.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2012 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>
#include "gazebo/util/system.hh"

#define GZ_DBL_MAX gazebo::math::MAX_D

#define GZ_DBL_MIN gazebo::math::MIN_D

#define GZ_DBL_INF gazebo::math::INF_D

#define GZ_FLT_MAX gazebo::math::MAX_F

#define GZ_FLT_MIN gazebo::math::MIN_F

#define GZ_UINT32_MAX gazebo::math::MAX_UI32

#define GZ_UINT32_MIN gazebo::math::MIN_UI32

#define GZ_INT32_MAX gazebo::math::MAX_I32

#define GZ_INT32_MIN gazebo::math::MIN_I32


namespace gazebo
{
  namespace math
  {

    static const double
    GAZEBO_DEPRECATED(8.0)
    MAX_D = std::numeric_limits<double>::max();

    static const double
    GAZEBO_DEPRECATED(8.0)
    MIN_D = std::numeric_limits<double>::min();

    static const double
    GAZEBO_DEPRECATED(8.0)
    INF_D = std::numeric_limits<double>::infinity();

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

    static const float
    GAZEBO_DEPRECATED(8.0)
    MAX_F = std::numeric_limits<float>::max();

    static const float
    GAZEBO_DEPRECATED(8.0)
    MIN_F = std::numeric_limits<float>::min();

    static const uint32_t
    GAZEBO_DEPRECATED(8.0)
    MAX_UI32 = std::numeric_limits<uint32_t>::max();

    static const uint32_t
    GAZEBO_DEPRECATED(8.0)
    MIN_UI32 = std::numeric_limits<uint32_t>::min();

    static const int32_t
    GAZEBO_DEPRECATED(8.0)
    MAX_I32 = std::numeric_limits<int32_t>::max();

    static const int32_t
    GAZEBO_DEPRECATED(8.0)
    MIN_I32 = std::numeric_limits<int32_t>::min();

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

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

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

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

#ifndef _WIN32
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
    inline float
    GAZEBO_DEPRECATED(8.0)
    fixnan(float _v)
    {
      return isnan(_v) || std::isinf(_v) ? 0.0f : _v;
    }

    inline double
    GAZEBO_DEPRECATED(8.0)
    fixnan(double _v)
    {
      return isnan(_v) || std::isinf(_v) ? 0.0 : _v;
    }
#ifndef _WIN32
#pragma GCC diagnostic pop
#endif

    template<typename T>
    inline T
    GAZEBO_DEPRECATED(8.0)
    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
    GAZEBO_DEPRECATED(8.0)
    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
    GAZEBO_DEPRECATED(8.0)
    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
    GAZEBO_DEPRECATED(8.0)
    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
    GAZEBO_DEPRECATED(8.0)
    equal(const T &_a, const T &_b, const T &_epsilon = 1e-6)
    {
      return std::fabs(_a - _b) <= _epsilon;
    }

    template<typename T>
    inline T
    GAZEBO_DEPRECATED(8.0)
    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
    GAZEBO_DEPRECATED(8.0)
    isPowerOfTwo(unsigned int _x)
    {
      return ((_x != 0) && ((_x & (~_x + 1)) == _x));
    }

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

#ifndef _WIN32
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
      if (isPowerOfTwo(_x))
        return _x;
#ifndef _WIN32
#pragma GCC diagnostic pop
#endif

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

      _x = _x << 1;

      return _x;
    }

    inline int
    GAZEBO_DEPRECATED(8.0)
    parseInt(const std::string& _input)
    {
      const char *p = _input.c_str();
#ifndef _WIN32
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
      if (!*p || *p == '?')
        return NAN_I;
#ifndef _WIN32
#pragma GCC diagnostic pop
#endif

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

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

      int 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
    GAZEBO_DEPRECATED(8.0)
    parseFloat(const std::string& _input)
    {
      const char *p = _input.c_str();
#ifndef _WIN32
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
      if (!*p || *p == '?')
        return NAN_D;
#ifndef _WIN32
#pragma GCC diagnostic pop
#endif
      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