├── .flake8 ├── cmake └── spatial_hashConfig.cmake.in ├── tests ├── CMakeLists.txt ├── utest_main.cpp ├── utest_block.cpp ├── utest_grid.cpp ├── utest_voxel_block.cpp ├── utest_neighbor_utils.cpp ├── utest_hash.cpp ├── utest_block_layer.cpp ├── utest_voxel_layer.cpp └── utest_layer.cpp ├── package.xml ├── .cmake-format.yaml ├── .pre-commit-config.yaml ├── LICENSE ├── .github └── workflows │ └── ci.yaml ├── README.md ├── CMakeLists.txt ├── include └── spatial_hash │ ├── impl │ ├── block_layer_impl.h │ ├── voxel_block_impl.h │ ├── neighbor_utils_impl.h │ ├── voxel_layer_impl.h │ └── layer_impl.h │ ├── types.h │ ├── block.h │ ├── hash.h │ ├── block_layer.h │ ├── neighbor_utils.h │ ├── grid.h │ ├── voxel_block.h │ ├── layer.h │ └── voxel_layer.h ├── src ├── block.cpp ├── neighbor_utils.cpp └── grid.cpp ├── .gitignore └── .clang-format /.flake8: -------------------------------------------------------------------------------- 1 | [flake8] 2 | max-line-length = 88 3 | extend-ignore = E203, W503 4 | -------------------------------------------------------------------------------- /cmake/spatial_hashConfig.cmake.in: -------------------------------------------------------------------------------- 1 | get_filename_component(spatial_hash_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) 2 | 3 | find_dependency(Eigen3 REQUIRED) 4 | find_package(PkgConfig REQUIRED) 5 | pkg_check_modules(glog REQUIRED IMPORTED_TARGET libglog) 6 | 7 | if(NOT TARGET spatial_hash::spatial_hash) 8 | include("${spatial_hash_CMAKE_DIR}/spatial_hashTargets.cmake") 9 | endif() 10 | 11 | set(spatial_hash_LIBRARIES spatial_hash::spatial_hash) 12 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(GTest REQUIRED) 2 | 3 | include(GoogleTest) 4 | enable_testing() 5 | 6 | add_executable( 7 | utest_${PROJECT_NAME} 8 | utest_main.cpp 9 | utest_block.cpp 10 | utest_block_layer.cpp 11 | utest_grid.cpp 12 | utest_hash.cpp 13 | utest_layer.cpp 14 | utest_neighbor_utils.cpp 15 | utest_voxel_block.cpp 16 | utest_voxel_layer.cpp 17 | ) 18 | target_include_directories(utest_${PROJECT_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) 19 | target_link_libraries(utest_${PROJECT_NAME} PRIVATE ${PROJECT_NAME} GTest::gtest_main) 20 | gtest_add_tests(TARGET utest_${PROJECT_NAME}) 21 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | spatial_hash 4 | 1.0.0 5 | A lightweight spatial hashing library. 6 | 7 | Lukas Schmid 8 | Lukas Schmid 9 | 10 | BSD 11 | 12 | cmake 13 | eigen 14 | libgoogle-glog-dev 15 | gtest 16 | 17 | 18 | cmake 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /.cmake-format.yaml: -------------------------------------------------------------------------------- 1 | parse: 2 | additional_commands: 3 | catkin_package: 4 | flags: [] 5 | kwargs: 6 | CAKTIN_DEPENDS: '*' 7 | DEPENDS: '*' 8 | INCLUDE_DIRS: '*' 9 | LIBRARIES: '*' 10 | catkin_add_gtest: 11 | pargs: 1 12 | format: 13 | line_width: 88 14 | tab_size: 2 15 | use_tabchars: false 16 | max_subgroups_hwrap: 5 17 | #max_pargs_hwrap: 6 18 | max_rows_cmdline: 2 19 | dangle_parens: true 20 | dangle_align: prefix 21 | min_prefix_chars: 4 22 | max_prefix_chars: 10 23 | max_lines_hwrap: 2 24 | command_case: canonical 25 | keyword_case: unchanged 26 | always_wrap: [add_library] 27 | enable_sort: true 28 | autosort: false 29 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: https://github.com/pre-commit/pre-commit-hooks 3 | rev: v4.4.0 4 | hooks: 5 | - id: check-yaml 6 | - id: check-json 7 | - id: check-xml 8 | - id: end-of-file-fixer 9 | - id: trailing-whitespace 10 | - id: check-added-large-files 11 | - id: check-merge-conflict 12 | 13 | - repo: https://github.com/pre-commit/mirrors-clang-format 14 | rev: "v16.0.6" 15 | hooks: 16 | - id: clang-format 17 | 18 | - repo: https://github.com/cpplint/cpplint 19 | rev: "1.6.1" 20 | hooks: 21 | - id: cpplint 22 | args: 23 | [ 24 | "--filter=-whitespace/line_length,-legal/copyright,-build/include_order,-runtime/references,-build/c++11,-build/namespaces", 25 | ] 26 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024, MIT-SPARK Lab, Massachusetts Institute of Technology. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /.github/workflows/ci.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | name: 'Spatial Hash: Build and Test' 3 | on: 4 | push: 5 | branches: [main] 6 | pull_request: 7 | branches: [main] 8 | jobs: 9 | lint: 10 | runs-on: ubuntu-latest 11 | steps: 12 | - uses: actions/checkout@v4 13 | - name: Dependencies 14 | run: sudo apt-get update && sudo apt install pipx 15 | - name: Lint 16 | run: pipx install pre-commit && cd ${{github.workspace}} && pre-commit run --all-files 17 | cmake: 18 | runs-on: ubuntu-latest 19 | steps: 20 | - uses: actions/checkout@v4 21 | - name: Dependencies 22 | run: sudo apt-get update && sudo apt install libgtest-dev libeigen3-dev libgoogle-glog-dev 23 | - name: Configure 24 | run: cmake -B ${{github.workspace}}/build -DBUILD_TESTING=ON -DCMAKE_BUILD_TYPE=Release 25 | - name: Build 26 | run: cmake --build ${{github.workspace}}/build --config Release 27 | - name: Test 28 | working-directory: ${{github.workspace}}/build 29 | run: ctest -C Release 30 | ros2: 31 | runs-on: ubuntu-latest 32 | container: ros:jazzy-ros-base 33 | steps: 34 | - uses: actions/checkout@v4 35 | with: {path: src/spatial_hash} 36 | - name: Dependencies 37 | run: | 38 | sudo apt-get update 39 | rosdep update --rosdistro jazzy && rosdep install --rosdistro jazzy --from-paths src --ignore-src -r -y 40 | - name: Build 41 | shell: bash 42 | run: | 43 | source /opt/ros/jazzy/setup.bash 44 | colcon build --cmake-args --no-warn-unused-cli -DCMAKE_BUILD_TYPE=Release 45 | - name: Test 46 | shell: bash 47 | run: |- 48 | source /opt/ros/jazzy/setup.bash 49 | colcon test 50 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![Spatial Hash: Build and Test](https://github.com/MIT-SPARK/Spatial-Hash/actions/workflows/ci.yaml/badge.svg)](https://github.com/MIT-SPARK/Spatial-Hash/actions/workflows/ci.yaml) 2 | 3 | # Spatial Hash 4 | A minimal library for spatial data structures based on voxel-block-hashing. 5 | 6 | ## Table of contents 7 | - [Credits](#credits) 8 | - [Installation](#installation) 9 | 10 | ## Credits 11 | This library was inspired by data structures used in [voxblox](https://github.com/ethz-asl/voxblox). 12 | It was developed by [Lukas Schmid](https://schmluk.github.io/) at the [MIT-SPARK Lab](http://mit.edu/sparklab) and is released under a [BSD-3-Clause License](LICENSE)! Additional contributions welcome! This work was supported in part by the Swiss National Science Foundation and Amazon. 13 | 14 | ## Installation 15 | 16 | 1. Install dependencies: 17 | ``` 18 | sudo apt install libeigen3-dev libgoogle-glog-dev libgtest-dev 19 | ``` 20 | Alternatively, if building with catkin or ROS, 21 | ``` 22 | rosdep install --from-paths . --ignore-src -r -y 23 | ``` 24 | will install all required dependencies (from either the `src` directory of the workspace or the repo directory itself). 25 | 26 | 2. Clone repository: 27 | ``` 28 | cd ~/catkin_ws/src 29 | git clone git@github.mit.edu:SPARK/Spatial-Hash.git spatial_hash 30 | ``` 31 | 3. Build & install via cmake: 32 | ``` 33 | cd spatial_hash 34 | mkdir build 35 | cd build 36 | cmake .. 37 | make -j 38 | 39 | # optionally install this package 40 | sudo make install 41 | ``` 42 | 43 | For ROS, build via catkin: 44 | ``` 45 | catkin build spatial_hash 46 | ``` 47 | 48 | 4. Setup pre-commit for contributing: 49 | ``` 50 | pip install pre-commit 51 | pre-commit install 52 | ``` 53 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | project(spatial_hash VERSION 1.0.0) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | set(CMAKE_CXX_EXTENSIONS OFF) 7 | 8 | add_compile_options(-Wall -Wextra) 9 | 10 | find_package(Eigen3 REQUIRED) 11 | find_package(PkgConfig REQUIRED) 12 | pkg_check_modules(glog REQUIRED IMPORTED_TARGET libglog) 13 | 14 | add_library( 15 | ${PROJECT_NAME} 16 | src/block.cpp 17 | src/grid.cpp 18 | src/neighbor_utils.cpp 19 | ) 20 | 21 | target_include_directories( 22 | ${PROJECT_NAME} PUBLIC $ 23 | $ 24 | ) 25 | target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen PkgConfig::glog) 26 | 27 | include(CTest) 28 | if(BUILD_TESTING) 29 | enable_testing() 30 | add_subdirectory(tests) 31 | endif() 32 | 33 | include(GNUInstallDirs) 34 | include(CMakePackageConfigHelpers) 35 | set_property(TARGET ${PROJECT_NAME} PROPERTY POSITION_INDEPENDENT_CODE 1) 36 | add_library( 37 | spatial_hash::${PROJECT_NAME} ALIAS ${PROJECT_NAME} 38 | ) 39 | 40 | install(TARGETS ${PROJECT_NAME} EXPORT spatial_hash-targets 41 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 42 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 43 | ) 44 | install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) 45 | install(EXPORT spatial_hash-targets FILE spatial_hashTargets.cmake NAMESPACE spatial_hash:: 46 | DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/spatial_hash 47 | ) 48 | 49 | write_basic_package_version_file( 50 | ${CMAKE_CURRENT_BINARY_DIR}/spatial_hashConfigVersion.cmake VERSION ${PROJECT_VERSION} 51 | COMPATIBILITY AnyNewerVersion 52 | ) 53 | 54 | configure_package_config_file( 55 | ${CMAKE_CURRENT_LIST_DIR}/cmake/spatial_hashConfig.cmake.in 56 | ${CMAKE_CURRENT_BINARY_DIR}/spatial_hashConfig.cmake 57 | INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/spatial_hash 58 | ) 59 | 60 | install(FILES ${CMAKE_CURRENT_BINARY_DIR}/spatial_hashConfig.cmake 61 | ${CMAKE_CURRENT_BINARY_DIR}/spatial_hashConfigVersion.cmake 62 | DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/spatial_hash 63 | ) 64 | -------------------------------------------------------------------------------- /tests/utest_main.cpp: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #include 36 | 37 | auto main(int argc, char** argv) -> int { 38 | ::testing::InitGoogleTest(&argc, argv); 39 | return RUN_ALL_TESTS(); 40 | } 41 | -------------------------------------------------------------------------------- /include/spatial_hash/impl/block_layer_impl.h: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #pragma once 36 | 37 | #include 38 | #include 39 | 40 | #include "spatial_hash/block_layer.h" 41 | 42 | namespace spatial_hash { 43 | 44 | template 45 | template 46 | typename BlockLayer::BlockPtr BlockLayer::allocateBlockPtr( 47 | const BlockIndex& block_index, 48 | Args&&... args) { 49 | const auto it = this->blocks_.find(block_index); 50 | if (it != this->blocks_.end()) { 51 | return it->second; 52 | } 53 | 54 | auto block = std::make_shared(this->block_grid_.voxel_size, block_index, args...); 55 | this->blocks_[block_index] = block; 56 | return block; 57 | } 58 | 59 | } // namespace spatial_hash 60 | -------------------------------------------------------------------------------- /src/block.cpp: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #include "spatial_hash/block.h" 36 | 37 | namespace spatial_hash { 38 | 39 | Block::Block(float block_size, const BlockIndex& index) : block_size(block_size), index(index) {} 40 | 41 | Block& Block::operator=(const Block& other) { 42 | if (this == &other) { 43 | return *this; 44 | } 45 | 46 | const_cast(block_size) = other.block_size; 47 | const_cast(index) = other.index; 48 | updated = other.updated; 49 | return *this; 50 | } 51 | 52 | Block& Block::operator=(Block&& other) { 53 | if (this == &other) { 54 | return *this; 55 | } 56 | 57 | const_cast(block_size) = other.block_size; 58 | const_cast(index) = other.index; 59 | updated = other.updated; 60 | return *this; 61 | } 62 | 63 | } // namespace spatial_hash 64 | -------------------------------------------------------------------------------- /include/spatial_hash/types.h: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #pragma once 36 | 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | namespace spatial_hash { 43 | 44 | // 3D coordinates in space. 45 | using Point = Eigen::Vector3f; 46 | 47 | // Indices. 48 | using IndexType = int; 49 | using Index = Eigen::Matrix; 50 | using LongIndexType = int64_t; 51 | using LongIndex = Eigen::Matrix; 52 | 53 | // Predefined index types. 54 | using BlockIndex = Index; // Block indices. 55 | using VoxelIndex = Index; // Local voxel indices. 56 | using GlobalIndex = Index; // Global voxel indices. 57 | using VoxelKey = std::pair; // 58 | 59 | // Index containers. 60 | using BlockIndices = std::vector; 61 | using VoxelIndices = std::vector; 62 | using GlobalIndices = std::vector; 63 | using VoxelKeys = std::vector; 64 | 65 | } // namespace spatial_hash 66 | -------------------------------------------------------------------------------- /src/neighbor_utils.cpp: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #include "spatial_hash/neighbor_utils.h" 36 | 37 | namespace spatial_hash { 38 | 39 | NeighborSearch::NeighborSearch(size_t connectivity) : connectivity(connectivity) { 40 | checkConnectivity(); 41 | } 42 | 43 | void NeighborSearch::checkConnectivity() const { 44 | CHECK(connectivity == 6 || connectivity == 18 || connectivity == 26) 45 | << "Invalid connectivity value: " << connectivity << ", must be 6, 18, or 26."; 46 | } 47 | 48 | const Eigen::Matrix NeighborSearch::kNeighborOffsets = [] { 49 | Eigen::Matrix offsets; 50 | // clang-format off 51 | offsets << 0, -1, 1, 0, 0, 0, 0, -1, -1, 1, 1, 0, 0, 0, 0, -1, 1, -1, 1, -1, -1, -1, -1, 1, 1, 1, 1, 52 | 0, 0, 0, -1, 1, 0, 0, -1, 1, -1, 1, -1, -1, 1, 1, 0, 0, 0, 0, -1, -1, 1, 1, -1, -1, 1, 1, 53 | 0, 0, 0, 0, 0, -1, 1, 0, 0, 0, 0, -1, 1, -1, 1, -1, -1, 1, 1, -1, 1, -1, 1, -1, 1, -1, 1; 54 | // clang-format on 55 | return offsets; 56 | }(); 57 | 58 | } // namespace spatial_hash 59 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # Created by https://www.toptal.com/developers/gitignore/api/python,c++ 3 | # Edit at https://www.toptal.com/developers/gitignore?templates=python,c++ 4 | 5 | ### C++ ### 6 | # Prerequisites 7 | *.d 8 | 9 | # Compiled Object files 10 | *.slo 11 | *.lo 12 | *.o 13 | *.obj 14 | 15 | # Precompiled Headers 16 | *.gch 17 | *.pch 18 | 19 | # Linker files 20 | *.ilk 21 | 22 | # Debugger Files 23 | *.pdb 24 | 25 | # Compiled Dynamic libraries 26 | *.so 27 | *.dylib 28 | *.dll 29 | 30 | # Fortran module files 31 | *.mod 32 | *.smod 33 | 34 | # Compiled Static libraries 35 | *.lai 36 | *.la 37 | *.a 38 | *.lib 39 | 40 | # Executables 41 | *.exe 42 | *.out 43 | *.app 44 | 45 | ### Python ### 46 | # Byte-compiled / optimized / DLL files 47 | __pycache__/ 48 | *.py[cod] 49 | *$py.class 50 | 51 | # C extensions 52 | 53 | # Distribution / packaging 54 | .Python 55 | build/ 56 | develop-eggs/ 57 | dist/ 58 | downloads/ 59 | eggs/ 60 | .eggs/ 61 | lib/ 62 | lib64/ 63 | parts/ 64 | sdist/ 65 | var/ 66 | wheels/ 67 | pip-wheel-metadata/ 68 | share/python-wheels/ 69 | *.egg-info/ 70 | .installed.cfg 71 | *.egg 72 | MANIFEST 73 | 74 | # PyInstaller 75 | # Usually these files are written by a python script from a template 76 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 77 | *.manifest 78 | *.spec 79 | 80 | # Installer logs 81 | pip-log.txt 82 | pip-delete-this-directory.txt 83 | 84 | # Unit test / coverage reports 85 | htmlcov/ 86 | .tox/ 87 | .nox/ 88 | .coverage 89 | .coverage.* 90 | .cache 91 | nosetests.xml 92 | coverage.xml 93 | *.cover 94 | *.py,cover 95 | .hypothesis/ 96 | .pytest_cache/ 97 | pytestdebug.log 98 | 99 | # Translations 100 | *.mo 101 | *.pot 102 | 103 | # Django stuff: 104 | *.log 105 | local_settings.py 106 | db.sqlite3 107 | db.sqlite3-journal 108 | 109 | # Flask stuff: 110 | instance/ 111 | .webassets-cache 112 | 113 | # Scrapy stuff: 114 | .scrapy 115 | 116 | # Sphinx documentation 117 | docs/_build/ 118 | doc/_build/ 119 | 120 | # PyBuilder 121 | target/ 122 | 123 | # Jupyter Notebook 124 | .ipynb_checkpoints 125 | 126 | # IPython 127 | profile_default/ 128 | ipython_config.py 129 | 130 | # pyenv 131 | .python-version 132 | 133 | # pipenv 134 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 135 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 136 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 137 | # install all needed dependencies. 138 | #Pipfile.lock 139 | 140 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 141 | __pypackages__/ 142 | 143 | # Celery stuff 144 | celerybeat-schedule 145 | celerybeat.pid 146 | 147 | # SageMath parsed files 148 | *.sage.py 149 | 150 | # Environments 151 | .env 152 | .venv 153 | env/ 154 | venv/ 155 | ENV/ 156 | env.bak/ 157 | venv.bak/ 158 | pythonenv* 159 | 160 | # Spyder project settings 161 | .spyderproject 162 | .spyproject 163 | 164 | # Rope project settings 165 | .ropeproject 166 | 167 | # mkdocs documentation 168 | /site 169 | 170 | # mypy 171 | .mypy_cache/ 172 | .dmypy.json 173 | dmypy.json 174 | 175 | # Pyre type checker 176 | .pyre/ 177 | 178 | # pytype static type analyzer 179 | .pytype/ 180 | 181 | # profiling data 182 | .prof 183 | 184 | # IDEs 185 | .vscode/ 186 | 187 | compile_commands.json 188 | install 189 | 190 | # Catkin test outputs 191 | Testing/ 192 | -------------------------------------------------------------------------------- /tests/utest_block.cpp: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | namespace spatial_hash { 41 | 42 | TEST(Block, Copy) { 43 | struct TestBlock : public Block { 44 | TestBlock(float block_size, const BlockIndex& index) : Block(block_size, index) {} 45 | int data = 0; 46 | }; 47 | 48 | TestBlock block(1.0f, BlockIndex(1, 2, 3)); 49 | block.updated = true; 50 | block.data = 42; 51 | 52 | // Copy constructor. 53 | TestBlock block_copy(block); 54 | EXPECT_EQ(block_copy.index, block.index); 55 | EXPECT_EQ(block_copy.block_size, block.block_size); 56 | EXPECT_EQ(block_copy.updated, block.updated); 57 | EXPECT_EQ(block_copy.data, block.data); 58 | 59 | // Copy assignment. 60 | TestBlock block_copy_assign(2.0f, BlockIndex(4, 5, 6)); 61 | block_copy_assign = block; 62 | EXPECT_EQ(block_copy_assign.index, block.index); 63 | EXPECT_EQ(block_copy_assign.block_size, block.block_size); 64 | EXPECT_EQ(block_copy_assign.updated, block.updated); 65 | EXPECT_EQ(block_copy_assign.data, block.data); 66 | 67 | // Move constructor. 68 | TestBlock block_move(std::move(block)); 69 | EXPECT_EQ(block_move.index, block_copy.index); 70 | EXPECT_EQ(block_move.block_size, block_copy.block_size); 71 | EXPECT_EQ(block_move.updated, block_copy.updated); 72 | EXPECT_EQ(block_move.data, block_copy.data); 73 | 74 | // Move assignment. 75 | TestBlock block_move_assign(3.0f, BlockIndex(7, 8, 9)); 76 | block_move_assign = std::move(block_copy); 77 | EXPECT_EQ(block_move_assign.index, block_copy_assign.index); 78 | EXPECT_EQ(block_move_assign.block_size, block_copy_assign.block_size); 79 | EXPECT_EQ(block_move_assign.updated, block_copy_assign.updated); 80 | EXPECT_EQ(block_move_assign.data, block_copy_assign.data); 81 | } 82 | 83 | } // namespace spatial_hash 84 | -------------------------------------------------------------------------------- /include/spatial_hash/impl/voxel_block_impl.h: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #pragma once 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | #include "spatial_hash/voxel_block.h" 44 | 45 | namespace spatial_hash { 46 | 47 | template 48 | VoxelBlock::VoxelBlock(float voxel_size, size_t voxels_per_side, const BlockIndex& index) 49 | : IndexGrid(voxel_size), 50 | Block(voxel_size * voxels_per_side, index), 51 | voxels_per_side(voxels_per_side), 52 | voxels(numVoxels()) { 53 | CHECK((voxels_per_side & (voxels_per_side - 1)) == 0) << "voxels_per_side must be a power of 2"; 54 | } 55 | 56 | template 57 | VoxelBlock::VoxelBlock(const VoxelBlock& other) 58 | : IndexGrid(other), 59 | Block(other), 60 | voxels_per_side(other.voxels_per_side), 61 | voxels(other.voxels) {} 62 | 63 | template 64 | VoxelBlock::VoxelBlock(VoxelBlock&& other) 65 | : IndexGrid(std::move(other)), 66 | Block(std::move(other)), 67 | voxels_per_side(std::move(other.voxels_per_side)), 68 | voxels(std::move(other.voxels)) {} 69 | 70 | template 71 | VoxelBlock& VoxelBlock::operator=(const VoxelBlock& other) { 72 | IndexGrid::operator=(other); 73 | Block::operator=(other); 74 | const_cast(voxels_per_side) = other.voxels_per_side; 75 | voxels = other.voxels; 76 | return *this; 77 | } 78 | 79 | template 80 | VoxelBlock& VoxelBlock::operator=(VoxelBlock&& other) { 81 | IndexGrid::operator=(std::move(other)); 82 | Block::operator=(std::move(other)); 83 | const_cast(voxels_per_side) = std::move(other.voxels_per_side); 84 | voxels = std::move(other.voxels); 85 | return *this; 86 | } 87 | 88 | } // namespace spatial_hash 89 | -------------------------------------------------------------------------------- /include/spatial_hash/block.h: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #pragma once 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | #include "spatial_hash/grid.h" 44 | #include "spatial_hash/types.h" 45 | 46 | namespace spatial_hash { 47 | 48 | /** 49 | * @brief Base data structure for data to be stored in a BlockGrid. 50 | */ 51 | struct Block { 52 | // Types. 53 | using Ptr = std::shared_ptr; 54 | using ConstPtr = std::shared_ptr; 55 | 56 | // Constructors. 57 | /** 58 | * @brief Construct a block with a given block size and its index in the grid. 59 | * @param block_size The side length of the block. 60 | * @param index Index of the block in the grid. 61 | */ 62 | Block(float block_size, const BlockIndex& index); 63 | 64 | Block(const Block& other) = default; 65 | 66 | Block(Block&& other) = default; 67 | 68 | virtual ~Block() = default; 69 | 70 | Block& operator=(const Block& other); 71 | 72 | Block& operator=(Block&& other); 73 | 74 | // Config. 75 | // Number of voxels per side of the block. 76 | const float block_size; 77 | 78 | // Index of the block in the grid 79 | const BlockIndex index; 80 | 81 | // Attributes. 82 | // Flag indicating if the block has been updated. Can be set or unset by any user. 83 | mutable bool updated = false; 84 | 85 | // Block info. 86 | /** 87 | * @brief Get the inverse of the block size. 88 | */ 89 | float blockSizeInv() const { return 1.0f / block_size; } 90 | 91 | /** 92 | * @brief Get the center point of the block. 93 | */ 94 | Point position() const { return centerPointFromIndex(index, block_size); } 95 | 96 | /** 97 | * @brief Get the origin point of the block. 98 | */ 99 | Point origin() const { return originPointFromIndex(index, block_size); } 100 | }; 101 | 102 | } // namespace spatial_hash 103 | -------------------------------------------------------------------------------- /include/spatial_hash/hash.h: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #pragma once 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include "spatial_hash/types.h" 43 | 44 | namespace spatial_hash { 45 | 46 | /** 47 | * @brief Hash function for indices. 48 | */ 49 | struct IndexHash { 50 | // 1290 is the maximum number to fill a 32-bit integer. Afterwards, collisions will occur through 51 | // overflow. 52 | static constexpr unsigned int s1 = 1290; 53 | static constexpr unsigned int s2 = s1 * s1; 54 | 55 | int operator()(const Index& index) const { 56 | return static_cast(index.x()) + static_cast(index.y()) * s1 + 57 | static_cast(index.z()) * s2; 58 | } 59 | }; 60 | 61 | /** 62 | * @brief Hash function for long indices. 63 | */ 64 | struct LongIndexHash { 65 | // 2097152 is the maximum number to fill a 64-bit integer. Afterwards, collisions will occur 66 | // through overflow. 67 | static constexpr int64_t s1 = 2097152; 68 | static constexpr int64_t s2 = s1 * s1; 69 | 70 | int64_t operator()(const LongIndex& index) const { 71 | return index.x() + index.y() * s1 + index.z() * s2; 72 | } 73 | }; 74 | 75 | template 76 | using IndexHashMap = 77 | std::unordered_map, 81 | Eigen::aligned_allocator>>; 82 | 83 | template 84 | using LongIndexHashMap = 85 | std::unordered_map, 89 | Eigen::aligned_allocator>>; 90 | using IndexSet = 91 | std::unordered_set, Eigen::aligned_allocator>; 92 | 93 | using LongIndexSet = std::unordered_set, 96 | Eigen::aligned_allocator>; 97 | 98 | } // namespace spatial_hash 99 | -------------------------------------------------------------------------------- /src/grid.cpp: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #include "spatial_hash/grid.h" 36 | 37 | namespace spatial_hash { 38 | 39 | BlockIndex blockIndexFromGlobalIndex(const GlobalIndex& global_index, 40 | const size_t voxels_per_side) { 41 | return BlockIndex(global_index.x() / voxels_per_side, 42 | global_index.y() / voxels_per_side, 43 | global_index.z() / voxels_per_side); 44 | } 45 | 46 | VoxelIndex localIndexFromGlobalIndex(const GlobalIndex& global_index, 47 | const size_t voxels_per_side) { 48 | return VoxelIndex(global_index.x() % voxels_per_side, 49 | global_index.y() % voxels_per_side, 50 | global_index.z() % voxels_per_side); 51 | } 52 | 53 | GlobalIndex globalIndexFromLocalIndices(const BlockIndex& block_index, 54 | const VoxelIndex& local_index, 55 | const size_t voxels_per_side) { 56 | return GlobalIndex(local_index.x() + block_index.x() * voxels_per_side, 57 | local_index.y() + block_index.y() * voxels_per_side, 58 | local_index.z() + block_index.z() * voxels_per_side); 59 | } 60 | 61 | GlobalIndex globalIndexFromKey(const VoxelKey& key, const size_t voxels_per_side) { 62 | return globalIndexFromLocalIndices(key.first, key.second, voxels_per_side); 63 | } 64 | 65 | VoxelKey keyFromGlobalIndex(const GlobalIndex& global_index, const size_t voxels_per_side) { 66 | return VoxelKey(blockIndexFromGlobalIndex(global_index, voxels_per_side), 67 | localIndexFromGlobalIndex(global_index, voxels_per_side)); 68 | } 69 | 70 | VoxelIndex voxelIndexFromLinearIndex(const size_t linear_index, const size_t voxels_per_side) { 71 | int rem = linear_index; 72 | VoxelIndex result; 73 | std::div_t div_temp = std::div(rem, voxels_per_side * voxels_per_side); 74 | rem = div_temp.rem; 75 | result.z() = div_temp.quot; 76 | div_temp = std::div(rem, voxels_per_side); 77 | result.y() = div_temp.quot; 78 | result.x() = div_temp.rem; 79 | return result; 80 | } 81 | 82 | size_t linearIndexFromVoxelIndex(const VoxelIndex& index, const size_t voxels_per_side) { 83 | return index.x() + voxels_per_side * (index.y() + index.z() * voxels_per_side); 84 | } 85 | 86 | } // namespace spatial_hash 87 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | AccessModifierOffset: -1 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: false 7 | AlignConsecutiveDeclarations: false 8 | AlignOperands: true 9 | AlignTrailingComments: true 10 | AllowAllParametersOfDeclarationOnNextLine: false 11 | AllowShortBlocksOnASingleLine: false 12 | AllowShortCaseLabelsOnASingleLine: false 13 | AllowShortFunctionsOnASingleLine: All 14 | AllowShortIfStatementsOnASingleLine: true 15 | AllowShortLoopsOnASingleLine: true 16 | AlwaysBreakAfterDefinitionReturnType: None 17 | AlwaysBreakAfterReturnType: None 18 | AlwaysBreakBeforeMultilineStrings: true 19 | AlwaysBreakTemplateDeclarations: true 20 | BinPackArguments: false 21 | BinPackParameters: false 22 | BraceWrapping: 23 | AfterClass: false 24 | AfterControlStatement: false 25 | AfterEnum: false 26 | AfterFunction: false 27 | AfterNamespace: false 28 | AfterObjCDeclaration: false 29 | AfterStruct: false 30 | AfterUnion: false 31 | BeforeCatch: false 32 | BeforeElse: false 33 | IndentBraces: false 34 | BreakBeforeBinaryOperators: None 35 | BreakBeforeBraces: Attach 36 | BreakBeforeTernaryOperators: true 37 | BreakConstructorInitializersBeforeComma: false 38 | CommentPragmas: "^ IWYU pragma:" 39 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 40 | ConstructorInitializerIndentWidth: 4 41 | ContinuationIndentWidth: 4 42 | Cpp11BracedListStyle: true 43 | DerivePointerAlignment: true 44 | DisableFormat: false 45 | ExperimentalAutoDetectBinPacking: false 46 | ForEachMacros: 47 | - foreach 48 | - Q_FOREACH 49 | - BOOST_FOREACH 50 | IncludeCategories: 51 | # Spacers 52 | - Regex: "^$" 53 | Priority: 15 54 | - Regex: "^$" 55 | Priority: 25 56 | - Regex: "^$" 57 | Priority: 35 58 | - Regex: "^$" 59 | Priority: 45 60 | # C system headers 61 | - Regex: '^[<"](aio|arpa/inet|assert|complex|cpio|ctype|curses|dirent|dlfcn|errno|fcntl|fenv|float|fmtmsg|fnmatch|ftw|glob|grp|iconv|inttypes|iso646|langinfo|libgen|limits|locale|math|monetary|mqueue|ndbm|netdb|net/if|netinet/in|netinet/tcp|nl_types|poll|pthread|pwd|regex|sched|search|semaphore|setjmp|signal|spawn|stdalign|stdarg|stdatomic|stdbool|stddef|stdint|stdio|stdlib|stdnoreturn|string|strings|stropts|sys/ipc|syslog|sys/mman|sys/msg|sys/resource|sys/select|sys/sem|sys/shm|sys/socket|sys/stat|sys/statvfs|sys/time|sys/times|sys/types|sys/uio|sys/un|sys/utsname|sys/wait|tar|term|termios|tgmath|threads|time|trace|uchar|ulimit|uncntrl|unistd|utime|utmpx|wchar|wctype|wordexp)\.h[">]$' 62 | Priority: 10 63 | # C++ system headers 64 | - Regex: '^[<"](algorithm|any|array|atomic|bitset|cassert|ccomplex|cctype|cerrno|cfenv|cfloat|charconv|chrono|cinttypes|ciso646|climits|clocale|cmath|codecvt|complex|condition_variable|csetjmp|csignal|cstdalign|cstdarg|cstdbool|cstddef|cstdint|cstdio|cstdlib|cstring|ctgmath|ctime|cuchar|cwchar|cwctype|deque|exception|execution|filesystem|forward_list|fstream|functional|future|initializer_list|iomanip|ios|iosfwd|iostream|istream|iterator|limits|list|locale|map|memory|memory_resource|mutex|new|numeric|optional|ostream|queue|random|ratio|regex|scoped_allocator|set|shared_mutex|sstream|stack|stdexcept|streambuf|string|string_view|strstream|system_error|thread|tuple|type_traits|typeindex|typeinfo|unordered_map|unordered_set|utility|valarray|variant|vector)[">]$' 65 | Priority: 20 66 | # Other library h files (with angles) 67 | - Regex: "^<" 68 | Priority: 30 69 | # Your project's h files (with angles) 70 | - Regex: "^ 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #pragma once 36 | 37 | #include 38 | #include 39 | 40 | #include "spatial_hash/neighbor_utils.h" 41 | 42 | namespace spatial_hash { 43 | 44 | template 45 | BlockNeighborSearch::BlockNeighborSearch(const Layer& layer, size_t connectivity) 46 | : NeighborSearch(connectivity), layer_(layer) {} 47 | 48 | template 49 | std::vector NeighborSearch::neighborIndices(const IndexT& index, 50 | const bool include_self) const { 51 | const size_t offset = include_self ? 0 : 1; 52 | std::vector neighbors; 53 | neighbors.reserve(connectivity + offset); 54 | for (size_t i = offset; i <= connectivity; ++i) { 55 | neighbors.emplace_back(index + kNeighborOffsets.col(i).cast()); 56 | } 57 | return neighbors; 58 | } 59 | 60 | template 61 | std::vector BlockNeighborSearch::neighborBlocks( 62 | const BlockIndex& block_index, 63 | const bool include_self) const { 64 | std::vector neighbors; 65 | neighbors.reserve(static_cast(connectivity) + static_cast(include_self)); 66 | for (const auto& index : neighborIndices(block_index, include_self)) { 67 | const auto block = layer_.getBlockPtr(index); 68 | if (block) { 69 | neighbors.push_back(block.get()); 70 | } 71 | } 72 | 73 | return neighbors; 74 | } 75 | 76 | template 77 | VoxelNeighborSearch::VoxelNeighborSearch(const VoxelLayer& layer, 78 | size_t connectivity) 79 | : NeighborSearch(connectivity), layer_(layer) {} 80 | 81 | template 82 | std::vector VoxelNeighborSearch::neighborKeys(const VoxelKey& key, 83 | const bool include_self) const { 84 | std::vector neighbors; 85 | neighbors.reserve(static_cast(connectivity) + static_cast(include_self)); 86 | for (const auto& neighbor_index : 87 | neighborIndices(globalIndexFromKey(key, layer_.voxels_per_side), include_self)) { 88 | neighbors.push_back(keyFromGlobalIndex(neighbor_index, layer_.voxels_per_side)); 89 | } 90 | return neighbors; 91 | } 92 | 93 | template 94 | std::vector::VoxelType*> 95 | VoxelNeighborSearch::neighborVoxels(const GlobalIndex& index, 96 | const bool include_self) const { 97 | std::vector neighbors; 98 | neighbors.reserve(static_cast(connectivity) + static_cast(include_self)); 99 | for (const auto& neighbor_index : neighborIndices(index, include_self)) { 100 | const auto voxel = layer_.getVoxelPtr(neighbor_index); 101 | if (voxel) { 102 | neighbors.push_back(voxel); 103 | } 104 | } 105 | return neighbors; 106 | } 107 | 108 | } // namespace spatial_hash 109 | -------------------------------------------------------------------------------- /include/spatial_hash/impl/voxel_layer_impl.h: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #pragma once 36 | 37 | #include 38 | #include 39 | 40 | #include "spatial_hash/voxel_layer.h" 41 | 42 | namespace spatial_hash { 43 | 44 | template 45 | VoxelLayer& VoxelLayer::operator=(const VoxelLayer& other) { 46 | if (this == &other) { 47 | return *this; 48 | } 49 | 50 | const_cast(voxels_per_side) = other.voxels_per_side; 51 | Grid::operator=(other); 52 | BlockLayer::operator=(other); 53 | return *this; 54 | } 55 | 56 | template 57 | VoxelLayer& VoxelLayer::operator=(VoxelLayer&& other) { 58 | if (this == &other) { 59 | return *this; 60 | } 61 | 62 | const_cast(voxels_per_side) = other.voxels_per_side; 63 | Grid::operator=(other); 64 | BlockLayer::operator=(other); 65 | return *this; 66 | } 67 | 68 | template 69 | typename VoxelLayer::VoxelType* VoxelLayer::getVoxelPtr( 70 | const GlobalIndex& voxel_index) { 71 | const BlockIndex block_index = blockIndexFromGlobalIndex(voxel_index, voxels_per_side); 72 | const auto it = this->blocks_.find(block_index); 73 | if (it == this->blocks_.end()) { 74 | return nullptr; 75 | } 76 | 77 | const VoxelIndex local_index = localIndexFromGlobalIndex(voxel_index, voxels_per_side); 78 | return &it->second->getVoxel(local_index); 79 | } 80 | 81 | template 82 | const typename VoxelLayer::VoxelType* VoxelLayer::getVoxelPtr( 83 | const GlobalIndex& voxel_index) const { 84 | const BlockIndex block_index = blockIndexFromGlobalIndex(voxel_index, voxels_per_side); 85 | const auto it = this->blocks_.find(block_index); 86 | if (it == this->blocks_.end()) { 87 | return nullptr; 88 | } 89 | 90 | const VoxelIndex local_index = localIndexFromGlobalIndex(voxel_index, voxels_per_side); 91 | return &it->second->getVoxel(local_index); 92 | } 93 | 94 | template 95 | typename VoxelLayer::VoxelType* VoxelLayer::getVoxelPtr(const VoxelKey& key) { 96 | const auto it = this->blocks_.find(key.first); 97 | if (it == this->blocks_.end()) { 98 | return nullptr; 99 | } 100 | return &it->second->getVoxel(key.second); 101 | } 102 | 103 | template 104 | const typename VoxelLayer::VoxelType* VoxelLayer::getVoxelPtr( 105 | const VoxelKey& key) const { 106 | const auto it = this->blocks_.find(key.first); 107 | if (it == this->blocks_.end()) { 108 | return nullptr; 109 | } 110 | return &it->second->getVoxel(key.second); 111 | } 112 | 113 | template 114 | template 115 | typename VoxelLayer::BlockPtr VoxelLayer::allocateBlockPtr( 116 | const BlockIndex& block_index, 117 | Args&&... args) { 118 | const auto it = this->blocks_.find(block_index); 119 | if (it != this->blocks_.end()) { 120 | return it->second; 121 | } 122 | 123 | auto block = std::make_shared(voxel_size, voxels_per_side, block_index, args...); 124 | this->blocks_[block_index] = block; 125 | return block; 126 | } 127 | 128 | } // namespace spatial_hash 129 | -------------------------------------------------------------------------------- /include/spatial_hash/block_layer.h: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #pragma once 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | #include "spatial_hash/block.h" 44 | #include "spatial_hash/grid.h" 45 | #include "spatial_hash/hash.h" 46 | #include "spatial_hash/layer.h" 47 | #include "spatial_hash/types.h" 48 | 49 | namespace spatial_hash { 50 | 51 | /** 52 | * @brief Layer containing data blocks, where BlockT must be a subclass of Block and have 53 | * the same constructor as Block. 54 | * @tparam BlockT The type of blocks contained in the layer. Must be a subclass of Block and 55 | * have the same constructor as Block. 56 | */ 57 | template 58 | class BlockLayer : public Layer { 59 | public: 60 | // Types. 61 | using Ptr = std::shared_ptr; 62 | using ConstPtr = std::shared_ptr; 63 | using BlockType = BlockT; 64 | using BlockPtr = std::shared_ptr; 65 | 66 | // Constructors. 67 | /** 68 | * @brief Construct a layer of voxel blocks. 69 | * @param voxel_size The side length of each voxel. 70 | * @param voxels_per_side The number of voxels per side of each block. Must be a power of 2. 71 | */ 72 | explicit BlockLayer(float block_size) : Layer(block_size) { 73 | static_assert(std::is_base_of::value, 74 | "BlockT must be a subclass of Block to create a BlockLayer."); 75 | } 76 | 77 | virtual ~BlockLayer() = default; 78 | 79 | // NOTE(lschmid): This is the only shadowing function since allocation requires different 80 | // constructor arguments. 81 | /** 82 | * @brief Gets the block at a given index or allocates a new block if it does not exist. 83 | * @param block_index The block index to retrieve or allocate. 84 | * @param args Additional arguments to pass to the block constructor. 85 | * @tparam Args The types of the additional arguments. 86 | */ 87 | template 88 | BlockT& allocateBlock(const BlockIndex& block_index, Args&&... args) { 89 | return *allocateBlockPtr(block_index, std::forward(args)...); 90 | } 91 | 92 | /** 93 | * @brief Gets the block at a given index or allocates a new block if it does not exist. 94 | * @param block_index The block index to retrieve or allocate. 95 | * @param args Additional arguments to pass to the block constructor. 96 | * @tparam Args The types of the additional arguments. 97 | */ 98 | template 99 | BlockPtr allocateBlockPtr(const BlockIndex& block_index, Args&&... args); 100 | 101 | /** 102 | * @brief Get the indices of all blocks with the updated flag set to true. 103 | */ 104 | BlockIndices updatedBlockIndices() const { 105 | return this->blockIndicesWithCondition(kUpdatedCondition); 106 | } 107 | 108 | /** 109 | * @brief Get pointers to all blocks with the updated flag set to true. 110 | */ 111 | std::vector updatedBlocks() { return this->blocksWithCondition(kUpdatedCondition); } 112 | std::vector updatedBlocks() const { 113 | return this->blocksWithCondition(kUpdatedCondition); 114 | } 115 | 116 | protected: 117 | inline static const std::function kUpdatedCondition = 118 | [](const BlockT& block) { return block.updated; }; 119 | }; 120 | 121 | } // namespace spatial_hash 122 | 123 | #include "spatial_hash/impl/block_layer_impl.h" 124 | -------------------------------------------------------------------------------- /tests/utest_grid.cpp: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #include 36 | #include 37 | 38 | namespace spatial_hash { 39 | 40 | TEST(Grid, LinearIndices) { 41 | // Test conversion between linear and voxel indices. 42 | const size_t voxel_per_side = 16; 43 | VoxelIndex voxel_index(0, 0, 0); 44 | size_t linear_index = 0; 45 | EXPECT_EQ(voxel_index, voxelIndexFromLinearIndex(linear_index, voxel_per_side)); 46 | EXPECT_EQ(linear_index, linearIndexFromVoxelIndex(voxel_index, voxel_per_side)); 47 | 48 | voxel_index = VoxelIndex(5, 0, 0); 49 | linear_index = 5; 50 | EXPECT_EQ(voxel_index, voxelIndexFromLinearIndex(linear_index, voxel_per_side)); 51 | EXPECT_EQ(linear_index, linearIndexFromVoxelIndex(voxel_index, voxel_per_side)); 52 | 53 | voxel_index = VoxelIndex(0, 5, 0); 54 | linear_index = 5 * voxel_per_side; 55 | EXPECT_EQ(voxel_index, voxelIndexFromLinearIndex(linear_index, voxel_per_side)); 56 | EXPECT_EQ(linear_index, linearIndexFromVoxelIndex(voxel_index, voxel_per_side)); 57 | 58 | voxel_index = VoxelIndex(0, 0, 5); 59 | linear_index = 5 * voxel_per_side * voxel_per_side; 60 | EXPECT_EQ(voxel_index, voxelIndexFromLinearIndex(linear_index, voxel_per_side)); 61 | EXPECT_EQ(linear_index, linearIndexFromVoxelIndex(voxel_index, voxel_per_side)); 62 | 63 | voxel_index = VoxelIndex(2, 3, 4); 64 | linear_index = 2 + 3 * voxel_per_side + 4 * voxel_per_side * voxel_per_side; 65 | EXPECT_EQ(voxel_index, voxelIndexFromLinearIndex(linear_index, voxel_per_side)); 66 | EXPECT_EQ(linear_index, linearIndexFromVoxelIndex(voxel_index, voxel_per_side)); 67 | } 68 | 69 | TEST(Grid, GlobalIndices) { 70 | // Test block index from global index. 71 | const size_t voxels_per_side = 16; 72 | 73 | // Trivial index. 74 | GlobalIndex global_index(0, 0, 0); 75 | EXPECT_EQ(BlockIndex(0, 0, 0), blockIndexFromGlobalIndex(global_index, voxels_per_side)); 76 | EXPECT_EQ(VoxelIndex(0, 0, 0), localIndexFromGlobalIndex(global_index, voxels_per_side)); 77 | 78 | // Non-trivial index. 79 | global_index = GlobalIndex(1, 17, 33); 80 | EXPECT_EQ(BlockIndex(0, 1, 2), blockIndexFromGlobalIndex(global_index, voxels_per_side)); 81 | EXPECT_EQ(VoxelIndex(1, 1, 1), localIndexFromGlobalIndex(global_index, voxels_per_side)); 82 | 83 | // Corner. 84 | global_index = GlobalIndex(16, 16, 16); 85 | EXPECT_EQ(BlockIndex(1, 1, 1), blockIndexFromGlobalIndex(global_index, voxels_per_side)); 86 | EXPECT_EQ(VoxelIndex(0, 0, 0), localIndexFromGlobalIndex(global_index, voxels_per_side)); 87 | 88 | // Get global index from local index. 89 | EXPECT_EQ(GlobalIndex(0, 0, 0), 90 | globalIndexFromLocalIndices(BlockIndex(0, 0, 0), VoxelIndex(0, 0, 0), 16)); 91 | EXPECT_EQ(GlobalIndex(1, 17, 33), 92 | globalIndexFromLocalIndices(BlockIndex(0, 1, 2), VoxelIndex(1, 1, 1), 16)); 93 | EXPECT_EQ(GlobalIndex(16, 16, 16), 94 | globalIndexFromLocalIndices(BlockIndex(1, 1, 1), VoxelIndex(0, 0, 0), 16)); 95 | } 96 | 97 | TEST(Grid, Positions) { 98 | // Test conversion between index and 3D position. 99 | const float voxel_size = 0.1; 100 | 101 | BlockIndex block_index(0, 0, 0); 102 | Point position(0.05, 0.05, 0.05); 103 | EXPECT_EQ(position, centerPointFromIndex(block_index, voxel_size)); 104 | EXPECT_EQ(block_index, indexFromPoint(position, voxel_size)); 105 | 106 | block_index = BlockIndex(1, 2, 3); 107 | position = Point(0.15, 0.25, 0.35); 108 | EXPECT_EQ(position, centerPointFromIndex(block_index, voxel_size)); 109 | EXPECT_EQ(block_index, indexFromPoint(position, 1.0f / voxel_size)); 110 | } 111 | 112 | } // namespace spatial_hash 113 | -------------------------------------------------------------------------------- /tests/utest_voxel_block.cpp: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #include 36 | #include 37 | 38 | namespace spatial_hash { 39 | 40 | TEST(VoxelBlock, IndicesValid) { 41 | const VoxelBlock block(0.1, 16, BlockIndex(0, 0, 0)); 42 | 43 | // Linear Index. 44 | EXPECT_TRUE(block.isValidLinearIndex(0)); 45 | EXPECT_TRUE(block.isValidLinearIndex(4095)); 46 | EXPECT_FALSE(block.isValidLinearIndex(4096)); 47 | EXPECT_FALSE(block.isValidLinearIndex(-1)); 48 | 49 | // Local voxel Index. 50 | EXPECT_TRUE(block.isValidVoxelIndex(VoxelIndex(0, 0, 0))); 51 | EXPECT_TRUE(block.isValidVoxelIndex(VoxelIndex(15, 15, 15))); 52 | EXPECT_FALSE(block.isValidVoxelIndex(VoxelIndex(16, 15, 15))); 53 | EXPECT_FALSE(block.isValidVoxelIndex(VoxelIndex(15, 16, 15))); 54 | EXPECT_FALSE(block.isValidVoxelIndex(VoxelIndex(15, 15, 16))); 55 | EXPECT_FALSE(block.isValidVoxelIndex(VoxelIndex(-1, 15, 15))); 56 | EXPECT_FALSE(block.isValidVoxelIndex(VoxelIndex(15, -1, 15))); 57 | EXPECT_FALSE(block.isValidVoxelIndex(VoxelIndex(15, 15, -1))); 58 | } 59 | 60 | TEST(VoxelBlock, IndexConversion) { 61 | const VoxelBlock block(0.1, 16, BlockIndex(1, 2, 3)); 62 | 63 | // Linear to local. 64 | EXPECT_EQ(VoxelIndex(0, 0, 0), block.getVoxelIndex(0)); 65 | EXPECT_EQ(VoxelIndex(15, 15, 15), block.getVoxelIndex(4095)); 66 | EXPECT_EQ(VoxelIndex(1, 0, 0), block.getVoxelIndex(1)); 67 | EXPECT_EQ(VoxelIndex(0, 1, 0), block.getVoxelIndex(16)); 68 | EXPECT_EQ(VoxelIndex(0, 0, 1), block.getVoxelIndex(256)); 69 | 70 | // Local to linear. 71 | EXPECT_EQ(0, block.getLinearIndex(VoxelIndex(0, 0, 0))); 72 | EXPECT_EQ(4095, block.getLinearIndex(VoxelIndex(15, 15, 15))); 73 | EXPECT_EQ(1, block.getLinearIndex(VoxelIndex(1, 0, 0))); 74 | EXPECT_EQ(16, block.getLinearIndex(VoxelIndex(0, 1, 0))); 75 | EXPECT_EQ(256, block.getLinearIndex(VoxelIndex(0, 0, 1))); 76 | 77 | // Point to local. 78 | EXPECT_EQ(VoxelIndex(0, 0, 0), block.getVoxelIndex(Point(1.6, 3.2, 4.8))); 79 | EXPECT_EQ(VoxelIndex(15, 15, 15), block.getVoxelIndex(Point(3.15, 4.75, 6.35))); 80 | } 81 | 82 | TEST(VoxelBlock, Iterator) { 83 | VoxelBlock block(0.1, 16, BlockIndex(0, 0, 0)); 84 | for (size_t i = 0; i < block.numVoxels(); ++i) { 85 | block.getVoxel(i) = i; 86 | } 87 | 88 | // Check iterators follow the correct order. 89 | size_t i = 0; 90 | for (const auto& voxel : block) { 91 | EXPECT_EQ(voxel, i++); 92 | } 93 | } 94 | 95 | TEST(VoxelBlock, VoxelPosition) { 96 | const VoxelBlock block(0.1, 16, BlockIndex(0, 0, 0)); 97 | EXPECT_NEAR( 98 | (Point(0.05, 0.05, 0.05) - block.getVoxelPosition(VoxelIndex(0, 0, 0))).norm(), 0, 1e-6); 99 | EXPECT_NEAR( 100 | (Point(1.55, 1.55, 1.55) - block.getVoxelPosition(VoxelIndex(15, 15, 15))).norm(), 0, 1e-6); 101 | EXPECT_NEAR( 102 | (Point(0.15, 0.05, 0.05) - block.getVoxelPosition(VoxelIndex(1, 0, 0))).norm(), 0, 1e-6); 103 | EXPECT_NEAR( 104 | (Point(0.05, 0.15, 0.05) - block.getVoxelPosition(VoxelIndex(0, 1, 0))).norm(), 0, 1e-6); 105 | EXPECT_NEAR( 106 | (Point(0.05, 0.05, 0.15) - block.getVoxelPosition(VoxelIndex(0, 0, 1))).norm(), 0, 1e-6); 107 | } 108 | 109 | TEST(VoxelBlock, Assignment) { 110 | VoxelBlock block(0.1, 16, BlockIndex(0, 0, 0)); 111 | block.getVoxel(VoxelIndex(0, 0, 0)) = 42; 112 | block.getVoxel(VoxelIndex(15, 15, 15)) = 24; 113 | block.updated = true; 114 | 115 | // Assignment. 116 | VoxelBlock block_copy = block; 117 | EXPECT_EQ(block_copy.getVoxel(VoxelIndex(0, 0, 0)), 42); 118 | EXPECT_EQ(block_copy.getVoxel(VoxelIndex(15, 15, 15)), 24); 119 | EXPECT_EQ(block_copy.updated, block.updated); 120 | EXPECT_EQ(block_copy.index, block.index); 121 | EXPECT_EQ(block_copy.numVoxels(), block.numVoxels()); 122 | } 123 | 124 | } // namespace spatial_hash 125 | -------------------------------------------------------------------------------- /tests/utest_neighbor_utils.cpp: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #include 36 | #include 37 | 38 | namespace spatial_hash { 39 | 40 | TEST(NeighborUtils, IndicesValid) { 41 | // Index search. 42 | const NeighborSearch search(6); 43 | const Index index(0, 0, 0); 44 | const auto neighbors = search.neighborIndices(index, true); 45 | EXPECT_EQ(7, neighbors.size()); 46 | EXPECT_EQ(index, neighbors[0]); 47 | EXPECT_EQ(Index(-1, 0, 0), neighbors[1]); 48 | EXPECT_EQ(Index(1, 0, 0), neighbors[2]); 49 | EXPECT_EQ(Index(0, -1, 0), neighbors[3]); 50 | EXPECT_EQ(Index(0, 1, 0), neighbors[4]); 51 | EXPECT_EQ(Index(0, 0, -1), neighbors[5]); 52 | EXPECT_EQ(Index(0, 0, 1), neighbors[6]); 53 | 54 | // Long Index search. 55 | const LongIndex index2(1, 1, 1); 56 | const auto neighbors2 = search.neighborIndices(index2, false); 57 | EXPECT_EQ(6, neighbors2.size()); 58 | EXPECT_EQ(LongIndex(0, 1, 1), neighbors2[0]); 59 | 60 | // 18-neighbor search. 61 | const NeighborSearch search2(18); 62 | const auto neighbors3 = search2.neighborIndices(index, true); 63 | EXPECT_EQ(19, neighbors3.size()); 64 | 65 | // 26 neighbor search. 66 | const NeighborSearch search3(26); 67 | const auto neighbors4 = search3.neighborIndices(Index(0, 2, -2), false); 68 | EXPECT_EQ(26, neighbors4.size()); 69 | } 70 | 71 | TEST(NeighborUtils, BlockNeighborSearch) { 72 | Layer layer(1.0f); 73 | const BlockNeighborSearch search(layer, 6); 74 | 75 | // Index search. 76 | const BlockIndex block_index(1, 2, 3); 77 | auto neighbor_indices = search.neighborIndices(block_index, true); 78 | EXPECT_EQ(7, neighbor_indices.size()); 79 | 80 | // Block search. 81 | auto neighbors = search.neighborBlocks(block_index, true); 82 | EXPECT_EQ(0, neighbors.size()); 83 | 84 | // With blocks. 85 | layer.allocateBlock(block_index) = 123; 86 | layer.allocateBlock(BlockIndex(1, 2, 4)) = 124; 87 | layer.allocateBlock(BlockIndex(1, 3, 3)) = 133; 88 | EXPECT_EQ(3, layer.numBlocks()); 89 | 90 | const auto neighbors2 = search.neighborBlocks(block_index, true); 91 | EXPECT_EQ(3, neighbors2.size()); 92 | EXPECT_EQ(123, *neighbors2[0]); 93 | EXPECT_EQ(133, *neighbors2[1]); 94 | EXPECT_EQ(124, *neighbors2[2]); 95 | } 96 | 97 | TEST(NeighborUtils, VoxelNeighborSearch) { 98 | VoxelLayer> layer(1.0f, 4); 99 | for (int x = 0; x < 8; ++x) { 100 | for (int y = 0; y < 8; ++y) { 101 | for (int z = 0; z < 8; ++z) { 102 | layer.allocateVoxel(GlobalIndex(x, y, z)) = x * 100 + y * 10 + z; 103 | } 104 | } 105 | } 106 | 107 | const VoxelNeighborSearch search(layer, 6); 108 | const GlobalIndex index(4, 5, 6); 109 | const VoxelKey key{BlockIndex(1, 1, 1), VoxelIndex(0, 1, 2)}; 110 | 111 | // Index search. 112 | auto neighbors = search.neighborIndices(index, true); 113 | EXPECT_EQ(7, neighbors.size()); 114 | EXPECT_EQ(index, neighbors[0]); 115 | EXPECT_EQ(GlobalIndex(3, 5, 6), neighbors[1]); 116 | 117 | // Key search. 118 | const auto neighbors2 = search.neighborKeys(key, true); 119 | EXPECT_EQ(7, neighbors2.size()); 120 | EXPECT_EQ(BlockIndex(1, 1, 1), neighbors2[0].first); 121 | EXPECT_EQ(VoxelIndex(0, 1, 2), neighbors2[0].second); 122 | EXPECT_EQ(BlockIndex(0, 1, 1), neighbors2[1].first); 123 | EXPECT_EQ(VoxelIndex(3, 1, 2), neighbors2[1].second); 124 | 125 | // Voxel search. 126 | const auto neighbors3 = search.neighborVoxels(index, true); 127 | EXPECT_EQ(7, neighbors3.size()); 128 | EXPECT_EQ(456, *neighbors3[0]); 129 | EXPECT_EQ(356, *neighbors3[1]); 130 | EXPECT_EQ(556, *neighbors3[2]); 131 | 132 | // On boundary. 133 | const VoxelNeighborSearch search2(layer, 26); 134 | auto neighbors4 = search2.neighborVoxels(index); 135 | EXPECT_EQ(26, neighbors4.size()); 136 | 137 | neighbors4 = search2.neighborVoxels(GlobalIndex(0, 0, 0), true); 138 | EXPECT_EQ(8, neighbors4.size()); 139 | 140 | neighbors4 = search2.neighborVoxels(GlobalIndex(1, 0, 0), true); 141 | EXPECT_EQ(12, neighbors4.size()); 142 | 143 | neighbors4 = search2.neighborVoxels(GlobalIndex(1, 1, 0), true); 144 | EXPECT_EQ(18, neighbors4.size()); 145 | } 146 | 147 | } // namespace spatial_hash 148 | -------------------------------------------------------------------------------- /include/spatial_hash/impl/layer_impl.h: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #pragma once 36 | 37 | #include 38 | #include 39 | 40 | #include "spatial_hash/layer.h" 41 | 42 | namespace spatial_hash { 43 | 44 | template 45 | void Layer::copyBlocks(const Layer& other) { 46 | blocks_.clear(); 47 | for (const auto& [index, block_ptr] : other.blocks_) { 48 | blocks_[index] = std::make_shared(*block_ptr); 49 | } 50 | } 51 | 52 | template 53 | Layer& Layer::operator=(const Layer& other) { 54 | if (this == &other) { 55 | return *this; 56 | } 57 | 58 | const_cast(block_grid_) = other.block_grid_; 59 | copyBlocks(other); 60 | return *this; 61 | } 62 | 63 | template 64 | BlockIndices Layer::allocatedBlockIndices() const { 65 | BlockIndices indices; 66 | indices.reserve(blocks_.size()); 67 | for (const auto& kv : blocks_) { 68 | indices.push_back(kv.first); 69 | } 70 | return indices; 71 | } 72 | 73 | template 74 | BlockIndices Layer::blockIndicesWithCondition( 75 | std::function condition) const { 76 | BlockIndices indices; 77 | for (const auto& [index, block_ptr] : blocks_) { 78 | if (condition(*block_ptr)) { 79 | indices.push_back(index); 80 | } 81 | } 82 | return indices; 83 | } 84 | 85 | template 86 | std::vector Layer::blocksWithCondition( 87 | std::function condition) { 88 | std::vector blocks; 89 | for (const auto& [index, block_ptr] : blocks_) { 90 | if (condition(*block_ptr)) { 91 | blocks.push_back(block_ptr.get()); 92 | } 93 | } 94 | return blocks; 95 | } 96 | 97 | template 98 | std::vector Layer::blocksWithCondition( 99 | std::function condition) const { 100 | std::vector blocks; 101 | for (const auto& [index, block_ptr] : blocks_) { 102 | if (condition(*block_ptr)) { 103 | blocks.push_back(block_ptr.get()); 104 | } 105 | } 106 | return blocks; 107 | } 108 | 109 | template 110 | BlockT& Layer::getBlock(const BlockIndex& block_index) { 111 | const auto it = blocks_.find(block_index); 112 | if (it == blocks_.end()) { 113 | LOG(FATAL) << "Accessed unallocated block at " << block_index.transpose(); 114 | } 115 | return *it->second; 116 | } 117 | 118 | template 119 | const BlockT& Layer::getBlock(const BlockIndex& block_index) const { 120 | const auto it = blocks_.find(block_index); 121 | if (it == blocks_.end()) { 122 | LOG(FATAL) << "Accessed unallocated block at " << block_index.transpose(); 123 | } 124 | return *it->second; 125 | } 126 | 127 | template 128 | typename Layer::BlockPtr Layer::getBlockPtr(const BlockIndex& block_index) { 129 | const auto it = blocks_.find(block_index); 130 | if (it == blocks_.end()) { 131 | return nullptr; 132 | } 133 | return it->second; 134 | } 135 | 136 | template 137 | typename Layer::BlockConstPtr Layer::getBlockPtr( 138 | const BlockIndex& block_index) const { 139 | const auto it = blocks_.find(block_index); 140 | if (it == blocks_.end()) { 141 | return nullptr; 142 | } 143 | return it->second; 144 | } 145 | 146 | template 147 | template 148 | typename Layer::BlockPtr Layer::allocateBlockPtr(const BlockIndex& block_index, 149 | Args&&... args) { 150 | const auto it = blocks_.find(block_index); 151 | if (it != blocks_.end()) { 152 | return it->second; 153 | } 154 | 155 | auto block = std::make_shared(args...); 156 | blocks_[block_index] = block; 157 | return block; 158 | } 159 | 160 | template 161 | void Layer::removeBlock(const BlockIndex& block_index) { 162 | blocks_.erase(block_index); 163 | } 164 | 165 | template 166 | template 167 | void Layer::removeBlocks(const BlockIndexIterable& block_indices) { 168 | for (const auto& block_index : block_indices) { 169 | removeBlock(block_index); 170 | } 171 | } 172 | 173 | template 174 | void Layer::clear() { 175 | blocks_.clear(); 176 | } 177 | 178 | } // namespace spatial_hash 179 | -------------------------------------------------------------------------------- /include/spatial_hash/neighbor_utils.h: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #pragma once 36 | 37 | #include 38 | #include 39 | 40 | #include "spatial_hash/grid.h" 41 | #include "spatial_hash/types.h" 42 | #include "spatial_hash/voxel_layer.h" 43 | 44 | namespace spatial_hash { 45 | 46 | /** 47 | * @brief Index-based neighbor search. 48 | */ 49 | struct NeighborSearch { 50 | // Constructors. 51 | explicit NeighborSearch(size_t connectivity); 52 | virtual ~NeighborSearch() = default; 53 | 54 | /** 55 | * @brief Get the n-connectivity neighbors of a given index. 56 | * @param index The index to find neighbors for. 57 | * @param include_self Whether to include the index itself in the list of neighbors. 58 | */ 59 | template 60 | std::vector neighborIndices(const IndexT& index, const bool include_self = false) const; 61 | 62 | const size_t connectivity; 63 | 64 | protected: 65 | // Neighbor offsets ordered for self->6->18->26 connectivity. 66 | static const Eigen::Matrix kNeighborOffsets; 67 | void checkConnectivity() const; 68 | }; 69 | 70 | /** 71 | * @brief Neighbor search over block layers. For now only supports searching over const layers. 72 | */ 73 | template 74 | struct BlockNeighborSearch : public NeighborSearch { 75 | BlockNeighborSearch(const Layer& layer, size_t connectivity); 76 | virtual ~BlockNeighborSearch() = default; 77 | 78 | /** 79 | * @brief Get pointers to all allocated neighbor blocks of a given block. 80 | * @param block_index The index of the block to find neighbors for. 81 | * @param include_self Whether to include the block itself in the list of neighbors. 82 | */ 83 | std::vector neighborBlocks(const BlockIndex& block_index, 84 | const bool include_self = false) const; 85 | 86 | // TODO(lschmid): In the future support a non-const version of this that can also allocated 87 | // neighbor blocks. 88 | 89 | protected: 90 | const Layer& layer_; 91 | }; 92 | 93 | /** 94 | * @brief Neighbor search over voxel layers. For now only supports searching over const layers. 95 | */ 96 | template 97 | struct VoxelNeighborSearch : public NeighborSearch { 98 | using VoxelType = typename BlockT::VoxelType; 99 | 100 | VoxelNeighborSearch(const VoxelLayer& layer, size_t connectivity); 101 | 102 | virtual ~VoxelNeighborSearch() = default; 103 | 104 | /** 105 | * @brief Get the global voxel indices of all neighbors of a given voxel. 106 | * @param index The global voxel index of the voxel to find neighbors for. 107 | * @param include_self Whether to include the voxel itself in the list of neighbors. 108 | */ 109 | GlobalIndices neighborIndices(const GlobalIndex& index, const bool include_self = false) const { 110 | return NeighborSearch::neighborIndices(index, include_self); 111 | } 112 | 113 | /** 114 | * @brief Get the indices of all neighbors of a given voxel. 115 | * @param key The key of the voxel to find neighbors for. 116 | * @param include_self Whether to include the voxel itself in the list of neighbors. 117 | */ 118 | std::vector neighborKeys(const VoxelKey& key, const bool include_self = false) const; 119 | 120 | /** 121 | * @brief Get pointers to all allocated neighbor voxels of a given voxel. 122 | * @param index The global voxel index of the voxel to find neighbors for. 123 | * @param include_self Whether to include the voxel itself in the list of neighbors. 124 | */ 125 | std::vector neighborVoxels(const GlobalIndex& index, 126 | const bool include_self = false) const; 127 | 128 | /** 129 | * @brief Get pointers to all allocated neighbor voxels of a given voxel. 130 | * @param key The key of the voxel to find neighbors for. 131 | * @param include_self Whether to include the voxel itself in the list of neighbors. 132 | */ 133 | std::vector neighborVoxels(const VoxelKey& key, 134 | const bool include_self = false) const { 135 | return neighborVoxels(globalIndexFromKey(key, layer_.voxels_per_side), include_self); 136 | } 137 | 138 | // TODO(lschmid): In the future support a non-const version of this that can also allocated 139 | // neighbor voxel blocks. 140 | 141 | protected: 142 | const VoxelLayer& layer_; 143 | }; 144 | 145 | } // namespace spatial_hash 146 | 147 | #include "spatial_hash/impl/neighbor_utils_impl.h" 148 | -------------------------------------------------------------------------------- /tests/utest_hash.cpp: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | namespace spatial_hash { 41 | 42 | template 43 | IndexType randomIndex(const Type range, Generator& gen) { 44 | std::uniform_int_distribution distribution(-range, range); 45 | return IndexType(distribution(gen), distribution(gen), distribution(gen)); 46 | } 47 | 48 | // Compute the badness of a hash map. The lower the better the hash. 0 is optimal 49 | template 50 | double computeMapBadness(const Map& map) { 51 | double const lambda = static_cast(map.size()) / map.bucket_count(); 52 | 53 | double cost = 0; 54 | for (auto const& [k, _] : map) cost += map.bucket_size(map.bucket(k)); 55 | cost /= map.size(); 56 | 57 | return std::max(0.0, cost / (1 + lambda) - 1); 58 | } 59 | 60 | TEST(Hash, HashMapAccess) { 61 | IndexHashMap map; 62 | map[Index(0, 0, 0)] = 1; 63 | map[Index(1, 0, 0)] = 2; 64 | map[Index(0, 1, 0)] = 3; 65 | map[Index(0, 0, 1)] = 4; 66 | 67 | EXPECT_EQ(map[Index(0, 0, 0)], 1); 68 | EXPECT_EQ(map[Index(1, 0, 0)], 2); 69 | EXPECT_EQ(map[Index(0, 1, 0)], 3); 70 | EXPECT_EQ(map[Index(0, 0, 1)], 4); 71 | 72 | LongIndexHashMap long_map; 73 | long_map[LongIndex(0, 0, 0)] = 1; 74 | long_map[LongIndex(1, 0, 0)] = 2; 75 | long_map[LongIndex(0, 1, 0)] = 3; 76 | long_map[LongIndex(0, 0, 1)] = 4; 77 | 78 | EXPECT_EQ(long_map[LongIndex(0, 0, 0)], 1); 79 | EXPECT_EQ(long_map[LongIndex(1, 0, 0)], 2); 80 | EXPECT_EQ(long_map[LongIndex(0, 1, 0)], 3); 81 | EXPECT_EQ(long_map[LongIndex(0, 0, 1)], 4); 82 | } 83 | 84 | // NOTE(lschmid): Takes a few seconds to compute and onlyu relevant if hash is updated (see if it 85 | // gets better or worse, might need more thorough test cases though.) 86 | TEST(Hash, DISABLED_HashMapCollisions) { 87 | std::random_device rd; 88 | std::mt19937 gen(rd()); 89 | 90 | IndexHashMap map; 91 | int range = 1290; 92 | 93 | // Within range. 94 | for (size_t i = 0; i < 100000; ++i) { 95 | map[randomIndex(range, gen)] = i; 96 | } 97 | double badness = computeMapBadness(map); 98 | EXPECT_NEAR(badness, 0.002408, 1e-6); 99 | 100 | // Out of range. 101 | range = 10000; 102 | map.clear(); 103 | for (size_t i = 0; i < 100000; ++i) { 104 | map[randomIndex(range, gen)] = i; 105 | } 106 | badness = computeMapBadness(map); 107 | EXPECT_NEAR(badness, 0.0, 1e-6); 108 | 109 | // Dense. 110 | range = 200; 111 | map.clear(); 112 | for (int x = -range; x < range; ++x) { 113 | for (int y = -range; y < range; ++y) { 114 | for (int z = -range; z < range; ++z) { 115 | map[Index(x, y, z)] = 0; 116 | } 117 | } 118 | } 119 | badness = computeMapBadness(map); 120 | EXPECT_NEAR(badness, 0.047845, 1e-6); 121 | } 122 | 123 | TEST(Hash, IndexSets) { 124 | IndexSet set; 125 | set.insert(Index(0, 0, 0)); 126 | set.insert(Index(1, 0, 0)); 127 | set.insert(Index(0, 1, 0)); 128 | set.insert(Index(0, 0, 1)); 129 | 130 | // Lookup. 131 | EXPECT_TRUE(set.find(Index(0, 0, 0)) != set.end()); 132 | EXPECT_TRUE(set.find(Index(1, 0, 0)) != set.end()); 133 | EXPECT_TRUE(set.find(Index(0, 1, 0)) != set.end()); 134 | EXPECT_TRUE(set.find(Index(0, 0, 1)) != set.end()); 135 | 136 | // Collisions. 137 | EXPECT_EQ(set.size(), 4); 138 | set.insert(Index(0, 0, 0)); 139 | set.insert(Index(1, 0, 0)); 140 | EXPECT_EQ(set.size(), 4); 141 | 142 | // Erase. 143 | set.erase(Index(0, 0, 0)); 144 | EXPECT_TRUE(set.find(Index(0, 0, 0)) == set.end()); 145 | EXPECT_EQ(set.size(), 3); 146 | 147 | LongIndexSet long_set; 148 | long_set.insert(LongIndex(0, 0, 0)); 149 | long_set.insert(LongIndex(1, 0, 0)); 150 | long_set.insert(LongIndex(0, 1, 0)); 151 | long_set.insert(LongIndex(0, 0, 1)); 152 | 153 | // Lookup. 154 | EXPECT_TRUE(long_set.find(LongIndex(0, 0, 0)) != long_set.end()); 155 | EXPECT_TRUE(long_set.find(LongIndex(1, 0, 0)) != long_set.end()); 156 | EXPECT_TRUE(long_set.find(LongIndex(0, 1, 0)) != long_set.end()); 157 | EXPECT_TRUE(long_set.find(LongIndex(0, 0, 1)) != long_set.end()); 158 | 159 | // Collisions. 160 | EXPECT_EQ(long_set.size(), 4); 161 | long_set.insert(LongIndex(0, 0, 0)); 162 | long_set.insert(LongIndex(1, 0, 0)); 163 | EXPECT_EQ(long_set.size(), 4); 164 | 165 | // Erase. 166 | long_set.erase(LongIndex(0, 0, 0)); 167 | EXPECT_TRUE(long_set.find(LongIndex(0, 0, 0)) == long_set.end()); 168 | EXPECT_EQ(long_set.size(), 3); 169 | } 170 | 171 | } // namespace spatial_hash 172 | -------------------------------------------------------------------------------- /tests/utest_block_layer.cpp: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #include 36 | #include 37 | 38 | namespace spatial_hash { 39 | 40 | struct TestBlock : public Block { 41 | TestBlock(float block_size, const BlockIndex& index) : Block(block_size, index) {} 42 | int data = 0; 43 | }; 44 | 45 | TEST(BlockLayer, Constructor) { 46 | BlockLayer layer(2.0f); 47 | EXPECT_EQ(layer.blockSize(), 2.0f); 48 | EXPECT_EQ(layer.blockSizeInv(), 0.5f); 49 | EXPECT_EQ(layer.numBlocks(), 0); 50 | 51 | // Allocate some data. 52 | const BlockIndex i1(1, 2, 3); 53 | layer.allocateBlock(i1).data = 1; 54 | const BlockIndex i2(4, 5, 6); 55 | layer.allocateBlock(i2).data = 2; 56 | const BlockIndex i3(7, 8, 9); 57 | auto& block = layer.allocateBlock(i3); 58 | block.data = 3; 59 | block.updated = true; 60 | EXPECT_EQ(layer.numBlocks(), 3); 61 | 62 | // Copy constructor. 63 | const BlockLayer layer_copy(layer); 64 | EXPECT_EQ(layer_copy.blockSize(), 2.0f); 65 | EXPECT_EQ(layer_copy.blockSizeInv(), 0.5f); 66 | EXPECT_EQ(layer_copy.numBlocks(), 3); 67 | EXPECT_EQ(layer_copy.getBlock(i1).data, 1); 68 | EXPECT_EQ(layer_copy.getBlock(i2).data, 2); 69 | EXPECT_EQ(layer_copy.getBlock(i3).data, 3); 70 | EXPECT_FALSE(layer_copy.getBlock(i1).updated); 71 | EXPECT_TRUE(layer_copy.getBlock(i3).updated); 72 | 73 | // Assignment operator. 74 | BlockLayer layer_assign(1.0f); 75 | layer_assign = layer; 76 | EXPECT_EQ(layer_assign.blockSize(), 2.0f); 77 | EXPECT_EQ(layer_assign.blockSizeInv(), 0.5f); 78 | EXPECT_EQ(layer_assign.numBlocks(), 3); 79 | EXPECT_EQ(layer_assign.getBlock(i1).data, 1); 80 | EXPECT_EQ(layer_assign.getBlock(i2).data, 2); 81 | EXPECT_EQ(layer_assign.getBlock(i3).data, 3); 82 | EXPECT_FALSE(layer_copy.getBlock(i1).updated); 83 | EXPECT_TRUE(layer_copy.getBlock(i3).updated); 84 | 85 | // Check old block unchanged. 86 | EXPECT_EQ(layer.blockSize(), 2.0f); 87 | EXPECT_EQ(layer.blockSizeInv(), 0.5f); 88 | EXPECT_EQ(layer.numBlocks(), 3); 89 | EXPECT_EQ(layer.getBlock(i1).data, 1); 90 | EXPECT_EQ(layer.getBlock(i2).data, 2); 91 | EXPECT_EQ(layer.getBlock(i3).data, 3); 92 | EXPECT_FALSE(layer_copy.getBlock(i1).updated); 93 | EXPECT_TRUE(layer_copy.getBlock(i3).updated); 94 | } 95 | 96 | TEST(BlockLayer, BlockAttributes) { 97 | BlockLayer layer(2.0f); 98 | const auto& block = layer.allocateBlock(BlockIndex(1, 2, 3)); 99 | 100 | // Check block attributes. 101 | EXPECT_EQ(block.index, BlockIndex(1, 2, 3)); 102 | EXPECT_EQ(block.block_size, 2.0f); 103 | EXPECT_EQ(block.position(), Point(3.0f, 5.0f, 7.0f)); 104 | EXPECT_EQ(block.origin(), Point(2.0f, 4.0f, 6.0f)); 105 | EXPECT_EQ(block.data, 0); 106 | EXPECT_FALSE(block.updated); 107 | } 108 | 109 | TEST(BlockLayer, BlockConstructors) { 110 | struct ComplicatedBlock : public Block { 111 | ComplicatedBlock(float block_size, const BlockIndex& index, int a, bool b) 112 | : Block(block_size, index), a(a), b(b) {} 113 | const int a; 114 | const bool b; 115 | }; 116 | 117 | BlockLayer layer(2.0f); 118 | const auto& block = layer.allocateBlock(BlockIndex(1, 2, 3), 4, true); 119 | EXPECT_EQ(block.a, 4); 120 | EXPECT_EQ(block.b, true); 121 | } 122 | 123 | TEST(BlockLayer, updatedBlocks) { 124 | BlockLayer layer(1.0f); 125 | layer.allocateBlock(BlockIndex(0, 0, 0)).data = 0; 126 | layer.allocateBlock(BlockIndex(1, 0, 0)).data = 1; 127 | layer.allocateBlock(BlockIndex(0, 1, 0)).data = 2; 128 | layer.allocateBlock(BlockIndex(0, 0, 1)).data = 3; 129 | 130 | // Get blocks with condition. 131 | auto blocks_indices = layer.updatedBlockIndices(); 132 | EXPECT_EQ(blocks_indices.size(), 0); 133 | 134 | // Update some blocks. 135 | layer.getBlock(BlockIndex(1, 0, 0)).updated = true; 136 | layer.getBlock(BlockIndex(0, 0, 1)).updated = true; 137 | 138 | blocks_indices = layer.updatedBlockIndices(); 139 | EXPECT_EQ(blocks_indices.size(), 2); 140 | EXPECT_TRUE(std::find(blocks_indices.begin(), blocks_indices.end(), BlockIndex(1, 0, 0)) != 141 | blocks_indices.end()); // hash is unordered. 142 | EXPECT_TRUE(std::find(blocks_indices.begin(), blocks_indices.end(), BlockIndex(0, 0, 1)) != 143 | blocks_indices.end()); 144 | 145 | // Iterate over blocks with condition. 146 | int sum = 0; 147 | for (auto block : const_cast&>(layer).updatedBlocks()) { 148 | sum += block->data; 149 | block->updated = false; 150 | } 151 | EXPECT_EQ(sum, 4); 152 | EXPECT_EQ(layer.updatedBlockIndices().size(), 0); 153 | } 154 | 155 | } // namespace spatial_hash 156 | -------------------------------------------------------------------------------- /tests/utest_voxel_layer.cpp: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #include 36 | #include 37 | 38 | namespace spatial_hash { 39 | 40 | TEST(VoxelLayer, Access) { 41 | VoxelLayer> layer(0.1, 16); 42 | layer.allocateBlock(BlockIndex(0, 0, 0)); 43 | 44 | // Existing voxel. 45 | const Point p_valid = Point::Zero(); 46 | EXPECT_TRUE(layer.hasBlock(p_valid)); 47 | EXPECT_TRUE(layer.hasVoxel(p_valid)); 48 | const GlobalIndex i_valid = indexFromPoint(p_valid, 10.0f); 49 | EXPECT_TRUE(layer.hasVoxel(i_valid)); 50 | 51 | // Non-existing voxel. 52 | const Point p_invalid = Point(1.65, 0.15, 0.15); 53 | EXPECT_FALSE(layer.hasBlock(p_invalid)); 54 | EXPECT_FALSE(layer.hasVoxel(p_invalid)); 55 | const GlobalIndex i_invalid = indexFromPoint(p_invalid, 10.0f); 56 | EXPECT_FALSE(layer.hasVoxel(i_invalid)); 57 | 58 | // Get and set same voxel. 59 | auto& voxel = layer.getVoxel(p_valid); 60 | EXPECT_EQ(voxel, 0); 61 | voxel = 1; 62 | const auto& voxel_const = layer.getVoxel(i_valid); 63 | EXPECT_EQ(voxel_const, 1); 64 | } 65 | 66 | TEST(VoxelLayer, AllocateBlocks) { 67 | VoxelLayer> layer(0.1, 16); 68 | const Point point = Point::Zero(); 69 | const GlobalIndex index = indexFromPoint(point, 10.0f); 70 | 71 | // Allocate voxels (includes the block). 72 | auto& voxel = layer.allocateVoxel(point); 73 | EXPECT_EQ(voxel, 0); 74 | voxel = 42; 75 | const auto& voxel_const = layer.allocateVoxel(index); 76 | EXPECT_EQ(voxel_const, 42); 77 | EXPECT_EQ(layer.numBlocks(), 1); 78 | 79 | // Allocate block. 80 | layer.allocateBlock(BlockIndex(1, 0, 0)); 81 | const auto& block = layer.getBlock(BlockIndex(1, 0, 0)); 82 | EXPECT_EQ(block.index, BlockIndex(1, 0, 0)); 83 | EXPECT_EQ(block.voxel_size, 0.1f); 84 | EXPECT_EQ(block.voxels_per_side, 16); 85 | EXPECT_EQ(block.position(), Point(2.4, 0.8, 0.8)); 86 | EXPECT_EQ(layer.numBlocks(), 2); 87 | } 88 | 89 | TEST(VoxelLayer, AllocateVoxels) { 90 | struct ComplicatedBlock : public VoxelBlock { 91 | ComplicatedBlock(float voxel_size, 92 | size_t voxels_per_side, 93 | const BlockIndex& index, 94 | const std::string& test) 95 | : VoxelBlock(voxel_size, voxels_per_side, index), test(test) {} 96 | const std::string test; 97 | }; 98 | 99 | VoxelLayer layer(0.1, 16); 100 | const Point point(12, 23, 34); 101 | 102 | layer.allocateVoxel(point, "test1") = 42; 103 | EXPECT_TRUE(layer.hasVoxel(point)); 104 | EXPECT_EQ(layer.getVoxel(point), 42); 105 | EXPECT_EQ(layer.getBlock(layer.getBlockIndex(point)).test, "test1"); 106 | } 107 | 108 | TEST(VoxelLayer, Iterators) { 109 | VoxelLayer> layer(0.1, 16); 110 | layer.allocateBlock(BlockIndex(0, 0, 0)); 111 | layer.allocateBlock(BlockIndex(1, 0, 0)); 112 | layer.allocateBlock(BlockIndex(0, 1, 0)); 113 | layer.allocateBlock(BlockIndex(0, 0, 1)); 114 | 115 | // Iterate over all voxels. 116 | size_t num_voxels = 0; 117 | for (auto& voxel : layer.voxels()) { 118 | voxel = 1; 119 | } 120 | for (const auto& voxel : const_cast>*>(&layer)->voxels()) { 121 | num_voxels += voxel; 122 | } 123 | EXPECT_EQ(num_voxels, std::pow(16, 3) * 4); 124 | } 125 | 126 | TEST(VoxelLayer, Assignment) { 127 | VoxelLayer> layer(0.1, 16); 128 | const BlockIndex block_index(1, 2, 3); 129 | auto& block = layer.allocateBlock(block_index); 130 | block.getVoxel(12) = 34; 131 | 132 | // Copy constructor. 133 | VoxelLayer> layer_copy(layer); 134 | EXPECT_EQ(layer_copy.numBlocks(), 1); 135 | EXPECT_TRUE(layer_copy.hasBlock(block_index)); 136 | EXPECT_EQ(layer_copy.getBlock(block_index).getVoxel(12), 34); 137 | EXPECT_EQ(layer_copy.voxel_size, 0.1f); 138 | EXPECT_EQ(layer_copy.voxels_per_side, 16); 139 | 140 | // Copy assignment. 141 | VoxelLayer> layer_copy_assign(0.2, 8); 142 | layer_copy_assign = layer; 143 | EXPECT_EQ(layer_copy_assign.numBlocks(), 1); 144 | EXPECT_TRUE(layer_copy_assign.hasBlock(block_index)); 145 | EXPECT_EQ(layer_copy_assign.getBlock(block_index).getVoxel(12), 34); 146 | EXPECT_EQ(layer_copy_assign.voxel_size, 0.1f); 147 | EXPECT_EQ(layer_copy_assign.voxels_per_side, 16); 148 | 149 | // Move constructor. 150 | VoxelLayer> layer_move(std::move(layer)); 151 | EXPECT_EQ(layer_move.numBlocks(), 1); 152 | EXPECT_TRUE(layer_move.hasBlock(block_index)); 153 | EXPECT_EQ(layer_move.getBlock(block_index).getVoxel(12), 34); 154 | EXPECT_EQ(layer_move.voxel_size, 0.1f); 155 | EXPECT_EQ(layer_move.voxels_per_side, 16); 156 | 157 | // Move assignment. 158 | VoxelLayer> layer_move_assign(0.2, 8); 159 | layer_move_assign = std::move(layer_copy); 160 | EXPECT_EQ(layer_move_assign.numBlocks(), 1); 161 | EXPECT_TRUE(layer_move_assign.hasBlock(block_index)); 162 | EXPECT_EQ(layer_move_assign.getBlock(block_index).getVoxel(12), 34); 163 | EXPECT_EQ(layer_move_assign.voxel_size, 0.1f); 164 | EXPECT_EQ(layer_move_assign.voxels_per_side, 16); 165 | } 166 | 167 | } // namespace spatial_hash 168 | -------------------------------------------------------------------------------- /include/spatial_hash/grid.h: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #pragma once 36 | 37 | #include 38 | 39 | #include "spatial_hash/types.h" 40 | 41 | namespace spatial_hash { 42 | 43 | /** 44 | * @brief Compute the grid index from a coordinate point. 45 | * @param point The point to compute the grid index from. 46 | * @param voxel_size_inv The inverse of the grid size. Must be > 0. 47 | * @tparam IndexT The type of the grid index. 48 | */ 49 | template 50 | IndexT indexFromPoint(const Point& point, const float voxel_size_inv) { 51 | return IndexT(std::floor(point.x() * voxel_size_inv), 52 | std::floor(point.y() * voxel_size_inv), 53 | std::floor(point.z() * voxel_size_inv)); 54 | } 55 | 56 | /** 57 | * @brief Compute the cell center point from a grid index. 58 | * @param index The grid index to compute the point from. 59 | * @param voxel_size The size of the grid. 60 | * @tparam IndexT The type of the grid index. 61 | */ 62 | template 63 | Point centerPointFromIndex(const IndexT& index, const float voxel_size) { 64 | return (Point(index.x(), index.y(), index.z()) + Point(0.5f, 0.5f, 0.5f)) * voxel_size; 65 | } 66 | 67 | /** 68 | * @brief Compute the cell origin point from a grid index. 69 | * @param index The grid index to compute the point from. 70 | * @param voxel_size The size of the grid. 71 | * @tparam IndexT The type of the grid index. 72 | */ 73 | template 74 | Point originPointFromIndex(const IndexT& index, const float voxel_size) { 75 | return Point(index.x(), index.y(), index.z()) * voxel_size; 76 | } 77 | 78 | /** 79 | * @brief Compute the block index from a global voxel index. 80 | * @param global_index The global voxel index. 81 | * @param voxels_per_side The number of voxels per side of a block. 82 | */ 83 | BlockIndex blockIndexFromGlobalIndex(const GlobalIndex& global_index, const size_t voxels_per_side); 84 | 85 | /** 86 | * @brief Compute the local voxel index within a block from a global voxel index. 87 | * @param global_index The global voxel index. 88 | * @param voxels_per_side The number of voxels per side of a block. 89 | */ 90 | VoxelIndex localIndexFromGlobalIndex(const GlobalIndex& global_index, const size_t voxels_per_side); 91 | 92 | /** 93 | * @brief Compute the global voxel index from a local voxel index and block index. 94 | * @param local_index The local voxel index. 95 | * @param block_index The block index. 96 | * @param voxels_per_side The number of voxels per side of a block. 97 | */ 98 | GlobalIndex globalIndexFromLocalIndices(const BlockIndex& block_index, 99 | const VoxelIndex& local_index, 100 | const size_t voxels_per_side); 101 | /** 102 | * @brief Compute the global voxel index from a voxel key. 103 | * @param key The voxel key. 104 | * @param voxels_per_side The number of voxels per side of a block. 105 | */ 106 | GlobalIndex globalIndexFromKey(const VoxelKey& key, const size_t voxels_per_side); 107 | 108 | /** 109 | * @brief Compute the voxel key from a global voxel index. 110 | * @param global_index The global voxel index. 111 | * @param voxels_per_side The number of voxels per side of a block. 112 | */ 113 | VoxelKey keyFromGlobalIndex(const GlobalIndex& global_index, const size_t voxels_per_side); 114 | 115 | /** 116 | * @brief Compute the local voxel index from a linear index. Linear indices range from 0 to 117 | voxels_per_side^3-1 and traverse x->y->z. 118 | * @param linear_index The linear index to convert. 119 | * @param voxels_per_side The number of voxels per side of a block. 120 | */ 121 | VoxelIndex voxelIndexFromLinearIndex(const size_t linear_index, const size_t voxels_per_side); 122 | 123 | /** 124 | * @brief Compute the linear index from a local voxel index. Linear indices range from 0 to 125 | * voxels_per_side^3-1 and traverse x->y->z. 126 | * @param index The local voxel index. 127 | * @param voxels_per_side The number of voxels per side of a block. 128 | */ 129 | size_t linearIndexFromVoxelIndex(const VoxelIndex& index, const size_t voxels_per_side); 130 | 131 | /** 132 | * @brief A fixed resolution grid to move between points and grid indices. 133 | * @tparam IndexT The type of the grid index. 134 | */ 135 | template 136 | struct Grid { 137 | /** 138 | * @brief Construct a grid with a fixed resolution. 139 | * @param voxel_size The width of a cell. 140 | */ 141 | explicit Grid(const float voxel_size) : voxel_size(voxel_size), voxel_size_inv(1.f / voxel_size) { 142 | CHECK(voxel_size > 0.0f); 143 | } 144 | 145 | Grid(const Grid& other) : voxel_size(other.voxel_size), voxel_size_inv(other.voxel_size_inv) {} 146 | 147 | Grid(Grid&& other) : voxel_size(other.voxel_size), voxel_size_inv(other.voxel_size_inv) {} 148 | 149 | virtual ~Grid() = default; 150 | 151 | Grid& operator=(const Grid& other) { 152 | const_cast(voxel_size) = other.voxel_size; 153 | const_cast(voxel_size_inv) = other.voxel_size_inv; 154 | return *this; 155 | } 156 | 157 | Grid& operator=(Grid&& other) { 158 | const_cast(voxel_size) = other.voxel_size; 159 | const_cast(voxel_size_inv) = other.voxel_size_inv; 160 | return *this; 161 | } 162 | 163 | /** 164 | * @brief Compute the grid index from a coordinate point. 165 | * @param point The point to compute the grid index from. 166 | */ 167 | IndexT toIndex(const Point& point) const { return indexFromPoint(point, voxel_size_inv); } 168 | 169 | /** 170 | * @brief Compute the cell center point from a grid index. 171 | * @param index The grid index to compute the point from. 172 | */ 173 | Point toPoint(const IndexT& index) const { 174 | return centerPointFromIndex(index, voxel_size); 175 | } 176 | 177 | // Side length of one voxel. 178 | const float voxel_size; 179 | 180 | // Inverse of the voxel size. 181 | const float voxel_size_inv; 182 | }; 183 | 184 | // Common grid definitions. 185 | using IndexGrid = Grid; 186 | using LongIndexGrid = Grid; 187 | 188 | } // namespace spatial_hash 189 | -------------------------------------------------------------------------------- /tests/utest_layer.cpp: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #include 36 | #include 37 | 38 | namespace spatial_hash { 39 | 40 | TEST(Layer, Constructor) { 41 | struct MyBlock { 42 | int data; 43 | }; 44 | Layer layer(2.0f); 45 | EXPECT_EQ(layer.blockSize(), 2.0f); 46 | EXPECT_EQ(layer.blockSizeInv(), 0.5f); 47 | EXPECT_EQ(layer.numBlocks(), 0); 48 | 49 | // Allocate some data. 50 | const BlockIndex i1(1, 2, 3); 51 | layer.allocateBlock(i1).data = 1; 52 | const BlockIndex i2(4, 5, 6); 53 | layer.allocateBlock(i2).data = 2; 54 | const BlockIndex i3(7, 8, 9); 55 | layer.allocateBlock(i3).data = 3; 56 | EXPECT_EQ(layer.numBlocks(), 3); 57 | 58 | // Copy constructor. 59 | const Layer layer_copy(layer); 60 | EXPECT_EQ(layer_copy.blockSize(), 2.0f); 61 | EXPECT_EQ(layer_copy.blockSizeInv(), 0.5f); 62 | EXPECT_EQ(layer_copy.numBlocks(), 3); 63 | EXPECT_EQ(layer_copy.getBlock(i1).data, 1); 64 | EXPECT_EQ(layer_copy.getBlock(i2).data, 2); 65 | EXPECT_EQ(layer_copy.getBlock(i3).data, 3); 66 | 67 | // Assignment operator. 68 | Layer layer_assign(1.0f); 69 | layer_assign = layer; 70 | EXPECT_EQ(layer_assign.blockSize(), 2.0f); 71 | EXPECT_EQ(layer_assign.blockSizeInv(), 0.5f); 72 | EXPECT_EQ(layer_assign.numBlocks(), 3); 73 | EXPECT_EQ(layer_assign.getBlock(i1).data, 1); 74 | EXPECT_EQ(layer_assign.getBlock(i2).data, 2); 75 | EXPECT_EQ(layer_assign.getBlock(i3).data, 3); 76 | 77 | // Check old block unchanged. 78 | EXPECT_EQ(layer.blockSize(), 2.0f); 79 | EXPECT_EQ(layer.blockSizeInv(), 0.5f); 80 | EXPECT_EQ(layer.numBlocks(), 3); 81 | EXPECT_EQ(layer.getBlock(i1).data, 1); 82 | EXPECT_EQ(layer.getBlock(i2).data, 2); 83 | EXPECT_EQ(layer.getBlock(i3).data, 3); 84 | } 85 | 86 | TEST(Layer, RemoveBlocks) { 87 | Layer layer(1.0f); 88 | layer.allocateBlock(BlockIndex(0, 0, 0)); 89 | layer.allocateBlock(BlockIndex(1, 0, 0)); 90 | layer.allocateBlock(BlockIndex(0, 1, 0)); 91 | layer.allocateBlock(BlockIndex(0, 0, 1)); 92 | EXPECT_EQ(layer.numBlocks(), 4); 93 | 94 | // Remove block. 95 | layer.removeBlock(BlockIndex(0, 0, 0)); 96 | EXPECT_EQ(layer.numBlocks(), 3); 97 | EXPECT_FALSE(layer.hasBlock(BlockIndex(0, 0, 0))); 98 | 99 | // Remove non-existing block. 100 | layer.removeBlock(BlockIndex(0, 0, 0)); 101 | EXPECT_EQ(layer.numBlocks(), 3); 102 | 103 | // Remove all blocks. 104 | layer.clear(); 105 | EXPECT_EQ(layer.numBlocks(), 0); 106 | 107 | // Iterables. 108 | layer.allocateBlock(BlockIndex(0, 0, 0)); 109 | layer.allocateBlock(BlockIndex(1, 0, 0)); 110 | layer.allocateBlock(BlockIndex(0, 1, 0)); 111 | layer.allocateBlock(BlockIndex(0, 0, 1)); 112 | 113 | BlockIndices block_indices = {BlockIndex(0, 0, 0), BlockIndex(1, 0, 0)}; 114 | layer.removeBlocks(block_indices); 115 | EXPECT_EQ(layer.numBlocks(), 2); 116 | EXPECT_FALSE(layer.hasBlock(BlockIndex(0, 0, 0))); 117 | EXPECT_FALSE(layer.hasBlock(BlockIndex(1, 0, 0))); 118 | 119 | IndexSet block_indices_set = {BlockIndex(0, 0, 0), BlockIndex(0, 0, 1)}; 120 | layer.removeBlocks(block_indices_set); 121 | EXPECT_EQ(layer.numBlocks(), 1); 122 | EXPECT_FALSE(layer.hasBlock(BlockIndex(0, 0, 1))); 123 | } 124 | 125 | TEST(Layer, Access) { 126 | Layer layer(1.6); 127 | EXPECT_EQ(layer.numBlocks(), 0); 128 | 129 | // Alloacte a block. 130 | layer.allocateBlock(BlockIndex(0, 0, 0)); 131 | EXPECT_EQ(layer.numBlocks(), 1); 132 | 133 | // Has block. 134 | EXPECT_TRUE(layer.hasBlock(BlockIndex(0, 0, 0))); 135 | EXPECT_FALSE(layer.hasBlock(BlockIndex(1, 0, 0))); 136 | EXPECT_TRUE(layer.hasBlock(Point(0, 0, 0))); // lower bound included. 137 | EXPECT_TRUE(layer.hasBlock(Point(0.1, 0, 0))); 138 | EXPECT_FALSE(layer.hasBlock(Point(-0.1, 0, 0))); 139 | EXPECT_FALSE(layer.hasBlock(Point(1.6, 0, 0))); // upper bound excluded. 140 | 141 | // Block list. 142 | const auto block_list = layer.allocatedBlockIndices(); 143 | EXPECT_EQ(block_list.size(), 1); 144 | EXPECT_EQ(block_list[0], BlockIndex(0, 0, 0)); 145 | 146 | // Get block. 147 | auto& block = layer.getBlock(BlockIndex(0, 0, 0)); 148 | block = 123; 149 | const auto& block_const = layer.getBlock(Point(0, 0, 0)); 150 | EXPECT_EQ(block_const, 123); 151 | 152 | // Get block ptr. 153 | auto block_ptr = layer.getBlockPtr(BlockIndex(0, 0, 0)); 154 | EXPECT_NE(block_ptr, nullptr); 155 | block_ptr = layer.getBlockPtr(BlockIndex(1, 0, 0)); 156 | EXPECT_EQ(block_ptr, nullptr); 157 | } 158 | 159 | TEST(Layer, Iterators) { 160 | Layer layer(1.0f); 161 | layer.allocateBlock(BlockIndex(0, 0, 0)); 162 | layer.allocateBlock(BlockIndex(1, 0, 0)); 163 | layer.allocateBlock(BlockIndex(0, 1, 0)); 164 | layer.allocateBlock(BlockIndex(0, 0, 1)); 165 | 166 | // Iterate over all blocks. 167 | for (auto& block : layer) { 168 | block = 1; 169 | } 170 | size_t num_blocks = 0; 171 | for (const auto& block : *const_cast*>(&layer)) { 172 | num_blocks += block; 173 | } 174 | EXPECT_EQ(num_blocks, 4); 175 | } 176 | 177 | TEST(Layer, BlocksWithCondition) { 178 | struct Block { 179 | int data; 180 | }; 181 | Layer layer(1.0f); 182 | layer.allocateBlock(BlockIndex(0, 0, 0)).data = 1; 183 | layer.allocateBlock(BlockIndex(1, 0, 0)).data = 2; 184 | layer.allocateBlock(BlockIndex(0, 1, 0)).data = 3; 185 | layer.allocateBlock(BlockIndex(0, 0, 1)).data = 4; 186 | 187 | // Get blocks with condition. 188 | const std::function greater2 = [](const Block& block) { 189 | return block.data > 2; 190 | }; 191 | const auto blocks_indices = layer.blockIndicesWithCondition(greater2); 192 | EXPECT_EQ(blocks_indices.size(), 2); 193 | EXPECT_TRUE(std::find(blocks_indices.begin(), blocks_indices.end(), BlockIndex(0, 1, 0)) != 194 | blocks_indices.end()); // hash is unordered. 195 | EXPECT_TRUE(std::find(blocks_indices.begin(), blocks_indices.end(), BlockIndex(0, 0, 1)) != 196 | blocks_indices.end()); 197 | 198 | // Iterate over blocks with condition. 199 | for (auto block : layer.blocksWithCondition(greater2)) { 200 | block->data = 0; 201 | } 202 | for (const auto& block : layer) { 203 | EXPECT_LE(block.data, 2); 204 | } 205 | } 206 | 207 | } // namespace spatial_hash 208 | -------------------------------------------------------------------------------- /include/spatial_hash/voxel_block.h: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #pragma once 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | #include "spatial_hash/block.h" 44 | #include "spatial_hash/grid.h" 45 | 46 | namespace spatial_hash { 47 | 48 | /** 49 | * @brief A data structure for blocks of densely allocated 3D voxels. 50 | * @tparam VoxelT The voxel type contained in the block. 51 | * @note Local voxel indices range from [0,0,0] at the bottom left front corner to 52 | * [vps-1,vps-1,vps-1] at the top right back corner. Linear indices range from 0 to vps^3-1 and 53 | * traverse x->y->z. 54 | */ 55 | template 56 | struct VoxelBlock : public IndexGrid, public Block { 57 | // Types. 58 | using VoxelType = VoxelT; 59 | using Ptr = std::shared_ptr; 60 | using ConstPtr = std::shared_ptr; 61 | 62 | // Constructors. 63 | /** 64 | * @brief Construct a block with a given voxel size, number of voxels per side, and position. 65 | * @param voxel_size The side length of each voxel. 66 | * @param voxels_per_side The number of voxels per side of the block. Must be a power of 2. 67 | * @param index Index of the block in the grid. 68 | */ 69 | VoxelBlock(float voxel_size, size_t voxels_per_side, const BlockIndex& index); 70 | 71 | VoxelBlock(const VoxelBlock& other); 72 | 73 | VoxelBlock(VoxelBlock&& other); 74 | 75 | virtual ~VoxelBlock() = default; 76 | 77 | VoxelBlock& operator=(const VoxelBlock& other); 78 | 79 | VoxelBlock& operator=(VoxelBlock&& other); 80 | 81 | // Config. 82 | // Number of voxels per side of the block. 83 | const size_t voxels_per_side; 84 | 85 | /** 86 | * @brief Get the number of voxels in the block. 87 | */ 88 | size_t numVoxels() const { return voxels_per_side * voxels_per_side * voxels_per_side; } 89 | 90 | // Index checking and conversion. 91 | /** 92 | * @brief Check if a linear index is valid, i.e., it is within the block. 93 | * @param linear_index The linear index to check. 94 | */ 95 | bool isValidLinearIndex(size_t linear_index) const { return linear_index < numVoxels(); } 96 | 97 | /** 98 | * @brief Check if a local voxel index is valid, i.e., it is within the block. 99 | * @param index The local voxel index to check. 100 | */ 101 | bool isValidVoxelIndex(const VoxelIndex& index) const { 102 | return (index.array() >= 0).all() && (index.array() < voxels_per_side).all(); 103 | } 104 | 105 | /** 106 | * @brief Check whether the given voxel index is on the boundary of the block. 107 | * @param index The local voxel index to check. 108 | */ 109 | bool isOnBoundary(const VoxelIndex& index) const { 110 | return (index.array() == 0).any() || (index.array() == voxels_per_side - 1).any(); 111 | } 112 | bool isOnBoundary(const size_t& index) const { 113 | return isOnBoundary(voxelIndexFromLinearIndex(index, voxels_per_side)); 114 | } 115 | 116 | /** 117 | * @brief Compute the linear index from a local voxel index. This assumes the local index is 118 | * valid. Note that the resulting lienar index may be valid (but wrong) even if the local index is 119 | * not. 120 | * @param index The local voxel index. 121 | */ 122 | size_t getLinearIndex(const VoxelIndex& index) const { 123 | return linearIndexFromVoxelIndex(index, voxels_per_side); 124 | } 125 | 126 | /** 127 | * @brief Get the local voxel index from a linear index. The voxel index will be invalid if the 128 | * linear index is invalid. 129 | * @param linear_index The linear index to convert. 130 | */ 131 | VoxelIndex getVoxelIndex(size_t linear_index) const { 132 | return voxelIndexFromLinearIndex(linear_index, voxels_per_side); 133 | } 134 | 135 | /** 136 | * @brief Get the local voxel index from a global position. This voxel index may be invalid if the 137 | * point is outside the block. 138 | * @param position The global position to convert. 139 | */ 140 | VoxelIndex getVoxelIndex(const Point& position) const { return toIndex(position - origin()); } 141 | 142 | /** 143 | * @brief Get the global voxel index from a linear index. 144 | * @param linear_index The linear index to convert. 145 | */ 146 | GlobalIndex getGlobalVoxelIndex(size_t linear_index) const { 147 | return globalIndexFromLocalIndices(this->index, getVoxelIndex(linear_index), voxels_per_side); 148 | } 149 | 150 | /** 151 | * @brief Get the global voxel index from a local index. 152 | * @param index The local voxel index to convert. 153 | */ 154 | GlobalIndex getGlobalVoxelIndex(const VoxelIndex& index) const { 155 | return globalIndexFromLocalIndices(this->index, index, voxels_per_side); 156 | } 157 | 158 | /** 159 | * @brief Get the key from a local voxel index. 160 | * @param index The local voxel index. 161 | */ 162 | VoxelKey getVoxelKey(const VoxelIndex& index) const { return std::make_pair(this->index, index); } 163 | 164 | /** 165 | * @brief Get the key from a linear index. 166 | * @param linear_index The linear index. 167 | */ 168 | VoxelKey getVoxelKey(size_t linear_index) const { 169 | return getVoxelKey(getVoxelIndex(linear_index)); 170 | } 171 | 172 | // Access. 173 | /** 174 | * @brief Get the voxel at a linear index. This assumes the linear index is valid. 175 | */ 176 | VoxelType& getVoxel(size_t linear_index) { return voxels[linear_index]; } 177 | const VoxelType& getVoxel(size_t linear_index) const { return voxels[linear_index]; } 178 | 179 | /** 180 | * @brief Get the voxel at a local index. This assumes the local index is valid. 181 | */ 182 | VoxelType& getVoxel(const VoxelIndex& index) { return voxels[getLinearIndex(index)]; } 183 | const VoxelType& getVoxel(const VoxelIndex& index) const { return voxels[getLinearIndex(index)]; } 184 | 185 | // Additional functions. 186 | /** 187 | * @brief Get the position (cell center) of a voxel at a linear index in global frame. 188 | * @param linear_index The linear index. 189 | */ 190 | Point getVoxelPosition(size_t linear_index) const { 191 | return getVoxelPosition(getVoxelIndex(linear_index)); 192 | } 193 | 194 | /** 195 | * @brief Get the position (cell center) of a voxel at a local index in global frame. 196 | * @param index The local voxel index. 197 | */ 198 | Point getVoxelPosition(const VoxelIndex& index) const { 199 | return origin() + index.cast() * voxel_size + Point::Constant(voxel_size * 0.5f); 200 | } 201 | 202 | // Iterators. 203 | typename std::vector::iterator begin() { return voxels.begin(); } 204 | typename std::vector::iterator end() { return voxels.end(); } 205 | typename std::vector::const_iterator begin() const { return voxels.begin(); } 206 | typename std::vector::const_iterator end() const { return voxels.end(); } 207 | 208 | private: 209 | std::vector voxels; 210 | }; 211 | 212 | } // namespace spatial_hash 213 | 214 | #include "spatial_hash/impl/voxel_block_impl.h" 215 | -------------------------------------------------------------------------------- /include/spatial_hash/layer.h: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #pragma once 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | 46 | #include "spatial_hash/block.h" 47 | #include "spatial_hash/grid.h" 48 | #include "spatial_hash/hash.h" 49 | #include "spatial_hash/types.h" 50 | 51 | namespace spatial_hash { 52 | 53 | /** 54 | * @brief Layer of data blocks stored in a spatial hash grid, where BlockT can be any data structure 55 | * but should be copy constructable. 56 | * @tparam BlockT The type of blocks contained in the layer. Can be any data structure but should be 57 | * copy constructable. 58 | */ 59 | template 60 | class Layer { 61 | public: 62 | // Types. 63 | using Ptr = std::shared_ptr; 64 | using ConstPtr = std::shared_ptr; 65 | using BlockType = BlockT; 66 | using BlockPtr = std::shared_ptr; 67 | using BlockConstPtr = std::shared_ptr; 68 | using BlockMap = IndexHashMap; 69 | 70 | // Constructors. 71 | /** 72 | * @brief Construct a layer of data blocks. 73 | * @param block_size The side length of each block. 74 | */ 75 | explicit Layer(float block_size) : block_grid_(block_size) {} 76 | 77 | virtual ~Layer() = default; 78 | 79 | // Copy constructor and assignment create deep copies of the blocks. This assumes blocks are copy 80 | // constructible. 81 | Layer(const Layer& other) : block_grid_(other.block_grid_) { copyBlocks(other); } 82 | 83 | Layer& operator=(const Layer& other); 84 | 85 | // Layer info. 86 | float blockSize() const { return block_grid_.voxel_size; } 87 | float blockSizeInv() const { return block_grid_.voxel_size_inv; } 88 | 89 | /** 90 | * @brief Get the number of allocated blocks in the layer. 91 | */ 92 | size_t numBlocks() const { return blocks_.size(); } 93 | 94 | /** 95 | * @brief Get the indices of all allocated blocks in the layer. 96 | */ 97 | BlockIndices allocatedBlockIndices() const; 98 | 99 | /** 100 | * @brief Get the indices of all blocks that satisfy a given condition. 101 | * @param condition The condition to check. 102 | */ 103 | BlockIndices blockIndicesWithCondition(std::function condition) const; 104 | 105 | /** 106 | * @brief Get pointers to all blocks that satisfy a given condition. 107 | * @param condition The condition to check. 108 | */ 109 | std::vector blocksWithCondition(std::function condition); 110 | std::vector blocksWithCondition( 111 | std::function condition) const; 112 | 113 | // Index checking and conversion. 114 | /** 115 | * @brief Check if a block exists at a given index. 116 | * @param block_index The block index. 117 | */ 118 | bool hasBlock(const BlockIndex& block_index) const { return blocks_.count(block_index) > 0; } 119 | 120 | /** 121 | * @brief Check if a block exists at a given point. 122 | * @param point Coordinates to check. 123 | */ 124 | bool hasBlock(const Point& point) const { return hasBlock(block_grid_.toIndex(point)); } 125 | 126 | /** 127 | * @brief Get the block index of a given point. 128 | * @param point Global coordinates to check. 129 | */ 130 | BlockIndex getBlockIndex(const Point& point) const { return block_grid_.toIndex(point); } 131 | 132 | // Access. 133 | /** 134 | * @brief Get the block at a given index. This assumes the block exists. 135 | * @param block_index The block index. 136 | */ 137 | BlockT& getBlock(const BlockIndex& block_index); 138 | const BlockT& getBlock(const BlockIndex& block_index) const; 139 | 140 | /** 141 | * @brief Get the block at a given point. This assumes the block exists. 142 | * @param point Coordinates to check. 143 | */ 144 | BlockT& getBlock(const Point& point) { return getBlock(block_grid_.toIndex(point)); } 145 | const BlockT& getBlock(const Point& point) const { return getBlock(block_grid_.toIndex(point)); } 146 | 147 | /** 148 | * @brief Get the block at a given index. Returns nullptr if the block does not exist. 149 | * @param block_index The block index. 150 | */ 151 | BlockPtr getBlockPtr(const BlockIndex& block_index); 152 | BlockConstPtr getBlockPtr(const BlockIndex& block_index) const; 153 | 154 | /** 155 | * @brief Get the block at a given point. Returns nullptr if the block does not exist. 156 | */ 157 | BlockPtr getBlockPtr(const Point& point) { return getBlockPtr(block_grid_.toIndex(point)); } 158 | BlockConstPtr getBlockPtr(const Point& point) const { 159 | return getBlockPtr(block_grid_.toIndex(point)); 160 | } 161 | 162 | /** 163 | * @brief Gets the block at a given index or allocates a new block if it does not exist. 164 | * @param block_index The block index to retrieve or allocate. 165 | * @param args The arguments to pass to the block constructor. 166 | * @tparam Args The types of the arguments to pass to the block constructor. 167 | */ 168 | template 169 | BlockT& allocateBlock(const BlockIndex& block_index, Args&&... args) { 170 | return *allocateBlockPtr(block_index, std::forward(args)...); 171 | } 172 | 173 | /** 174 | * @brief Gets the block at a given point or allocates a new block if it does not exist. 175 | * @param block_index The point to retrieve or allocate. 176 | * @param args The arguments to pass to the block constructor. 177 | * @tparam Args The types of the arguments to pass to the block constructor. 178 | */ 179 | template 180 | BlockPtr allocateBlockPtr(const BlockIndex& block_index, Args&&... args); 181 | 182 | /** 183 | * @brief Remove a block from the layer. 184 | * @param block_index The index of the block to remove. 185 | */ 186 | void removeBlock(const BlockIndex& block_index); 187 | 188 | /** 189 | * @brief Remove several blocks from the layer. 190 | * @param block_indices The indices of the blocks to remove. 191 | * @tparam BlockIndexIterable An iterable type containing block indices. 192 | */ 193 | template 194 | void removeBlocks(const BlockIndexIterable& block_indices); 195 | 196 | /** 197 | * @brief Remove all blocks from the layer. 198 | */ 199 | void clear(); 200 | 201 | protected: 202 | const IndexGrid block_grid_; 203 | BlockMap blocks_; 204 | 205 | void copyBlocks(const Layer& other); 206 | 207 | public: 208 | // Iterators. 209 | class iterator { 210 | public: 211 | using MapIt = typename BlockMap::iterator; 212 | using iterator_category = std::forward_iterator_tag; 213 | using difference_type = std::ptrdiff_t; 214 | using value_type = BlockT; 215 | using pointer = BlockT*; 216 | using reference = Block&; 217 | 218 | explicit iterator(MapIt it) : it_(it) {} 219 | iterator& operator++() { 220 | it_++; 221 | return *this; 222 | } 223 | iterator operator++(int) { 224 | iterator retval = *this; 225 | ++(*this); 226 | return retval; 227 | } 228 | bool operator==(iterator other) const { return it_ == other.it_; } 229 | bool operator!=(iterator other) const { return it_ != other.it_; } 230 | BlockT& operator*() const { return *it_->second; } 231 | BlockT* operator->() const { return it_->second.get(); } 232 | 233 | private: 234 | MapIt it_; 235 | }; 236 | 237 | class const_iterator { 238 | public: 239 | using MapIt = typename BlockMap::const_iterator; 240 | using iterator_category = std::forward_iterator_tag; 241 | using difference_type = std::ptrdiff_t; 242 | using value_type = const BlockT; 243 | using pointer = const BlockT*; 244 | using reference = const Block&; 245 | 246 | explicit const_iterator(MapIt it) : it_(it) {} 247 | const_iterator& operator++() { 248 | it_++; 249 | return *this; 250 | } 251 | const_iterator operator++(int) { 252 | const_iterator retval = *this; 253 | ++(*this); 254 | return retval; 255 | } 256 | bool operator==(const_iterator other) const { return it_ == other.it_; } 257 | bool operator!=(const_iterator other) const { return it_ != other.it_; } 258 | const BlockT& operator*() const { return *it_->second; } 259 | const BlockT* operator->() const { return it_->second.get(); } 260 | 261 | private: 262 | MapIt it_; 263 | }; 264 | 265 | iterator begin() { return iterator(blocks_.begin()); } 266 | iterator end() { return iterator(blocks_.end()); } 267 | const_iterator begin() const { return const_iterator(blocks_.begin()); } 268 | const_iterator end() const { return const_iterator(blocks_.end()); } 269 | }; 270 | 271 | } // namespace spatial_hash 272 | 273 | #include "spatial_hash/impl/layer_impl.h" 274 | -------------------------------------------------------------------------------- /include/spatial_hash/voxel_layer.h: -------------------------------------------------------------------------------- 1 | /** ----------------------------------------------------------------------------- 2 | * Copyright (c) 2024 Massachusetts Institute of Technology. 3 | * All Rights Reserved. 4 | * 5 | * AUTHORS: Lukas Schmid 6 | * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology 7 | * YEAR: 2024 8 | * LICENSE: BSD 3-Clause 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright notice, this 14 | * list of conditions and the following disclaimer. 15 | * 16 | * 2. Redistributions in binary form must reproduce the above copyright notice, 17 | * this list of conditions and the following disclaimer in the documentation 18 | * and/or other materials provided with the distribution. 19 | * 20 | * 3. Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived from 22 | * this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * -------------------------------------------------------------------------- */ 35 | #pragma once 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | #include "spatial_hash/block_layer.h" 44 | #include "spatial_hash/grid.h" 45 | #include "spatial_hash/types.h" 46 | #include "spatial_hash/voxel_block.h" 47 | 48 | namespace spatial_hash { 49 | 50 | /** 51 | * @brief Layer of voxel blocks stored in a spatial hash grid, where BlockT is a subclass of 52 | * VoxelBlock and has the same constructor as VoxelBlock. 53 | * @tparam BlockT The type of blocks contained in the layer. Must be a subclass of VoxelBlock and 54 | * have the same constructor as VoxelBlock. 55 | */ 56 | template 57 | class VoxelLayer : protected Grid, public BlockLayer { 58 | public: 59 | // Types. 60 | using VoxelType = typename BlockT::VoxelType; 61 | using BlockType = BlockT; 62 | using Ptr = std::shared_ptr; 63 | using ConstPtr = std::shared_ptr; 64 | using BlockPtr = std::shared_ptr; 65 | 66 | // Constructors. 67 | /** 68 | * @brief Construct a layer of voxel blocks. 69 | * @param voxel_size The side length of each voxel. 70 | * @param voxels_per_side The number of voxels per side of each block. Must be a power of 2. 71 | */ 72 | VoxelLayer(float voxel_size, size_t voxels_per_side) 73 | : Grid(voxel_size), 74 | BlockLayer(voxel_size * voxels_per_side), 75 | voxels_per_side(voxels_per_side) { 76 | static_assert(std::is_base_of, BlockT>::value, 77 | "BlockT must be a subclass of VoxelBlock to create a VoxelLayer."); 78 | } 79 | 80 | VoxelLayer(const VoxelLayer& other) 81 | : Grid(other), 82 | BlockLayer(other), 83 | voxels_per_side(other.voxels_per_side) {} 84 | 85 | VoxelLayer(VoxelLayer&& other) 86 | : Grid(std::move(other)), 87 | BlockLayer(std::move(other)), 88 | voxels_per_side(std::move(other.voxels_per_side)) {} 89 | 90 | virtual ~VoxelLayer() = default; 91 | 92 | VoxelLayer& operator=(const VoxelLayer& other); 93 | 94 | VoxelLayer& operator=(VoxelLayer&& other); 95 | 96 | // Config. 97 | // Number of voxels per side of each block. 98 | const size_t voxels_per_side; 99 | using Grid::voxel_size; 100 | using Grid::voxel_size_inv; 101 | 102 | // Index checking and conversion. 103 | /** 104 | * @brief Check if a voxel exists at a given global voxel index. 105 | * @param voxel_index The global voxel index. 106 | */ 107 | bool hasVoxel(const GlobalIndex& voxel_index) const { 108 | return this->hasBlock(blockIndexFromGlobalIndex(voxel_index, voxels_per_side)); 109 | } 110 | 111 | /** 112 | * @brief Check if a voxel exists at a given point. 113 | * @param point Coordinates to check. 114 | */ 115 | bool hasVoxel(const Point& point) const { return this->hasBlock(point); } 116 | 117 | /** 118 | * @brief Check if a voxel exists at a given voxel key. 119 | * @param key The voxel key to lookup. 120 | */ 121 | bool hasVoxel(const VoxelKey& key) const { return hasVoxel(key.first); } 122 | 123 | /** 124 | * @brief Get the global index of a voxel at a given point. 125 | * @param point The coordinates to check. 126 | */ 127 | GlobalIndex getGlobalVoxelIndex(const Point& point) const { return toIndex(point); } 128 | 129 | /** 130 | * @brief Get the key of a voxel at a given point. 131 | */ 132 | VoxelKey getVoxelKey(const Point& point) const { 133 | return keyFromGlobalIndex(toIndex(point), voxels_per_side); 134 | } 135 | 136 | /** 137 | * @brief Get the key of a voxel at a given global voxel index. 138 | * @param index The global voxel index. 139 | */ 140 | VoxelKey getVoxelKey(const GlobalIndex& index) const { 141 | return keyFromGlobalIndex(index, voxels_per_side); 142 | } 143 | 144 | /** 145 | * @brief Get the center point of a voxel in global coordinates. 146 | * @param index The global voxel index. 147 | */ 148 | Point getVoxelPosition(const GlobalIndex& index) const { return toPoint(index); } 149 | 150 | /** 151 | * @brief Get the center point of a voxel in global coordinates. 152 | * @param key The voxel key . 153 | */ 154 | Point getVoxelPosition(const VoxelKey& key) const { 155 | return toPoint(globalIndexFromKey(key, voxels_per_side)); 156 | } 157 | 158 | // Access. 159 | /** 160 | * @brief Get the voxel at a given global voxel index. This assumes the voxel exists. 161 | * @param voxel_index The global voxel index. 162 | */ 163 | VoxelType& getVoxel(const GlobalIndex& voxel_index) { 164 | return this->getBlock(blockIndexFromGlobalIndex(voxel_index, voxels_per_side)) 165 | .getVoxel(localIndexFromGlobalIndex(voxel_index, voxels_per_side)); 166 | } 167 | const VoxelType& getVoxel(const GlobalIndex& voxel_index) const { 168 | return this->getBlock(blockIndexFromGlobalIndex(voxel_index, voxels_per_side)) 169 | .getVoxel(localIndexFromGlobalIndex(voxel_index, voxels_per_side)); 170 | } 171 | 172 | /** 173 | * @brief Get the voxel at a given point. This assumes the voxel exists. 174 | * @param point Global coordinates to check. 175 | */ 176 | VoxelType& getVoxel(const Point& point) { return getVoxel(toIndex(point)); } 177 | const VoxelType& getVoxel(const Point& point) const { return getVoxel(toIndex(point)); } 178 | 179 | /** 180 | * @brief Get the voxel at a given voxel key. This assumes the voxel exists. 181 | * @param key The voxel key . 182 | */ 183 | VoxelType& getVoxel(const VoxelKey& key) { 184 | return this->getBlock(key.first).getVoxel(key.second); 185 | } 186 | const VoxelType& getVoxel(const VoxelKey& key) const { 187 | return this->getBlock(key.first).getVoxel(key.second); 188 | } 189 | 190 | /** 191 | * @brief Get the voxel at a given global voxel index or nullptr if it does not exist. 192 | * @param voxel_index The global voxel index. 193 | */ 194 | VoxelType* getVoxelPtr(const GlobalIndex& voxel_index); 195 | const VoxelType* getVoxelPtr(const GlobalIndex& voxel_index) const; 196 | 197 | /** 198 | * @brief Get the voxel at a given point or nullptr if it does not exist. 199 | * @param point Position of the voxel in global coordinates. 200 | */ 201 | VoxelType* getVoxelPtr(const Point& point) { return getVoxelPtr(toIndex(point)); } 202 | const VoxelType* getVoxelPtr(const Point& point) const { return getVoxelPtr(toIndex(point)); } 203 | 204 | /** 205 | * @brief Get the voxel at a given voxel key or nullptr if it does not exist. 206 | * @param key The voxel key . 207 | */ 208 | VoxelType* getVoxelPtr(const VoxelKey& key); 209 | const VoxelType* getVoxelPtr(const VoxelKey& key) const; 210 | 211 | /** 212 | * @brief Get the voxel at a given global voxel or allocate the corresponding block if it does not 213 | * exist. 214 | * @param voxel_index The global voxel index. 215 | * @param args Additional arguments to pass to the block constructor. 216 | * @tparam Args The types of the additional arguments. 217 | */ 218 | template 219 | VoxelType& allocateVoxel(const GlobalIndex& voxel_index, Args&&... args) { 220 | return allocateBlock(blockIndexFromGlobalIndex(voxel_index, voxels_per_side), 221 | std::forward(args)...) 222 | .getVoxel(localIndexFromGlobalIndex(voxel_index, voxels_per_side)); 223 | } 224 | 225 | /** 226 | * @brief Get the voxel at a given point or allocate the corresponding block if it does not exist. 227 | * @param point Global coordinates to check. 228 | * @param args Additional arguments to pass to the block constructor. 229 | * @tparam Args The types of the additional arguments. 230 | */ 231 | template 232 | VoxelType& allocateVoxel(const Point& point, Args&&... args) { 233 | return allocateVoxel(toIndex(point), std::forward(args)...); 234 | } 235 | 236 | /** 237 | * @brief Get the voxel at a given voxel key or allocate the corresponding block if it does not 238 | * exist. 239 | * @param key The voxel key . 240 | * @param args Additional arguments to pass to the block constructor. 241 | * @tparam Args The types of the additional arguments. 242 | */ 243 | template 244 | VoxelType& allocateVoxel(const VoxelKey& key, Args&&... args) { 245 | return allocateBlock(key.first, std::forward(args)...).getVoxel(key.second); 246 | } 247 | 248 | // NOTE(lschmid): This is the only shadowing function since allocation requires different 249 | // constructor arguments. 250 | /** 251 | * @brief Gets the block at a given index or allocates a new block if it does not exist. 252 | * @param block_index The block index to retrieve or allocate. 253 | * @param args Additional arguments to pass to the block constructor. 254 | * @tparam Args The types of the additional arguments. 255 | */ 256 | template 257 | BlockT& allocateBlock(const BlockIndex& block_index, Args&&... args) { 258 | return *allocateBlockPtr(block_index, std::forward(args)...); 259 | } 260 | 261 | /** 262 | * @brief Gets the block at a given index or allocates a new block if it does not exist. 263 | * @param block_index The block index to retrieve or allocate. 264 | * @param args Additional arguments to pass to the block constructor. 265 | * @tparam Args The types of the additional arguments. 266 | */ 267 | template 268 | BlockPtr allocateBlockPtr(const BlockIndex& block_index, Args&&... args); 269 | 270 | // Iterators. 271 | struct VoxelIterable { 272 | using BlockMap = typename Layer::BlockMap; 273 | using MapIt = typename BlockMap::const_iterator; 274 | 275 | VoxelIterable(const BlockMap& blocks, size_t num_voxels) 276 | : blocks_(blocks), num_voxels_(num_voxels) {} 277 | virtual ~VoxelIterable() = default; 278 | 279 | class iterator { 280 | public: 281 | using iterator_category = std::forward_iterator_tag; 282 | using difference_type = std::ptrdiff_t; 283 | using value_type = VoxelType; 284 | using pointer = VoxelType*; 285 | using reference = VoxelType&; 286 | 287 | iterator(MapIt map_it, MapIt map_end, size_t voxel_idx, size_t num_voxels) 288 | : map_it_(map_it), map_end_(map_end), voxel_idx_(voxel_idx), num_voxels_(num_voxels) {} 289 | 290 | iterator& operator++() { 291 | voxel_idx_++; 292 | if (voxel_idx_ >= num_voxels_ && map_it_ != map_end_) { 293 | map_it_++; 294 | if (map_it_ != map_end_) { 295 | voxel_idx_ = 0; 296 | } 297 | } 298 | return *this; 299 | } 300 | 301 | iterator operator++(int) { 302 | iterator retval = *this; 303 | ++(*this); 304 | return retval; 305 | } 306 | bool operator==(iterator other) const { 307 | return map_it_ == other.map_it_ && voxel_idx_ == other.voxel_idx_; 308 | } 309 | bool operator!=(iterator other) const { return !(*this == other); } 310 | VoxelType& operator*() const { return map_it_->second->getVoxel(voxel_idx_); } 311 | VoxelType* operator->() const { return &map_it_->second->getVoxel(voxel_idx_); } 312 | 313 | private: 314 | MapIt map_it_; 315 | const MapIt map_end_; 316 | size_t voxel_idx_; 317 | const size_t num_voxels_; 318 | }; 319 | 320 | class const_iterator { 321 | public: 322 | using iterator_category = std::forward_iterator_tag; 323 | using difference_type = std::ptrdiff_t; 324 | using value_type = const VoxelType; 325 | using pointer = const VoxelType*; 326 | using reference = const VoxelType&; 327 | 328 | const_iterator(MapIt map_it, MapIt map_end, size_t voxel_idx, size_t num_voxels) 329 | : map_it_(map_it), map_end_(map_end), voxel_idx_(voxel_idx), num_voxels_(num_voxels) {} 330 | 331 | const_iterator& operator++() { 332 | voxel_idx_++; 333 | if (voxel_idx_ >= num_voxels_ && map_it_ != map_end_) { 334 | map_it_++; 335 | if (map_it_ != map_end_) { 336 | voxel_idx_ = 0; 337 | } 338 | } 339 | return *this; 340 | } 341 | 342 | const_iterator operator++(int) { 343 | const_iterator retval = *this; 344 | ++(*this); 345 | return retval; 346 | } 347 | bool operator==(const_iterator other) const { 348 | return map_it_ == other.map_it_ && voxel_idx_ == other.voxel_idx_; 349 | } 350 | bool operator!=(const_iterator other) const { return !(*this == other); } 351 | const VoxelType& operator*() const { return map_it_->second->getVoxel(voxel_idx_); } 352 | const VoxelType* operator->() const { return &map_it_->second->getVoxel(voxel_idx_); } 353 | 354 | private: 355 | MapIt map_it_; 356 | const MapIt map_end_; 357 | size_t voxel_idx_; 358 | const size_t num_voxels_; 359 | }; 360 | 361 | iterator begin() { return iterator(blocks_.begin(), blocks_.end(), 0, num_voxels_); } 362 | iterator end() { return iterator(blocks_.end(), blocks_.end(), num_voxels_, num_voxels_); } 363 | const_iterator begin() const { 364 | return const_iterator(blocks_.begin(), blocks_.end(), 0, num_voxels_); 365 | } 366 | const_iterator end() const { 367 | return const_iterator(blocks_.end(), blocks_.end(), num_voxels_, num_voxels_); 368 | } 369 | 370 | private: 371 | const BlockMap& blocks_; 372 | const size_t num_voxels_; 373 | }; 374 | 375 | /** 376 | * @brief Returns a struct that allows iterating through all voxels in the layer. 377 | */ 378 | VoxelIterable voxels() { 379 | return VoxelIterable(this->blocks_, voxels_per_side * voxels_per_side * voxels_per_side); 380 | } 381 | const VoxelIterable voxels() const { 382 | return VoxelIterable(this->blocks_, voxels_per_side * voxels_per_side * voxels_per_side); 383 | } 384 | }; 385 | 386 | } // namespace spatial_hash 387 | 388 | #include "spatial_hash/impl/voxel_layer_impl.h" 389 | --------------------------------------------------------------------------------