// 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) {
    auto tmp_p = Eigen::Vector<double, 1 + d>();
    tmp_p << p, 1.;
    tmp_p = mtx * tmp_p;
    if (!fequal(tmp_p.w(), 1.)) {
      tmp_p /= tmp_p.w();
    }
    res.push_back(tmp_p.template head<d>());
  }
  return res;
}