diff --git a/include/spatial_hash/voxel_block.h b/include/spatial_hash/voxel_block.h index 32fd872..7f70bb3 100644 --- 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 index 2772342..b17eedd 100644 --- a/tests/utest_voxel_block.cpp +++ b/tests/utest_voxel_block.cpp @@ -58,7 +58,7 @@ TEST(VoxelBlock, IndicesValid) { } TEST(VoxelBlock, IndexConversion) { - const VoxelBlock block(0.1, 16, BlockIndex(0, 0, 0)); + const VoxelBlock 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) {