Fix/neighbor_search

include/spatial_hash/impl/neighbor_utils_impl.h

@@ -42,18 +42,17 @@
 namespace spatial_hash {
 
 template <typename BlockT>
-BlockNeighborSearch<BlockT>::BlockNeighborSearch(const Layer<BlockT>& layer,
-                                                 Connectivity connectivity)
+BlockNeighborSearch<BlockT>::BlockNeighborSearch(const Layer<BlockT>& layer, size_t connectivity)
     : NeighborSearch(connectivity), layer_(layer) {}
 
 template <typename IndexT>
 std::vector<IndexT> NeighborSearch::neighborIndices(const IndexT& index,
                                                     const bool include_self) const {
-  std::vector<IndexT> neighbors(static_cast<size_t>(connectivity) +
-                                static_cast<size_t>(include_self));
-
-  for (size_t i = include_self ? 0 : 1; i < neighbors.size(); ++i) {
-    neighbors[i] = index + kNeighborOffsets.col(i).cast<typename IndexT::Scalar>();
+  const size_t offset = include_self ? 0 : 1;
+  std::vector<IndexT> neighbors;
+  neighbors.reserve(connectivity + offset);
+  for (size_t i = offset; i <= connectivity; ++i) {
+    neighbors.emplace_back(index + kNeighborOffsets.col(i).cast<typename IndexT::Scalar>());
   }
   return neighbors;
 }
@@ -64,7 +63,6 @@ std::vector<const BlockT*> BlockNeighborSearch<BlockT>::neighborBlocks(
     const bool include_self) const {
   std::vector<const BlockT*> neighbors;
   neighbors.reserve(static_cast<size_t>(connectivity) + static_cast<size_t>(include_self));
-
   for (const auto& index : neighborIndices(block_index, include_self)) {
     const auto block = layer_.getBlockPtr(index);
     if (block) {
@@ -77,7 +75,7 @@ std::vector<const BlockT*> BlockNeighborSearch<BlockT>::neighborBlocks(
 
 template <typename BlockT>
 VoxelNeighborSearch<BlockT>::VoxelNeighborSearch(const VoxelLayer<BlockT>& layer,
-                                                 Connectivity connectivity)
+                                                 size_t connectivity)
     : NeighborSearch(connectivity), layer_(layer) {}
 
 template <typename BlockT>

include/spatial_hash/neighbor_utils.h

@@ -47,12 +47,8 @@ namespace spatial_hash {
  * @brief Index-based neighbor search.
  */
 struct NeighborSearch {
-  // Number of neighbors to search for.
-  enum class Connectivity : uint32_t { k6 = 6u, k18 = 18u, k26 = 26u };
-
   // Constructors.
-  explicit NeighborSearch(Connectivity connectivity);
-  explicit NeighborSearch(uint32_t connectivity);
+  explicit NeighborSearch(size_t connectivity);
   virtual ~NeighborSearch() = default;
 
   /**
@@ -63,7 +59,7 @@ struct NeighborSearch {
   template <typename IndexT>
   std::vector<IndexT> neighborIndices(const IndexT& index, const bool include_self = false) const;
 
-  const Connectivity connectivity;
+  const size_t connectivity;
 
  protected:
   // Neighbor offsets ordered for self->6->18->26 connectivity.
@@ -76,10 +72,7 @@ struct NeighborSearch {
  */
 template <typename BlockT>
 struct BlockNeighborSearch : public NeighborSearch {
-  BlockNeighborSearch(const Layer<BlockT>& layer, Connectivity connectivity);
-  BlockNeighborSearch(const Layer<BlockT>& layer, uint32_t connectivity)
-      : BlockNeighborSearch(layer, static_cast<Connectivity>(connectivity)) {}
-
+  BlockNeighborSearch(const Layer<BlockT>& layer, size_t connectivity);
   virtual ~BlockNeighborSearch() = default;
 
   /**
@@ -104,9 +97,7 @@ template <typename BlockT>
 struct VoxelNeighborSearch : public NeighborSearch {
   using VoxelType = typename BlockT::VoxelType;
 
-  VoxelNeighborSearch(const VoxelLayer<BlockT>& layer, Connectivity connectivity);
-  VoxelNeighborSearch(const VoxelLayer<BlockT>& layer, uint32_t connectivity)
-      : VoxelNeighborSearch(layer, static_cast<Connectivity>(connectivity)) {}
+  VoxelNeighborSearch(const VoxelLayer<BlockT>& layer, size_t connectivity);
 
   virtual ~VoxelNeighborSearch() = default;
 

include/spatial_hash/voxel_block.h

@@ -132,6 +132,13 @@ struct VoxelBlock : public IndexGrid, public Block {
     return voxelIndexFromLinearIndex(linear_index, voxels_per_side);
   }
 
+  /**
+   * @brief Get the local voxel index from a global position. This voxel index may be invalid if the
+   * point is outside the block.
+   * @param position The global position to convert.
+   */
+  VoxelIndex getVoxelIndex(const Point& position) const { return toIndex(position - origin()); }
+
   /**
    * @brief Get the global voxel index from a linear index.
    * @param linear_index The linear index to convert.

src/neighbor_utils.cpp

@@ -36,19 +36,13 @@
 
 namespace spatial_hash {
 
-NeighborSearch::NeighborSearch(Connectivity connectivity) : connectivity(connectivity) {
-  checkConnectivity();
-}
-
-NeighborSearch::NeighborSearch(uint32_t connectivity)
-    : connectivity(static_cast<Connectivity>(connectivity)) {
+NeighborSearch::NeighborSearch(size_t connectivity) : connectivity(connectivity) {
   checkConnectivity();
 }
 
 void NeighborSearch::checkConnectivity() const {
-  CHECK(connectivity == Connectivity::k6 || connectivity == Connectivity::k18 ||
-        connectivity == Connectivity::k26)
-      << "Invalid connectivity value: " << static_cast<uint32_t>(connectivity);
+  CHECK(connectivity == 6 || connectivity == 18 || connectivity == 26)
+      << "Invalid connectivity value: " << connectivity << ", must be 6, 18, or 26.";
 }
 
 const Eigen::Matrix<int, 3, 27> NeighborSearch::kNeighborOffsets = [] {

tests/utest_neighbor_utils.cpp

@@ -44,11 +44,18 @@ TEST(NeighborUtils, IndicesValid) {
   const auto neighbors = search.neighborIndices(index, true);
   EXPECT_EQ(7, neighbors.size());
   EXPECT_EQ(index, neighbors[0]);
+  EXPECT_EQ(Index(-1, 0, 0), neighbors[1]);
+  EXPECT_EQ(Index(1, 0, 0), neighbors[2]);
+  EXPECT_EQ(Index(0, -1, 0), neighbors[3]);
+  EXPECT_EQ(Index(0, 1, 0), neighbors[4]);
+  EXPECT_EQ(Index(0, 0, -1), neighbors[5]);
+  EXPECT_EQ(Index(0, 0, 1), neighbors[6]);
 
   // Long Index search.
   const LongIndex index2(1, 1, 1);
   const auto neighbors2 = search.neighborIndices(index2, false);
   EXPECT_EQ(6, neighbors2.size());
+  EXPECT_EQ(LongIndex(0, 1, 1), neighbors2[0]);
 
   // 18-neighbor search.
   const NeighborSearch search2(18);
@@ -57,7 +64,7 @@ TEST(NeighborUtils, IndicesValid) {
 
   // 26 neighbor search.
   const NeighborSearch search3(26);
-  const auto neighbors4 = search3.neighborIndices(index, false);
+  const auto neighbors4 = search3.neighborIndices(Index(0, 2, -2), false);
   EXPECT_EQ(26, neighbors4.size());
 }
 

tests/utest_voxel_block.cpp

@@ -58,7 +58,7 @@ TEST(VoxelBlock, IndicesValid) {
 }
 
 TEST(VoxelBlock, IndexConversion) {
-  const VoxelBlock<int> block(0.1, 16, BlockIndex(0, 0, 0));
+  const VoxelBlock<int> block(0.1, 16, BlockIndex(1, 2, 3));
 
   // Linear to local.
   EXPECT_EQ(VoxelIndex(0, 0, 0), block.getVoxelIndex(0));
@@ -73,6 +73,10 @@ TEST(VoxelBlock, IndexConversion) {
   EXPECT_EQ(1, block.getLinearIndex(VoxelIndex(1, 0, 0)));
   EXPECT_EQ(16, block.getLinearIndex(VoxelIndex(0, 1, 0)));
   EXPECT_EQ(256, block.getLinearIndex(VoxelIndex(0, 0, 1)));
+
+  // Point to local.
+  EXPECT_EQ(VoxelIndex(0, 0, 0), block.getVoxelIndex(Point(1.6, 3.2, 4.8)));
+  EXPECT_EQ(VoxelIndex(15, 15, 15), block.getVoxelIndex(Point(3.15, 4.75, 6.35)));
 }
 
 TEST(VoxelBlock, Iterator) {