Medusa  1.1
Coordinate Free Mehless Method implementation
KDGrid.hpp
Go to the documentation of this file.
1 #ifndef MEDUSA_BITS_SPATIAL_SEARCH_KDGRID_HPP_
2 #define MEDUSA_BITS_SPATIAL_SEARCH_KDGRID_HPP_
3 
9 #include "KDGrid_fwd.hpp"
10 #include "Grid.hpp"
13 #include <iostream>
14 
15 namespace mm {
16 
17 template <typename vec_t>
19 KDGrid<vec_t>::compute_size(const vec_t& bot, const vec_t& top, scalar_t cell_size) {
20  IndexArray counts;
21  for (int i = 0; i < dim; ++i) {
22  assert_msg(bot[i] <= top[i], "Bottom bounds %s must be below top bounds %s.", bot, top);
23  counts[i] = iceil((top[i] - bot[i]) / cell_size) + 1;
24  }
25  return counts;
26 }
27 
28 template <typename vec_t>
30  for (int i = 0; i < dim; ++i) {
31  if (cur[i] == ref[i] + span) {
32  cur[i] = ref[i] - span;
33  } else {
34  cur[i] += 1;
35  return true;
36  }
37  }
38  return false;
39 }
40 
41 template <typename vec_t>
42 int KDGrid<vec_t>::insert(const vec_t& p, const KDGrid::IndexArray& index) {
43  assert_msg(grid_(index) == -1, "This grid cell is already occupied by point %d. Consider"
44  " using smaller cell size.", grid_(index));
45  int idx = points_.size();
46  grid_(index) = idx;
47  points_.push_back(p);
48  return idx;
49 }
50 
51 template <typename vec_t>
52 bool
54  int span = static_cast<int>(std::ceil(r/cell_size_));
55  scalar_t r2 = r*r;
56  IndexArray tmp_index = index;
57  for (int i = 0; i < dim; ++i) tmp_index[i] -= span;
58  do {
59  if (grid_.inBounds(tmp_index) && grid_(tmp_index) != -1) {
60  auto gp = points_[grid_(tmp_index)];
61  if ((p-gp).squaredNorm() < r2) {
62  return true;
63  }
64  }
65  } while (increment(tmp_index, span, index));
66  return false;
67 }
68 
69 template <typename vec_t>
71  Eigen::Matrix<scalar_t, dim, 1> idx = (p-bot_) / cell_size_;
72  IndexArray index;
73  for (int i = 0; i < dim; ++i) {
74  index[i] = static_cast<int>(idx[i]);
75  }
76  return index;
77 }
78 
80 template <typename V>
81 std::ostream& operator<<(std::ostream& os, const KDGrid<V>& search) {
82  os << "KDGrid search structure from " << search.bot_ << " to " << search.top_
83  << " with cell_size " << search.cell_size_ << " containing " << search.size()
84  << " points";
85  return os;
86 }
88 
89 } // namespace mm
90 
91 #endif // MEDUSA_BITS_SPATIAL_SEARCH_KDGRID_HPP_
mm
Root namespace for the whole library.
Definition: Gaussian.hpp:14
mm::KDGrid::size
int size() const
Get number of points stored in the structure.
Definition: KDGrid_fwd.hpp:65
mm::KDGrid::existsPointInSphere
bool existsPointInSphere(const vec_t &p, scalar_t r)
Check if any point exists in sphere centered at p with radius r.
Definition: KDGrid_fwd.hpp:81
dim
@ dim
Number of elements of this matrix.
Definition: MatrixBaseAddons.hpp:14
mm::KDGrid::compute_index
IndexArray compute_index(const vec_t &p)
Compute point multi-index.
Definition: KDGrid.hpp:70
mm::KDGrid::top_
vec_t top_
Upper bound of the box.
Definition: KDGrid_fwd.hpp:38
mm::operator<<
std::ostream & operator<<(std::ostream &os, const Gaussian< S > &b)
Output basic information about given Gaussian RBF.
Definition: Gaussian.hpp:37
numutils.hpp
assert_msg
#define assert_msg(cond,...)
Assert with better error reporting.
Definition: assert.hpp:75
mm::KDGrid::IndexArray
Grid< int, dim >::IndexArray IndexArray
Multi-index type.
Definition: KDGrid_fwd.hpp:42
KDGrid_fwd.hpp
mm::iceil
int iceil(T x)
Ceils a floating point to an integer.
Definition: numutils.hpp:27
mm::KDGrid::bot_
vec_t bot_
Lower bound of the box.
Definition: KDGrid_fwd.hpp:37
mm::KDGrid
Search structure over given d-dimensional box with given cell size.
Definition: KDGrid_fwd.hpp:29
assert.hpp
Grid.hpp
mm::KDGrid::compute_size
static IndexArray compute_size(const vec_t &bot, const vec_t &top, scalar_t cell_size)
Compute the sizes of the rid along each dimension.
Definition: KDGrid.hpp:19
mm::KDGrid::scalar_t
vector_t::scalar_t scalar_t
Scalar type used.
Definition: KDGrid_fwd.hpp:32
mm::KDGrid::cell_size_
scalar_t cell_size_
Size of a single cell.
Definition: KDGrid_fwd.hpp:39
mm::KDGrid::insert
int insert(const vec_t &p)
Insert a new point in the structure.
Definition: KDGrid_fwd.hpp:73
mm::KDGrid::increment
bool increment(IndexArray &cur, int span, const IndexArray &ref)
Increment a dim-ary counter.
Definition: KDGrid.hpp:29