gz/math/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 GZ_MATH_FUNCTIONS_HH_
#define GZ_MATH_FUNCTIONS_HH_
 
#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstdint>
#include <ostream>
#include <limits>
#include <string>
#include <tuple>
#include <utility>
#include <vector>
 
#include <gz/math/config.hh>
#include "gz/math/Export.hh"
 
template <typename T>
constexpr T GZ_MASSMATRIX3_DEFAULT_TOLERANCE = T(10);
 
// TODO(CH3): Deprecated. Remove on tock.
template <typename T>
constexpr T IGN_MASSMATRIX3_DEFAULT_TOLERANCE = T(10);
 
#ifdef M_PI
#define GZ_PI M_PI
#define GZ_PI_2 M_PI_2
#define GZ_PI_4 M_PI_4
#define GZ_SQRT2 M_SQRT2
#else
#define GZ_PI   3.14159265358979323846
#define GZ_PI_2 1.57079632679489661923
#define GZ_PI_4 0.78539816339744830962
#define GZ_SQRT2 1.41421356237309504880
#endif
 
#if defined __FLT_EVAL_METHOD__  &&  __FLT_EVAL_METHOD__ == 2
#define GZ_FP_VOLATILE volatile
#else
#define GZ_FP_VOLATILE
#endif
 
#define GZ_SPHERE_VOLUME(_radius) (4.0*GZ_PI*std::pow(_radius, 3)/3.0)
 
#define GZ_CONE_VOLUME(_r, _l) (_l * GZ_PI * std::pow(_r, 2) / 3.0)
 
#define GZ_CYLINDER_VOLUME(_r, _l) (_l * GZ_PI * std::pow(_r, 2))
 
#define GZ_BOX_VOLUME(_x, _y, _z) (_x *_y * _z)
 
#define GZ_BOX_VOLUME_V(_v) (_v.X() *_v.Y() * _v.Z())
 
namespace gz::math
{
  // Inline bracket to help doxygen filtering.
  inline namespace GZ_MATH_VERSION_NAMESPACE {
  //
  static const size_t GZ_ZERO_SIZE_T  = 0u;
 
  static const size_t GZ_ONE_SIZE_T   = 1u;
 
  static const size_t GZ_TWO_SIZE_T   = 2u;
 
  static const size_t GZ_THREE_SIZE_T = 3u;
 
  static const size_t GZ_FOUR_SIZE_T  = 4u;
 
  static const size_t GZ_FIVE_SIZE_T  = 5u;
 
  static const size_t GZ_SIX_SIZE_T   = 6u;
 
  static const size_t GZ_SEVEN_SIZE_T = 7u;
 
  static const size_t GZ_EIGHT_SIZE_T = 8u;
 
  static const size_t GZ_NINE_SIZE_T  = 9u;
 
  // TODO(CH3): Deprecated. Remove on tock.
  constexpr auto GZ_DEPRECATED(7) IGN_ZERO_SIZE_T  = &GZ_ZERO_SIZE_T;
  constexpr auto GZ_DEPRECATED(7) IGN_ONE_SIZE_T   = &GZ_ONE_SIZE_T;
  constexpr auto GZ_DEPRECATED(7) IGN_TWO_SIZE_T   = &GZ_TWO_SIZE_T;
  constexpr auto GZ_DEPRECATED(7) IGN_THREE_SIZE_T = &GZ_THREE_SIZE_T;
  constexpr auto GZ_DEPRECATED(7) IGN_FOUR_SIZE_T  = &GZ_FOUR_SIZE_T;
  constexpr auto GZ_DEPRECATED(7) IGN_FIVE_SIZE_T  = &GZ_FIVE_SIZE_T;
  constexpr auto GZ_DEPRECATED(7) IGN_SIX_SIZE_T   = &GZ_SIX_SIZE_T;
  constexpr auto GZ_DEPRECATED(7) IGN_SEVEN_SIZE_T = &GZ_SEVEN_SIZE_T;
  constexpr auto GZ_DEPRECATED(7) IGN_EIGHT_SIZE_T = &GZ_EIGHT_SIZE_T;
  constexpr auto GZ_DEPRECATED(7) IGN_NINE_SIZE_T  = &GZ_NINE_SIZE_T;
 
  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();
 
  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 / static_cast<T>(_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 / static_cast<T>(_values.size());
  }
 
  template<typename T>
  inline T max(const std::vector<T> &_values)
  {
    return *std::max_element(std::begin(_values), std::end(_values));
  }
 
  template<typename T>
  inline T min(const std::vector<T> &_values)
  {
    return *std::min_element(std::begin(_values), std::end(_values));
  }
 
  template<typename T>
  inline bool equal(const T &_a, const T &_b,
                    const T &_epsilon = T(1e-6))
  {
    GZ_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)
  {
    if (_b < _a)
      std::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);
  }
 
