13 template <
typename InputIterator>
15 InputIterator pnts_end)
17 Eigen::Vector3d plane_normal({0, 0, 0});
18 Eigen::Vector3d centroid({0, 0, 0});
19 for (
auto i=std::prev(pnts_end), j=pnts_begin; j!=pnts_end; i = j, ++j) {
22 plane_normal[0] += (pt_i[1] - pt_j[1])
23 * (pt_i[2] + pt_j[2]);
24 plane_normal[1] += (pt_i[2] - pt_j[2])
25 * (pt_i[0] + pt_j[0]);
26 plane_normal[2] += (pt_i[0] - pt_j[0])
27 * (pt_i[1] + pt_j[1]);
29 centroid += Eigen::Map<Eigen::Vector3d const>(pt_j.getCoords());
32 plane_normal.normalize();
33 auto n_pnts(std::distance(pnts_begin, pnts_end));
38 d = centroid.dot(plane_normal) / n_pnts;
40 return std::make_pair(plane_normal, d);
43 template <
class T_POINT>
45 const std::vector<T_POINT*>& pnts)
50 template <
class T_POINT>
52 const std::vector<T_POINT>& pnts)
54 Eigen::Vector3d plane_normal({0, 0, 0});
55 Eigen::Vector3d centroid({0, 0, 0});
56 std::size_t n_pnts(pnts.size());
57 for (std::size_t i = n_pnts - 1, j = 0; j < n_pnts; i = j, j++) {
58 plane_normal[0] += (pnts[i][1] - pnts[j][1])
59 * (pnts[i][2] + pnts[j][2]);
60 plane_normal[1] += (pnts[i][2] - pnts[j][2])
61 * (pnts[i][0] + pnts[j][0]);
62 plane_normal[2] += (pnts[i][0] - pnts[j][0])
63 * (pnts[i][1] + pnts[j][1]);
65 centroid += Eigen::Map<Eigen::Vector3d const>(pnts[j].getCoords());
68 plane_normal.normalize();
69 double const d = centroid.dot(plane_normal) / n_pnts;
70 return std::make_pair(plane_normal, d);
74 template <
typename InputIterator>
75 void rotatePoints(Eigen::Matrix3d
const& rot_mat, InputIterator pnts_begin,
76 InputIterator pnts_end)
78 for (
auto it = pnts_begin; it != pnts_end; ++it)
80 Eigen::Map<Eigen::Vector3d>((*it)->getCoords()) =
81 rot_mat * Eigen::Map<Eigen::Vector3d const>((*it)->getCoords());
85 template <
typename InputIterator1,
typename InputIterator2>
87 InputIterator1 p_pnts_end,
88 InputIterator2 r_pnts_begin,
89 InputIterator2 r_pnts_end)
91 assert(std::distance(p_pnts_begin, p_pnts_end) > 2);
94 auto const [plane_normal, d] =
101 for (
auto it = r_pnts_begin; it != r_pnts_end; ++it)
109 template <
typename P>
110 void rotatePoints(Eigen::Matrix3d
const& rot_mat, std::vector<P*>
const& pnts)
void rotatePoints(Eigen::Matrix3d const &rot_mat, InputIterator pnts_begin, InputIterator pnts_end)
Eigen::Matrix3d computeRotationMatrixToXY(Eigen::Vector3d const &n)
Eigen::Matrix3d rotatePointsToXY(InputIterator1 p_pnts_begin, InputIterator1 p_pnts_end, InputIterator2 r_pnts_begin, InputIterator2 r_pnts_end)
std::pair< Eigen::Vector3d, double > getNewellPlane(InputIterator pnts_begin, InputIterator pnts_end)