Program Listing for File MathUtil.h

↰ Return to documentation for file (include/apriltag_mit/AprilTags/MathUtil.h)

#ifndef APRILTAGS_MATHUTIL_H_
#define APRILTAGS_MATHUTIL_H_

#include <cmath>
#include <cfloat>
#include <cstdlib>
#include <utility>
#include <opencv2/core/core.hpp>

namespace AprilTags {

template <typename T>
constexpr T Pi() {
  static_assert(std::is_floating_point<T>::value,
                "Type must be floating point");
  return static_cast<T>(M_PI);
}

template <typename T>
constexpr T Pi_2() {
  static_assert(std::is_floating_point<T>::value,
                "Type must be floating point");
  return static_cast<T>(M_PI / 2);
}

static inline float Mod2Pi(float theta) {
  const float twopi = 2 * Pi<float>();
  const float twopi_inv = 1.f / (2.f * Pi<float>());

  const float abs_theta = std::abs(theta);
  const float q = abs_theta * twopi_inv + 0.5f;
  const int qi = (int)q;
  const float r = abs_theta - qi * twopi;
  return (theta < 0) ? -r : r;
}

//+/- Pi
static inline float Mod2Pi(float ref, float theta) {
  return ref + Mod2Pi(theta - ref);
}

static inline float Distance2D(const cv::Point2f& p0, const cv::Point2f& p1) {
  const auto dx = p0.x - p1.x;
  const auto dy = p0.y - p1.y;
  return std::sqrt(dx * dx + dy * dy);
}

static inline float Square(float x) { return x * x; }

}  // namespace AprilTags

#endif  // APRILTAGS_MATHUTIL_H_