#pragma once #include "assert.hpp" #include "KDTreeMutable_fwd.hpp" namespace meshless { void KDTreeMutable::insert(const Eigen::Vector3d& point) { assert_msg(point.array().isFinite().prod() == 1, "Invalid point."); auto n = points_.kdtree_get_point_count(); points_.add(point); tree.addPoints(n, n); ++size_; } void KDTreeMutable::insert(const std::vector& points) { auto n = points_.kdtree_get_point_count(); for(const auto& p : points) { assert_msg(p.array().isFinite().prod() == 1, "One of the points is invalid."); points_.add(p); } size_ += points.size(); tree.addPoints(n, n + points.size() - 1); } std::pair, std::vector> KDTreeMutable::query(const Eigen::Vector3d& point, int k) { assert_msg(point.array().isFinite().prod() == 1, "Invalid query point."); nanoflann::KNNResultSet resultSet(k); std::vector ret_index(k); std::vector out_dist_sqr(k); resultSet.init(&ret_index[0], &out_dist_sqr[0]); tree.findNeighbors(resultSet, point.data(), nanoflann::SearchParams(k)); assert_msg(resultSet.full(), "Not enough points in the tree, you requested %d points, " "but the tree contains only %d points.", k, size()); return { ret_index, out_dist_sqr }; } };