// Copyright 2024 SquareBlock Inc. All Rights Reserved.
// Author: tianlei.richard@qq.com (tianlei.richard)

#include "common.h"
#include <cmath>
#include <numeric>

template <typename FloatType>
bool fequal(const FloatType a, const FloatType b) {
  return std::fabs(a - b) <= std::numeric_limits<FloatType>::epsilon();
}

template <int d>
std::vector<Eigen::Vector<double, d>>
apply_transform(const TransformMatrix &mtx,
                const std::vector<Eigen::Vector<double, d>> &points) {
  std::vector<Eigen::Vector<double, d>> res{};
  for (const auto &p : points) {
    const auto &transform_res = mtx * p.homogeneous();
    res.push_back(transform_res.hnormalized());
  }
  return res;
}

template <int d>
void apply_transform_in_place(const TransformMatrix &mtx,
                              std::vector<Eigen::Vector<double, d>> &points) {
  for (auto &p : points) {
    p = mtx * p.homogeneous().hnormalized();
  }
}
