Program Listing for File constants.h

↰ Return to documentation for file (manif/constants.h)

#ifndef _MANIF_MANIF_CONSTANTS_H_
#define _MANIF_MANIF_CONSTANTS_H_

#include <cmath>
#include <limits>

#define MANIF_PI   3.141592653589793238462643383279502884
#define MANIF_PI_2 1.570796326794896619231321691639751442
#define MANIF_PI_4 0.785398163397448309615660845819875721

namespace manif {
namespace internal {

template <typename T>
T constexpr sqrtNewtonRaphson(T x, T curr, T prev)
{
  return curr == prev
          ? curr
          : sqrtNewtonRaphson(x, T(0.5) * (curr + x / curr), curr);
}

template <typename T>
T constexpr csqrt(T x)
{
  return x >= T(0) && x < std::numeric_limits<T>::infinity()
        ? sqrtNewtonRaphson(x, x, T(0))
        : std::numeric_limits<T>::quiet_NaN();
}

} /* namespace internal */

template <typename _Scalar>
struct Constants
{
  static constexpr _Scalar eps      = std::numeric_limits<_Scalar>::epsilon()*_Scalar(100);
  static constexpr _Scalar eps_sqrt = internal::csqrt(eps);

  static constexpr _Scalar to_rad = _Scalar(MANIF_PI / 180.0);
  static constexpr _Scalar to_deg = _Scalar(180.0 / MANIF_PI);
};

template <typename _Scalar>
constexpr _Scalar Constants<_Scalar>::eps;
template <typename _Scalar>
constexpr _Scalar Constants<_Scalar>::eps_sqrt;
template <typename _Scalar>
constexpr _Scalar Constants<_Scalar>::to_rad;
template <typename _Scalar>
constexpr _Scalar Constants<_Scalar>::to_deg;

} /* namespace manif  */

#endif /* _MANIF_MANIF_CONSTANTS_H_ */