You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
40 lines
1.3 KiB
40 lines
1.3 KiB
#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<Eigen::Vector3d>& 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<int>, std::vector<double>> KDTreeMutable::query(const Eigen::Vector3d& point, int k) {
|
|
assert_msg(point.array().isFinite().prod() == 1, "Invalid query point.");
|
|
nanoflann::KNNResultSet<double, int> resultSet(k);
|
|
std::vector<int> ret_index(k);
|
|
std::vector<double> 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 };
|
|
}
|
|
|
|
};
|