  template<typename T>
  inline void appendToStream(std::ostream &_out, T _number)
  {
    if (std::fpclassify(_number) == FP_ZERO)
    {
      _out << 0;
    }
    else
    {
      _out << _number;
    }
  }
 
  template<>
  inline void appendToStream(std::ostream &_out, int _number)
  {
    _out << _number;
  }
 
  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;
  }
 
  int GZ_MATH_VISIBLE parseInt(const std::string &_input);
 
  double GZ_MATH_VISIBLE parseFloat(const std::string &_input);
 
  std::pair<int64_t, int64_t> GZ_MATH_VISIBLE timePointToSecNsec(
      const std::chrono::steady_clock::time_point &_time);
 
  std::chrono::steady_clock::time_point GZ_MATH_VISIBLE
    secNsecToTimePoint(
      const uint64_t &_sec, const uint64_t &_nanosec);
 
  std::chrono::steady_clock::duration GZ_MATH_VISIBLE secNsecToDuration(
      const uint64_t &_sec, const uint64_t &_nanosec);
 
  std::pair<int64_t, int64_t> GZ_MATH_VISIBLE durationToSecNsec(
      const std::chrono::steady_clock::duration &_dur);
 
  // TODO(anyone): Replace this with std::chrono::days.
  typedef std::chrono::duration<uint64_t, std::ratio<86400>> days;
 
  template<class...Durations, class DurationIn>
  std::tuple<Durations...> breakDownDurations(DurationIn d) {
    std::tuple<Durations...> retval;
    using discard = int[];
    (void)discard{0, (void((
      (std::get<Durations>(retval) =
        std::chrono::duration_cast<Durations>(d)),
      (d -= std::chrono::duration_cast<DurationIn>(
        std::get<Durations>(retval))))), 0)...};
    return retval;
  }
 
  std::string GZ_MATH_VISIBLE timePointToString(
      const std::chrono::steady_clock::time_point &_point);
 
  std::string GZ_MATH_VISIBLE durationToString(
      const std::chrono::steady_clock::duration &_duration);
 
  bool GZ_MATH_VISIBLE splitTimeBasedOnTimeRegex(
      const std::string &_timeString,
      uint64_t & numberDays, uint64_t & numberHours,
      uint64_t & numberMinutes, uint64_t & numberSeconds,
      uint64_t & numberMilliseconds);
 
  inline bool isTimeString(const std::string &_timeString)
  {
    // These will be thrown away, just for making the function call
    uint64_t d, h, m, s, ms;
    return splitTimeBasedOnTimeRegex(_timeString, d, h, m, s, ms);
  }
 
  std::chrono::steady_clock::duration GZ_MATH_VISIBLE stringToDuration(
      const std::string &_timeString);
 
  std::chrono::steady_clock::time_point
  GZ_MATH_VISIBLE stringToTimePoint(const std::string &_timeString);
 
  // 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 GZ_MATH_VISIBLE Pair(
      const PairInput _a, const PairInput _b);
 
  std::tuple<PairInput, PairInput> GZ_MATH_VISIBLE Unpair(
      const PairOutput _key);
  }  // namespace GZ_MATH_VERSION_NAMESPACE
}  // namespace gz::math
#endif  // GZ_MATH_FUNCTIONS_HH_