add voxel index lookup from position (#2)

MIT-SPARK · Jun 17, 2024 · a010440 · a010440
1 parent d482272
commit a010440
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 1 deletion.
diff --git a/include/spatial_hash/voxel_block.h b/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.

diff --git a/tests/utest_voxel_block.cpp b/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) {