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 IGNITION_MATH_FUNCTIONS_HH_
#define IGNITION_MATH_FUNCTIONS_HH_

#include <chrono>
#include <cmath>
#include <algorithm>
#include <limits>
#include <string>
#include <iostream>
#include <vector>
#include <tuple>
#include <utility>
#include <cstdint>

#include <ignition/math/config.hh>
#include "ignition/math/Export.hh"

template <typename T>
constexpr T IGN_MASSMATRIX3_DEFAULT_TOLERANCE = T(10);

#define IGN_DBL_MAX ignition::math::DPRCT_MAX_D

#define IGN_DBL_MIN ignition::math::DPRCT_MIN_D

#define IGN_DBL_LOW ignition::math::DPRCT_LOW_D

#define IGN_DBL_INF ignition::math::DPRCT_INF_D

#define IGN_FLT_MAX ignition::math::DPRCT_MAX_F

#define IGN_FLT_MIN ignition::math::DPRCT_MIN_F

#define IGN_FLT_LOW ignition::math::DPRCT_LOW_F

#define IGN_FLT_INF ignition::math::DPRCT_INF_F

#define IGN_UINT16_MAX ignition::math::DPRCT_MAX_UI16

#define IGN_UINT16_MIN ignition::math::DPRCT_MIN_UI16

#define IGN_UINT16_LOW ignition::math::DPRCT_LOW_UI16

#define IGN_UINT16_INF ignition::math::DPRCT_INF_UI16

#define IGN_INT16_MAX ignition::math::DPRCT_MAX_I16

#define IGN_INT16_MIN ignition::math::DPRCT_MIN_I16

#define IGN_INT16_LOW ignition::math::DPRCT_LOW_I16

#define IGN_INT16_INF ignition::math::DPRCT_INF_I16

#define IGN_UINT32_MAX ignition::math::DPRCT_MAX_UI32

#define IGN_UINT32_MIN ignition::math::DPRCT_MIN_UI32

#define IGN_UINT32_LOW ignition::math::DPRCT_LOW_UI32

#define IGN_UINT32_INF ignition::math::DPRCT_INF_UI32

#define IGN_INT32_MAX ignition::math::DPRCT_MAX_I32

#define IGN_INT32_MIN ignition::math::DPRCT_MIN_I32

#define IGN_INT32_LOW ignition::math::DPRCT_LOW_I32

#define IGN_INT32_INF ignition::math::DPRCT_INF_I32

#define IGN_UINT64_MAX ignition::math::DPRCT_MAX_UI64

#define IGN_UINT64_MIN ignition::math::DPRCT_MIN_UI64

#define IGN_UINT64_LOW ignition::math::DPRCT_LOW_UI64

#define IGN_UINT64_INF ignition::math::DPRCT_INF_UI64

#define IGN_INT64_MAX ignition::math::DPRCT_MAX_I64

#define IGN_INT64_MIN ignition::math::DPRCT_MIN_I64

#define IGN_INT64_LOW ignition::math::DPRCT_LOW_I64

#define IGN_INT64_INF ignition::math::DPRCT_INF_I64

#ifdef M_PI
#define IGN_PI M_PI
#define IGN_PI_2 M_PI_2
#define IGN_PI_4 M_PI_4
#define IGN_SQRT2 M_SQRT2
#else
#define IGN_PI   3.14159265358979323846
#define IGN_PI_2 1.57079632679489661923
#define IGN_PI_4 0.78539816339744830962
#define IGN_SQRT2 1.41421356237309504880
#endif

#if defined __FLT_EVAL_METHOD__  &&  __FLT_EVAL_METHOD__ == 2
#define IGN_FP_VOLATILE volatile
#else
#define IGN_FP_VOLATILE
#endif

#define IGN_SPHERE_VOLUME(_radius) (4.0*IGN_PI*std::pow(_radius, 3)/3.0)

#define IGN_CYLINDER_VOLUME(_r, _l) (_l * IGN_PI * std::pow(_r, 2))

#define IGN_BOX_VOLUME(_x, _y, _z) (_x *_y * _z)

#define IGN_BOX_VOLUME_V(_v) (_v.X() *_v.Y() * _v.Z())

