// 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::pair<std::vector<Eigen::Vector<double, d>>, std::vector<double>>
apply_transform(const TransformMatrix &mtx,
                const std::vector<Eigen::Vector<double, d>> &points) {
  std::pair<std::vector<Eigen::Vector<double, d>>, std::vector<double>> res{};
  for (const auto &p : points) {
    const auto &transform_res = mtx * p.homogeneous();
    res.first.push_back(transform_res.hnormalized());
    res.second.push_back(transform_res.w());
  }
  return res;
}
