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.
 
 
 
 
 
 

91 lines
2.6 KiB

#ifndef MEDUSA_BITS_SPATIAL_SEARCH_KDGRID_HPP_
#define MEDUSA_BITS_SPATIAL_SEARCH_KDGRID_HPP_
/**
* @file
* Implementation of KDGrid.
*/
#include "KDGrid_fwd.hpp"
#include "Grid.hpp"
#include <medusa/bits/utils/numutils.hpp>
#include <medusa/bits/utils/assert.hpp>
#include <iostream>
namespace mm {
template <typename vec_t>
typename KDGrid<vec_t>::IndexArray
KDGrid<vec_t>::compute_size(const vec_t& bot, const vec_t& top, scalar_t cell_size) {
IndexArray counts;
for (int i = 0; i < dim; ++i) {
assert_msg(bot[i] <= top[i], "Bottom bounds %s must be below top bounds %s.", bot, top);
counts[i] = iceil((top[i] - bot[i]) / cell_size) + 1;
}
return counts;
}
template <typename vec_t>
bool KDGrid<vec_t>::increment(KDGrid::IndexArray& cur, int span, const KDGrid::IndexArray& ref) {
for (int i = 0; i < dim; ++i) {
if (cur[i] == ref[i] + span) {
cur[i] = ref[i] - span;
} else {
cur[i] += 1;
return true;
}
}
return false;
}
template <typename vec_t>
int KDGrid<vec_t>::insert(const vec_t& p, const KDGrid::IndexArray& index) {
assert_msg(grid_(index) == -1, "This grid cell is already occupied by point %d. Consider"
" using smaller cell size.", grid_(index));
int idx = points_.size();
grid_(index) = idx;
points_.push_back(p);
return idx;
}
template <typename vec_t>
bool
KDGrid<vec_t>::existsPointInSphere(const vec_t& p, scalar_t r, const KDGrid::IndexArray& index) {
int span = static_cast<int>(std::ceil(r/cell_size_));
scalar_t r2 = r*r;
IndexArray tmp_index = index;
for (int i = 0; i < dim; ++i) tmp_index[i] -= span;
do {
if (grid_.inBounds(tmp_index) && grid_(tmp_index) != -1) {
auto gp = points_[grid_(tmp_index)];
if ((p-gp).squaredNorm() < r2) {
return true;
}
}
} while (increment(tmp_index, span, index));
return false;
}
template <typename vec_t>
typename KDGrid<vec_t>::IndexArray KDGrid<vec_t>::compute_index(const vec_t& p) {
Eigen::Matrix<scalar_t, dim, 1> idx = (p-bot_) / cell_size_;
IndexArray index;
for (int i = 0; i < dim; ++i) {
index[i] = static_cast<int>(idx[i]);
}
return index;
}
/// @cond
template <typename V>
std::ostream& operator<<(std::ostream& os, const KDGrid<V>& search) {
os << "KDGrid search structure from " << search.bot_ << " to " << search.top_
<< " with cell_size " << search.cell_size_ << " containing " << search.size()
<< " points";
return os;
}
/// @endcond
} // namespace mm
#endif // MEDUSA_BITS_SPATIAL_SEARCH_KDGRID_HPP_