namespace ignition
{
  namespace math
  {
    // Inline bracket to help doxygen filtering.
    inline namespace IGNITION_MATH_VERSION_NAMESPACE {
    //
    static const size_t IGN_ZERO_SIZE_T  = 0u;

    static const size_t IGN_ONE_SIZE_T   = 1u;

    static const size_t IGN_TWO_SIZE_T   = 2u;

    static const size_t IGN_THREE_SIZE_T = 3u;

    static const size_t IGN_FOUR_SIZE_T  = 4u;

    static const size_t IGN_FIVE_SIZE_T  = 5u;

    static const size_t IGN_SIX_SIZE_T   = 6u;

    static const size_t IGN_SEVEN_SIZE_T = 7u;

    static const size_t IGN_EIGHT_SIZE_T = 8u;

    static const size_t IGN_NINE_SIZE_T  = 9u;

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

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

    static const double LOW_D = std::numeric_limits<double>::lowest();

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

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

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

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

    static const float LOW_F = std::numeric_limits<float>::lowest();

    static const float INF_F = std::numeric_limits<float>::infinity();

    static const float NAN_F = std::numeric_limits<float>::quiet_NaN();

    static const uint16_t MAX_UI16 = std::numeric_limits<uint16_t>::max();

    static const uint16_t MIN_UI16 = std::numeric_limits<uint16_t>::min();

    static const uint16_t LOW_UI16 = std::numeric_limits<uint16_t>::lowest();

    static const uint16_t INF_UI16 = std::numeric_limits<uint16_t>::infinity();

    static const int16_t MAX_I16 = std::numeric_limits<int16_t>::max();

    static const int16_t MIN_I16 = std::numeric_limits<int16_t>::min();

    static const int16_t LOW_I16 = std::numeric_limits<int16_t>::lowest();

    static const int16_t INF_I16 = std::numeric_limits<int16_t>::infinity();

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

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

    static const uint32_t LOW_UI32 = std::numeric_limits<uint32_t>::lowest();

    static const uint32_t INF_UI32 = std::numeric_limits<uint32_t>::infinity();

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

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

    static const int32_t LOW_I32 = std::numeric_limits<int32_t>::lowest();

    static const int32_t INF_I32 = std::numeric_limits<int32_t>::infinity();

    static const uint64_t MAX_UI64 = std::numeric_limits<uint64_t>::max();

    static const uint64_t MIN_UI64 = std::numeric_limits<uint64_t>::min();

    static const uint64_t LOW_UI64 = std::numeric_limits<uint64_t>::lowest();

    static const uint64_t INF_UI64 = std::numeric_limits<uint64_t>::infinity();

    static const int64_t MAX_I64 = std::numeric_limits<int64_t>::max();

    static const int64_t MIN_I64 = std::numeric_limits<int64_t>::min();

    static const int64_t LOW_I64 = std::numeric_limits<int64_t>::lowest();

    static const int64_t INF_I64 = std::numeric_limits<int64_t>::infinity();

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

    // variables created to deprecate macros in this file
    static const double IGN_DEPRECATED(3) DPRCT_MAX_D = MAX_D;
    static const double IGN_DEPRECATED(3) DPRCT_MIN_D = MIN_D;
    static const double IGN_DEPRECATED(3) DPRCT_LOW_D = LOW_D;
    static const double IGN_DEPRECATED(3) DPRCT_INF_D = INF_D;
    static const float IGN_DEPRECATED(3) DPRCT_MAX_F = MAX_F;
    static const float IGN_DEPRECATED(3) DPRCT_MIN_F = MIN_F;
    static const float IGN_DEPRECATED(3) DPRCT_LOW_F = LOW_F;
    static const float IGN_DEPRECATED(3) DPRCT_INF_F = INF_F;
    static const uint16_t IGN_DEPRECATED(3) DPRCT_MAX_UI16 = MAX_UI16;
    static const uint16_t IGN_DEPRECATED(3) DPRCT_MIN_UI16 = MIN_UI16;
    static const uint16_t IGN_DEPRECATED(3) DPRCT_LOW_UI16 = LOW_UI16;
    static const uint16_t IGN_DEPRECATED(3) DPRCT_INF_UI16 = INF_UI16;
    static const int16_t IGN_DEPRECATED(3) DPRCT_MAX_I16 = MAX_I16;
    static const int16_t IGN_DEPRECATED(3) DPRCT_MIN_I16 = MIN_I16;
    static const int16_t IGN_DEPRECATED(3) DPRCT_LOW_I16 = LOW_I16;
    static const int16_t IGN_DEPRECATED(3) DPRCT_INF_I16 = INF_I16;
    static const uint32_t IGN_DEPRECATED(3) DPRCT_MAX_UI32 = MAX_UI32;
    static const uint32_t IGN_DEPRECATED(3) DPRCT_MIN_UI32 = MIN_UI32;
    static const uint32_t IGN_DEPRECATED(3) DPRCT_LOW_UI32 = LOW_UI32;
    static const uint32_t IGN_DEPRECATED(3) DPRCT_INF_UI32 = INF_UI32;
    static const int32_t IGN_DEPRECATED(3) DPRCT_MAX_I32 = MAX_I32;
    static const int32_t IGN_DEPRECATED(3) DPRCT_MIN_I32 = MIN_I32;
    static const int32_t IGN_DEPRECATED(3) DPRCT_LOW_I32 = LOW_I32;
    static const int32_t IGN_DEPRECATED(3) DPRCT_INF_I32 = INF_I32;
    static const uint64_t IGN_DEPRECATED(3) DPRCT_MAX_UI64 = MAX_UI64;
    static const uint64_t IGN_DEPRECATED(3) DPRCT_MIN_UI64 = MIN_UI64;
    static const uint64_t IGN_DEPRECATED(3) DPRCT_LOW_UI64 = LOW_UI64;
    static const uint64_t IGN_DEPRECATED(3) DPRCT_INF_UI64 = INF_UI64;
    static const int64_t IGN_DEPRECATED(3) DPRCT_MAX_I64 = MAX_I64;
    static const int64_t IGN_DEPRECATED(3) DPRCT_MIN_I64 = MIN_I64;
    static const int64_t IGN_DEPRECATED(3) DPRCT_LOW_I64 = LOW_I64;
    static const int64_t IGN_DEPRECATED(3) DPRCT_INF_I64 = INF_I64;

    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 (std::isnan)(_v);
    }

