Program Listing for File utils.h

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

#ifndef _MANIF_MANIF_UTILS_H_
#define _MANIF_MANIF_UTILS_H_

#include "manif/constants.h"

namespace manif {

template <typename T>
T pi2pi(T angle)
{
  while (angle > T(MANIF_PI))   angle -= T(2. * MANIF_PI);
  while (angle <= T(-MANIF_PI)) angle += T(2. * MANIF_PI);

  return angle;
}

template<typename T>
constexpr T toRad(const T deg)
{
  return deg * Constants<T>::to_rad;
}

template<typename T>
constexpr T toDeg(const T rad)
{
  return rad * Constants<T>::to_deg;
}

template <typename T>
constexpr T approxSqrtInv(const T x)
{
  return (T(15) / T(8)) - (T(5) / T(4)) * x + (T(3) / T(8)) * x * x;
}

} /* namespace manif */

#endif /* _MANIF_MANIF_UTILS_H_ */