From 2f92b7c25f1d8efe2dae51a80fa1e01e41f202e2 Mon Sep 17 00:00:00 2001 From: lschmid Date: Sun, 16 Jun 2024 13:46:51 -0400 Subject: [PATCH 1/2] add voxel index lookup from position --- include/spatial_hash/voxel_block.h | 7 +++++++ tests/utest_voxel_block.cpp | 6 +++++- 2 files changed, 12 insertions(+), 1 deletion(-) 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) { From 208def8c4d52d73d370501cd7a16fba810fcaee2 Mon Sep 17 00:00:00 2001 From: lschmid Date: Mon, 17 Jun 2024 21:48:35 -0400 Subject: [PATCH 2/2] fix neighbor search and augment utest --- include/spatial_hash/impl/neighbor_utils_impl.h | 16 +++++++--------- include/spatial_hash/neighbor_utils.h | 17 ++++------------- src/neighbor_utils.cpp | 12 +++--------- tests/utest_neighbor_utils.cpp | 9 ++++++++- 4 files changed, 22 insertions(+), 32 deletions(-) diff --git a/include/spatial_hash/impl/neighbor_utils_impl.h b/include/spatial_hash/impl/neighbor_utils_impl.h index f78b46b..d4af827 100644 --- a/include/spatial_hash/impl/neighbor_utils_impl.h +++ b/include/spatial_hash/impl/neighbor_utils_impl.h @@ -42,18 +42,17 @@ namespace spatial_hash { template -BlockNeighborSearch::BlockNeighborSearch(const Layer& layer, - Connectivity connectivity) +BlockNeighborSearch::BlockNeighborSearch(const Layer& layer, size_t connectivity) : NeighborSearch(connectivity), layer_(layer) {} template std::vector NeighborSearch::neighborIndices(const IndexT& index, const bool include_self) const { - std::vector neighbors(static_cast(connectivity) + - static_cast(include_self)); - - for (size_t i = include_self ? 0 : 1; i < neighbors.size(); ++i) { - neighbors[i] = index + kNeighborOffsets.col(i).cast(); + const size_t offset = include_self ? 0 : 1; + std::vector neighbors; + neighbors.reserve(connectivity + offset); + for (size_t i = offset; i <= connectivity; ++i) { + neighbors.emplace_back(index + kNeighborOffsets.col(i).cast()); } return neighbors; } @@ -64,7 +63,6 @@ std::vector BlockNeighborSearch::neighborBlocks( const bool include_self) const { std::vector neighbors; neighbors.reserve(static_cast(connectivity) + static_cast(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 BlockNeighborSearch::neighborBlocks( template VoxelNeighborSearch::VoxelNeighborSearch(const VoxelLayer& layer, - Connectivity connectivity) + size_t connectivity) : NeighborSearch(connectivity), layer_(layer) {} template diff --git a/include/spatial_hash/neighbor_utils.h b/include/spatial_hash/neighbor_utils.h index 493da55..8637b27 100644 --- a/include/spatial_hash/neighbor_utils.h +++ b/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 std::vector 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 struct BlockNeighborSearch : public NeighborSearch { - BlockNeighborSearch(const Layer& layer, Connectivity connectivity); - BlockNeighborSearch(const Layer& layer, uint32_t connectivity) - : BlockNeighborSearch(layer, static_cast(connectivity)) {} - + BlockNeighborSearch(const Layer& layer, size_t connectivity); virtual ~BlockNeighborSearch() = default; /** @@ -104,9 +97,7 @@ template struct VoxelNeighborSearch : public NeighborSearch { using VoxelType = typename BlockT::VoxelType; - VoxelNeighborSearch(const VoxelLayer& layer, Connectivity connectivity); - VoxelNeighborSearch(const VoxelLayer& layer, uint32_t connectivity) - : VoxelNeighborSearch(layer, static_cast(connectivity)) {} + VoxelNeighborSearch(const VoxelLayer& layer, size_t connectivity); virtual ~VoxelNeighborSearch() = default; diff --git a/src/neighbor_utils.cpp b/src/neighbor_utils.cpp index 69395dd..3bbd08a 100644 --- a/src/neighbor_utils.cpp +++ b/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)) { +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(connectivity); + CHECK(connectivity == 6 || connectivity == 18 || connectivity == 26) + << "Invalid connectivity value: " << connectivity << ", must be 6, 18, or 26."; } const Eigen::Matrix NeighborSearch::kNeighborOffsets = [] { diff --git a/tests/utest_neighbor_utils.cpp b/tests/utest_neighbor_utils.cpp index 81eeea9..a51e99d 100644 --- a/tests/utest_neighbor_utils.cpp +++ b/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()); }