    inline bool isnan(double _v)
    {
      return (std::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;
    }

    inline bool isEven(const int _v)
    {
      return !(_v % 2);
    }

    inline bool isEven(const unsigned int _v)
    {
      return !(_v % 2);
    }

    inline bool isOdd(const int _v)
    {
      return (_v % 2) != 0;
    }

    inline bool isOdd(const unsigned int _v)
    {
      return (_v % 2) != 0;
    }

    template<typename T>
    inline int sgn(T _value)
    {
      return (T(0) < _value) - (_value < T(0));
    }

    template<typename T>
    inline int signum(T _value)
    {
      return sgn(_value);
    }

    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 = T(1e-6))
    {
      IGN_FP_VOLATILE T diff = std::abs(_a - _b);
      return diff <= _epsilon;
    }

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

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

    template<typename T>
    inline T precision(const T &_a, const unsigned int &_precision)
    {
      auto p = std::pow(10, _precision);
      return static_cast<T>(std::round(_a * p) / p);
    }

    template<typename T>
    inline void sort2(T &_a, T &_b)
    {
      using std::swap;
      if (_b < _a)
        swap(_a, _b);
    }

    template<typename T>
    inline void sort3(T &_a, T &_b, T &_c)
    {
      // _a <= _b
      sort2(_a, _b);
      // _a <= _c, _b <= _c
      sort2(_b, _c);
      // _a <= _b <= _c
      sort2(_a, _b);
    }

    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 roundUpMultiple(int _num, int _multiple)
    {
      if (_multiple == 0)
        return _num;

      int remainder = std::abs(_num) % _multiple;
      if (remainder == 0)
        return _num;

      if (_num < 0)
        return -(std::abs(_num) - remainder);
      else
        return _num + _multiple - remainder;
    }

    inline int parseInt(const std::string &_input)
    {
      // Return NAN_I if it is empty
      if (_input.empty())
      {
        return NAN_I;
      }
      // Return 0 if it is all spaces
      else if (_input.find_first_not_of(' ') == std::string::npos)
      {
        return 0;
      }

      // Otherwise try standard library
      try
      {
        return std::stoi(_input);
      }
      // if that fails, return NAN_I
      catch(...)
      {
        return NAN_I;
      }
    }

    inline double parseFloat(const std::string &_input)
    {
      // Return NAN_D if it is empty
      if (_input.empty())
      {
        return NAN_D;
      }
      // Return 0 if it is all spaces
      else if (_input.find_first_not_of(' ') == std::string::npos)
      {
        return 0;
      }

      // Otherwise try standard library
      try
      {
        return std::stod(_input);
      }
      // if that fails, return NAN_D
      catch(...)
      {
        return NAN_D;
      }
    }

    inline std::pair<int64_t, int64_t> durationToSecNsec(
        const std::chrono::steady_clock::duration &_dur)
    {
      auto s = std::chrono::duration_cast<std::chrono::seconds>(_dur);
      auto ns = std::chrono::duration_cast<std::chrono::nanoseconds>(_dur-s);
      return {s.count(), ns.count()};
    }

    // Degrade precision on Windows, which cannot handle 'long double'
    // values properly. See the implementation of Unpair.
    // 32 bit ARM processors also define 'long double' to be the same
    // size as 'double', and must also be degraded
#if defined _MSC_VER || defined __arm__
    using PairInput = uint16_t;
    using PairOutput = uint32_t;
#else
    using PairInput = uint32_t;
    using PairOutput = uint64_t;
#endif

    PairOutput IGNITION_MATH_VISIBLE Pair(
        const PairInput _a, const PairInput _b);

    std::tuple<PairInput, PairInput> IGNITION_MATH_VISIBLE Unpair(
        const PairOutput _key);
    }
  }
}

#endif