29 lines
796 B
C++
29 lines
796 B
C++
// 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;
|
|
}
|