├── .clang-format ├── .gitattributes ├── .github └── workflows │ └── test.yml ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake └── VoxelTraversalConfig.cmake.in ├── data ├── counts.bin ├── points.bin └── ray_origins.bin ├── setup.py ├── src ├── grid.h ├── python_bindings.cpp ├── python_bindings.h ├── ray.h ├── voxel_traversal.cpp └── voxel_traversal.h ├── tests ├── test_bindings.py ├── test_on_data.cpp ├── test_voxel_counter.cpp ├── test_voxel_intersect.cpp └── test_voxel_traversal.cpp └── voxel_traversal.png /.clang-format: -------------------------------------------------------------------------------- 1 | # Use the Google style in this project. 2 | BasedOnStyle: Google 3 | 4 | # Some folks prefer to write "int& foo" while others prefer "int &foo". The 5 | # Google Style Guide only asks for consistency within a project, we chose 6 | # "int& foo" for this project: 7 | DerivePointerAlignment: false 8 | PointerAlignment: Left -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | data filter=lfs diff=lfs merge=lfs -text 2 | data/counts.bin filter=lfs diff=lfs merge=lfs -text 3 | data/points.bin filter=lfs diff=lfs merge=lfs -text 4 | data/ray_origins.bin filter=lfs diff=lfs merge=lfs -text 5 | -------------------------------------------------------------------------------- /.github/workflows/test.yml: -------------------------------------------------------------------------------- 1 | name: Run tests 2 | 3 | on: 4 | push: 5 | branches: [ main ] 6 | pull_request: 7 | branches: [ main ] 8 | 9 | jobs: 10 | build: 11 | runs-on: ubuntu-latest 12 | steps: 13 | - name: Install cmake 14 | run: mkdir -p deps/cmake && wget --no-check-certificate --quiet -O - "https://github.com/Kitware/CMake/releases/download/v3.21.3/cmake-3.21.3-linux-x86_64.tar.gz" | tar --strip-components=1 -xz -C deps/cmake && export PATH=deps/cmake/bin:${PATH} 15 | 16 | - name: Install Eigen 17 | run: sudo apt-get install -y libeigen3-dev 18 | 19 | - name: Checkout commit 20 | uses: actions/checkout@v3 21 | with: 22 | lfs: true 23 | 24 | - name: Checkout LFS objects 25 | run: git lfs checkout 26 | 27 | - name: Configure 28 | run: | 29 | cmake -S. -Bbuild -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=./install 30 | 31 | - name: Build 32 | run: | 33 | cd build && cmake --build . --target install 34 | 35 | - name: Test 36 | run: | 37 | cd build && ctest -C Debug --output-on-failure --verbose 38 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio, WebStorm and Rider 2 | # Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839 3 | 4 | # User-specific stuff 5 | .idea/**/workspace.xml 6 | .idea/**/tasks.xml 7 | .idea/**/usage.statistics.xml 8 | .idea/**/dictionaries 9 | .idea/**/shelf 10 | 11 | # AWS User-specific 12 | .idea/**/aws.xml 13 | 14 | # Generated files 15 | .idea/**/contentModel.xml 16 | 17 | # Sensitive or high-churn files 18 | .idea/**/dataSources/ 19 | .idea/**/dataSources.ids 20 | .idea/**/dataSources.local.xml 21 | .idea/**/sqlDataSources.xml 22 | .idea/**/dynamic.xml 23 | .idea/**/uiDesigner.xml 24 | .idea/**/dbnavigator.xml 25 | 26 | # Gradle 27 | .idea/**/gradle.xml 28 | .idea/**/libraries 29 | 30 | # Gradle and Maven with auto-import 31 | # When using Gradle or Maven with auto-import, you should exclude module files, 32 | # since they will be recreated, and may cause churn. Uncomment if using 33 | # auto-import. 34 | # .idea/artifacts 35 | # .idea/compiler.xml 36 | # .idea/jarRepositories.xml 37 | # .idea/modules.xml 38 | # .idea/*.iml 39 | # .idea/modules 40 | # *.iml 41 | # *.ipr 42 | 43 | # CMake 44 | cmake-build-*/ 45 | 46 | # Mongo Explorer plugin 47 | .idea/**/mongoSettings.xml 48 | 49 | # File-based project format 50 | *.iws 51 | 52 | # IntelliJ 53 | out/ 54 | 55 | # mpeltonen/sbt-idea plugin 56 | .idea_modules/ 57 | 58 | # JIRA plugin 59 | atlassian-ide-plugin.xml 60 | 61 | # Cursive Clojure plugin 62 | .idea/replstate.xml 63 | 64 | # SonarLint plugin 65 | .idea/sonarlint/ 66 | 67 | # Crashlytics plugin (for Android Studio and IntelliJ) 68 | com_crashlytics_export_strings.xml 69 | crashlytics.properties 70 | crashlytics-build.properties 71 | fabric.properties 72 | 73 | # Editor-based Rest Client 74 | .idea/httpRequests 75 | 76 | # Android studio 3.1+ serialized cache file 77 | .idea/caches/build_file_checksums.ser 78 | 79 | # Byte-compiled / optimized / DLL files 80 | __pycache__/ 81 | *.py[cod] 82 | *$py.class 83 | 84 | # C extensions 85 | *.so 86 | 87 | # Distribution / packaging 88 | .Python 89 | build/ 90 | develop-eggs/ 91 | dist/ 92 | downloads/ 93 | eggs/ 94 | .eggs/ 95 | lib/ 96 | lib64/ 97 | parts/ 98 | sdist/ 99 | var/ 100 | wheels/ 101 | *.egg-info/ 102 | .installed.cfg 103 | *.egg 104 | MANIFEST 105 | 106 | # PyInstaller 107 | # Usually these files are written by a python script from a template 108 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 109 | *.manifest 110 | *.spec 111 | 112 | # Installer logs 113 | pip-log.txt 114 | pip-delete-this-directory.txt 115 | 116 | # Unit test / coverage reports 117 | htmlcov/ 118 | .tox/ 119 | .coverage 120 | .coverage.* 121 | .cache 122 | nosetests.xml 123 | coverage.xml 124 | *.cover 125 | .hypothesis/ 126 | .pytest_cache/ 127 | 128 | # Translations 129 | *.mo 130 | *.pot 131 | 132 | # Django stuff: 133 | *.log 134 | local_settings.py 135 | db.sqlite3 136 | 137 | # Flask stuff: 138 | instance/ 139 | .webassets-cache 140 | 141 | # Scrapy stuff: 142 | .scrapy 143 | 144 | # Sphinx documentation 145 | docs/_build/ 146 | 147 | # PyBuilder 148 | target/ 149 | 150 | # Jupyter Notebook 151 | .ipynb_checkpoints 152 | 153 | # pyenv 154 | .python-version 155 | 156 | # celery beat schedule file 157 | celerybeat-schedule 158 | 159 | # SageMath parsed files 160 | *.sage.py 161 | 162 | # Environments 163 | .env 164 | .venv 165 | env/ 166 | venv/ 167 | ENV/ 168 | env.bak/ 169 | venv.bak/ 170 | 171 | # Spyder project settings 172 | .spyderproject 173 | .spyproject 174 | 175 | # Rope project settings 176 | .ropeproject 177 | 178 | # mkdocs documentation 179 | /site 180 | 181 | # mypy 182 | .mypy_cache/ 183 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.21) 2 | 3 | # allow for project VERSION option 4 | if (POLICY CMP0048) 5 | cmake_policy(SET CMP0048 NEW) 6 | endif() 7 | if (POLICY CMP0003) 8 | cmake_policy(SET CMP0003 NEW) 9 | endif() 10 | 11 | project(VoxelTraversal VERSION 1.0 LANGUAGES CXX) 12 | 13 | # guard against in-source builds 14 | if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR}) 15 | message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") 16 | endif() 17 | 18 | set(CMAKE_CXX_STANDARD 17) 19 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 20 | 21 | set(CXX_CLANG_COMPILE_OPTIONS 22 | "-march=native" 23 | "-stdlib=libstdc++" 24 | "-Weverything" 25 | "-Wno-c++98-compat" 26 | "-Wno-c++98-c++11-c++14-compat" 27 | ) 28 | set(CXX_GCC_COMPILE_OPTIONS 29 | "-march=native" 30 | "-Wall" 31 | "-Wno-unknown-pragmas" 32 | ) 33 | 34 | find_package(Eigen3 3.3 REQUIRED NO_MODULE) 35 | 36 | # ------------------ INSTALL CONFIGURATION ----------------- 37 | # Must use GNUInstallDirs to install libraries into correct 38 | # locations on all platforms. 39 | include(GNUInstallDirs) 40 | include(CMakePackageConfigHelpers) 41 | 42 | set(CMAKEPACKAGE_INSTALL_DIR 43 | "${CMAKE_INSTALL_DATADIR}/VoxelTraversal/cmake" 44 | CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where voxelTraversal.cmake is installed" 45 | ) 46 | 47 | set(voxel_traversal_INCLUDE_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}") 48 | set(voxel_traversal_ROOT_DIR ${CMAKE_INSTALL_PREFIX}) 49 | 50 | 51 | # ------------- INSTALL CONFIGURATION (RPATH) -------------- 52 | # see https://gitlab.kitware.com/cmake/community/wikis/doc/cmake/RPATH-handling 53 | # use, i.e. don't skip the full RPATH for the build tree 54 | SET(CMAKE_SKIP_BUILD_RPATH FALSE) 55 | 56 | # when building, don't use the install RPATH already 57 | # (but later on when installing) 58 | SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) 59 | SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") 60 | 61 | # add the automatically determined parts of the RPATH 62 | # which point to directories outside the build tree to the install RPATH 63 | SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) 64 | 65 | # the RPATH to be used when installing, but only if it's not a system directory 66 | LIST(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/lib" isSystemDir) 67 | IF("${isSystemDir}" STREQUAL "-1") 68 | SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") 69 | ENDIF("${isSystemDir}" STREQUAL "-1") 70 | 71 | # ------------------- FETCH GTEST --------------------- 72 | include(FetchContent) 73 | FetchContent_Declare( 74 | googletest 75 | URL https://github.com/google/googletest/archive/refs/tags/release-1.11.0.zip 76 | ) 77 | # For Windows: Prevent overriding the parent project's compiler/linker settings 78 | set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) 79 | FetchContent_MakeAvailable(googletest) 80 | 81 | # ------------------- FETCH PYBIND11 ------------------ 82 | FetchContent_Declare( 83 | pybind11 84 | GIT_REPOSITORY https://github.com/pybind/pybind11 85 | GIT_TAG v2.9.2 86 | ) 87 | FetchContent_MakeAvailable(pybind11) 88 | 89 | # ------------- VOXEL TRAVERSAL LIBRARY ----------------- 90 | add_library(voxel_traversal SHARED src/voxel_traversal.cpp) 91 | 92 | set_target_properties(voxel_traversal PROPERTIES EXPORT_NAME VoxelTraversal) 93 | set_target_properties(voxel_traversal PROPERTIES VERSION ${PROJECT_VERSION}) 94 | set_target_properties(voxel_traversal PROPERTIES SOVERSION ${PROJECT_VERSION}) 95 | set_target_properties(voxel_traversal PROPERTIES PUBLIC_HEADER "src/grid.h;src/ray.h;src/voxel_traversal.h") 96 | 97 | target_include_directories(voxel_traversal PUBLIC 98 | $ 99 | $ 100 | ) 101 | 102 | target_link_libraries(voxel_traversal Eigen3::Eigen) 103 | 104 | if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") 105 | target_compile_options(voxel_traversal PRIVATE ${CXX_CLANG_COMPILE_OPTIONS}) 106 | elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") 107 | target_compile_options(voxel_traversal PRIVATE ${CXX_GCC_COMPILE_OPTIONS}) 108 | endif() 109 | 110 | set(voxel_traversal_INCLUDE_INSTALL_DIR 111 | "${CMAKE_INSTALL_INCLUDEDIR}/VoxelTraversal" 112 | CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where voxel_traversal header files are installed" 113 | ) 114 | 115 | install(TARGETS voxel_traversal EXPORT voxelTraversalTargets 116 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 117 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 118 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} 119 | PUBLIC_HEADER DESTINATION ${voxel_traversal_INCLUDE_INSTALL_DIR} 120 | ) 121 | 122 | # The VoxelTraversal target will be located in the VoxelTraversal namespace. 123 | # Other CMake targets can refer to it using VoxelTraversal::. 124 | export(TARGETS voxel_traversal NAMESPACE VoxelTraversal:: FILE VoxelTraversalTargets.cmake) 125 | 126 | # ---------------- python bindings -------------------- 127 | # TODO(risteon): make all python bindings optional 128 | pybind11_add_module(pytraversal SHARED src/python_bindings.cpp) 129 | target_link_libraries(pytraversal PRIVATE voxel_traversal Eigen3::Eigen) 130 | 131 | 132 | # ------------------- TESTS ------------------- 133 | enable_testing() 134 | ################################## 135 | add_executable( 136 | test_voxel_traversal 137 | tests/test_voxel_traversal.cpp 138 | ) 139 | target_include_directories(test_voxel_traversal PUBLIC 140 | $ 141 | $ 142 | ) 143 | target_link_libraries( 144 | test_voxel_traversal 145 | voxel_traversal 146 | Eigen3::Eigen 147 | gtest_main 148 | ) 149 | ################################## 150 | add_executable( 151 | test_voxel_counter 152 | tests/test_voxel_counter.cpp 153 | ) 154 | target_include_directories(test_voxel_counter PUBLIC 155 | $/src 156 | $ 157 | ) 158 | target_link_libraries( 159 | test_voxel_counter 160 | voxel_traversal 161 | Eigen3::Eigen 162 | gtest_main 163 | ) 164 | ################################## 165 | add_executable( 166 | test_voxel_intersect 167 | tests/test_voxel_intersect.cpp 168 | ) 169 | target_include_directories(test_voxel_intersect PUBLIC 170 | $/src 171 | $ 172 | ) 173 | target_link_libraries( 174 | test_voxel_intersect 175 | voxel_traversal 176 | Eigen3::Eigen 177 | gtest_main 178 | ) 179 | ################################## 180 | add_executable( 181 | test_on_data 182 | tests/test_on_data.cpp 183 | ) 184 | target_include_directories(test_on_data PUBLIC 185 | $/src 186 | $ 187 | ) 188 | target_link_libraries( 189 | test_on_data 190 | voxel_traversal 191 | Eigen3::Eigen 192 | gtest_main 193 | ) 194 | 195 | include(GoogleTest) 196 | gtest_discover_tests(test_voxel_traversal) 197 | gtest_discover_tests(test_voxel_counter) 198 | gtest_discover_tests(test_voxel_intersect) 199 | gtest_discover_tests(test_on_data PROPERTIES ENVIRONMENT "DATADIR=${CMAKE_CURRENT_SOURCE_DIR}/data/") 200 | 201 | 202 | # ------------------------ INSTALL ------------------------- 203 | configure_package_config_file( 204 | ${CMAKE_CURRENT_SOURCE_DIR}/cmake/VoxelTraversalConfig.cmake.in 205 | ${CMAKE_CURRENT_BINARY_DIR}/VoxelTraversalConfig.cmake 206 | PATH_VARS voxel_traversal_INCLUDE_DIRS voxel_traversal_ROOT_DIR 207 | INSTALL_DESTINATION ${CMAKEPACKAGE_INSTALL_DIR} 208 | ) 209 | 210 | write_basic_package_version_file (VoxelTraversalConfigVersion.cmake 211 | VERSION ${PROJECT_VERSION} 212 | COMPATIBILITY SameMajorVersion 213 | ) 214 | 215 | export(PACKAGE ${PROJECT_NAME}) 216 | 217 | install(EXPORT voxelTraversalTargets NAMESPACE VoxelTraversal:: DESTINATION ${CMAKEPACKAGE_INSTALL_DIR}) 218 | 219 | install(FILES ${CMAKE_CURRENT_BINARY_DIR}/VoxelTraversalConfig.cmake 220 | ${CMAKE_CURRENT_BINARY_DIR}/VoxelTraversalConfigVersion.cmake 221 | DESTINATION ${CMAKEPACKAGE_INSTALL_DIR}) 222 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Chris Gyurgyik 4 | 5 | Copyright (c) 2022 Christoph Rist 6 | 7 | Permission is hereby granted, free of charge, to any person obtaining a copy 8 | of this software and associated documentation files (the "Software"), to deal 9 | in the Software without restriction, including without limitation the rights 10 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | 14 | The above copyright notice and this permission notice shall be included in all 15 | copies or substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | SOFTWARE. 24 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Voxel Traversal 2 | 3 | [![Build Status](https://github.com/risteon/voxel-traversal/actions/workflows/test.yml/badge.svg)](https://github.com/risteon/voxel-traversal/actions/workflows/test.yml) 4 | [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) 5 | 6 | ![voxel traversal cover image](voxel_traversal.png?raw=true) 7 | 8 | A small library to compute voxel traversal on the CPU. 9 | The implemented algorithm is J. Amanatides, A. Woo: 10 | ["A Fast Voxel Traversal Algorithm"](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.42.3443&rep=rep1&type=pdf). 11 | It is based on the code prototype of Chris Gyurgyik 12 | [Fast-Voxel-Traversal-Algorithm](https://github.com/cgyurgyik/fast-voxel-traversal-algorithm). 13 | 14 | The contributions of this repository are: 15 | * **Tests!** 16 | * Python bindings (in progress...) 17 | * Installation and cmake packaging 18 | * Use Eigen for readability, vectorization, and grid counting. 19 | * Execution and timing on real LiDAR data from the [nuScenes dataset](https://www.nuscenes.org/). The demonstration data files are bundled with git lfs. 20 | 21 | ## Requirements and Dependencies 22 | * Eigen3 23 | * C++17 compiler 24 | 25 | ## Python bindings 26 | 27 | ```bash 28 | # setup python environment, make cmake >= 3.21 available, e.g. with conda install cmake=3.22 29 | $ python setup.py install 30 | # or 31 | $ python setup.py develop 32 | $ pytest tests 33 | ``` 34 | 35 | ## Run the Tests and Install 36 | ```bash 37 | $ git clone https://github.com/risteon/voxel-traversal.git 38 | $ mkdir build && cd build 39 | # set CMAKE_INSTALL_PREFIX to your liking 40 | # specify python version with -DPYTHON_EXECUTABLE= or -DPYBIND11_PYTHON_VERSION=3.XX 41 | $ cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=../install .. 42 | # build and install 43 | $ cmake --build . --target install -- -j 4 44 | # run tests 45 | $ ctest 46 | ``` 47 | 48 | ## Find with cmake & Usage 49 | 50 | Your project's CMakeLists.txt: 51 | ```cmake 52 | cmake_minimum_required(VERSION 3.21) 53 | project(your_project) 54 | set(CMAKE_CXX_STANDARD 17) 55 | 56 | find_package(Eigen3 3.3 REQUIRED NO_MODULE) 57 | find_package(VoxelTraversal REQUIRED) 58 | 59 | add_executable(your_executable main.cpp) 60 | target_link_libraries(your_executable VoxelTraversal::VoxelTraversal) 61 | ``` 62 | When configuring, set `CMAKE_PREFIX_PATH` to this project's install directory. 63 | 64 | Code example: 65 | ```c++ 66 | #include 67 | 68 | using namespace voxel_traversal; 69 | 70 | // types for vectors and indices 71 | using V3 = typename Grid3DSpatialDef::Vector3d; 72 | using C3 = typename Grid3DSpatialDef::Index3d; 73 | using R = Ray; 74 | 75 | const V3 bound_min(0.0, 0.0, 0.0); 76 | const V3 bound_max(2.0, 2.0, 2.0); 77 | const C3 voxel_count(2, 2, 2); 78 | Grid3DSpatialDef grid(bound_min, bound_max, voxel_count); 79 | // use this subclass to count traversed voxels 80 | Grid3DTraversalCounter grid_counter(bound_min, bound_max, voxel_count); 81 | 82 | // Ray 83 | const auto ray = R::fromOriginDir({.5, .5, .5}, {1., 0., 0.}); 84 | 85 | // determine which voxels are traversed (in order from origin to end) 86 | TraversedVoxels traversed{}; 87 | const auto does_intersect = traverseVoxelGrid(ray, grid, traversed); 88 | // count traversed voxels 89 | traverseVoxelGrid(ray, grid_counter); 90 | ``` 91 | -------------------------------------------------------------------------------- /cmake/VoxelTraversalConfig.cmake.in: -------------------------------------------------------------------------------- 1 | # This file exports the voxelTraversal:: CMake target which should be passed to the 2 | # target_link_libraries command. 3 | 4 | @PACKAGE_INIT@ 5 | 6 | include ("${CMAKE_CURRENT_LIST_DIR}/voxelTraversalTargets.cmake") -------------------------------------------------------------------------------- /data/counts.bin: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:7c8be5cc52f78a17f44de3902d074f4a6e1625433b4d09fe564fed7ae4e3f6cb 3 | size 1524 4 | -------------------------------------------------------------------------------- /data/points.bin: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:662e70e962c157d5faa8d1cfc49f73867984fdcb22fdbf8985cd54f12a162ca3 3 | size 116383812 4 | -------------------------------------------------------------------------------- /data/ray_origins.bin: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:297dff98f437a96d61893117b28bd0be8d4b0098a3d3a0ad81edfade1713eed7 3 | size 4572 4 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016 The Pybind Development Team, All rights reserved. 2 | # 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # 6 | # 1. Redistributions of source code must retain the above copyright notice, this 7 | # list of conditions and the following disclaimer. 8 | # 9 | # 2. Redistributions in binary form must reproduce the above copyright notice, 10 | # this list of conditions and the following disclaimer in the documentation 11 | # and/or other materials provided with the distribution. 12 | # 13 | # 3. Neither the name of the copyright holder nor the names of its contributors 14 | # may be used to endorse or promote products derived from this software 15 | # without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 21 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 22 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 23 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 24 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 25 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | # 28 | # You are under no obligation whatsoever to provide any bug fixes, patches, or 29 | # upgrades to the features, functionality or performance of the source code 30 | # ("Enhancements") to anyone; however, if you choose to make your Enhancements 31 | # available either publicly, or directly to the author of this software, without 32 | # imposing a separate written license agreement for such Enhancements, then you 33 | # hereby grant the following license: a non-exclusive, royalty-free perpetual 34 | # license to install, use, modify, prepare derivative works, incorporate into 35 | # other computer software, distribute, and sublicense such enhancements or 36 | # derivative works thereof, in binary and source code form. 37 | 38 | import os 39 | import re 40 | import subprocess 41 | import sys 42 | 43 | from setuptools import Extension, setup 44 | from setuptools.command.build_ext import build_ext 45 | 46 | # Convert distutils Windows platform specifiers to CMake -A arguments 47 | PLAT_TO_CMAKE = { 48 | "win32": "Win32", 49 | "win-amd64": "x64", 50 | "win-arm32": "ARM", 51 | "win-arm64": "ARM64", 52 | } 53 | 54 | 55 | # A CMakeExtension needs a sourcedir instead of a file list. 56 | # The name must be the _single_ output extension from the CMake build. 57 | # If you need multiple extensions, see scikit-build. 58 | class CMakeExtension(Extension): 59 | def __init__(self, name, sourcedir=""): 60 | Extension.__init__(self, name, sources=[]) 61 | self.sourcedir = os.path.abspath(sourcedir) 62 | 63 | 64 | class CMakeBuild(build_ext): 65 | def build_extension(self, ext): 66 | extdir = os.path.abspath(os.path.dirname(self.get_ext_fullpath(ext.name))) 67 | 68 | # required for auto-detection & inclusion of auxiliary "native" libs 69 | if not extdir.endswith(os.path.sep): 70 | extdir += os.path.sep 71 | 72 | debug = int(os.environ.get("DEBUG", 0)) if self.debug is None else self.debug 73 | cfg = "Debug" if debug else "Release" 74 | 75 | # CMake lets you override the generator - we need to check this. 76 | # Can be set with Conda-Build, for example. 77 | cmake_generator = os.environ.get("CMAKE_GENERATOR", "") 78 | 79 | # Set Python_EXECUTABLE instead if you use PYBIND11_FINDPYTHON 80 | # EXAMPLE_VERSION_INFO shows you how to pass a value into the C++ code 81 | # from Python. 82 | cmake_args = [ 83 | f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY={extdir}", 84 | f"-DPYTHON_EXECUTABLE={sys.executable}", 85 | f"-DCMAKE_BUILD_TYPE={cfg}", # not used on MSVC, but no harm 86 | ] 87 | build_args = [] 88 | # Adding CMake arguments set as environment variable 89 | # (needed e.g. to build for ARM OSx on conda-forge) 90 | if "CMAKE_ARGS" in os.environ: 91 | cmake_args += [item for item in os.environ["CMAKE_ARGS"].split(" ") if item] 92 | 93 | # In this example, we pass in the version to C++. You might not need to. 94 | cmake_args += [f"-DEXAMPLE_VERSION_INFO={self.distribution.get_version()}"] 95 | 96 | if self.compiler.compiler_type != "msvc": 97 | # Using Ninja-build since it a) is available as a wheel and b) 98 | # multithreads automatically. MSVC would require all variables be 99 | # exported for Ninja to pick it up, which is a little tricky to do. 100 | # Users can override the generator with CMAKE_GENERATOR in CMake 101 | # 3.15+. 102 | if not cmake_generator or cmake_generator == "Ninja": 103 | try: 104 | import ninja # noqa: F401 105 | 106 | ninja_executable_path = os.path.join(ninja.BIN_DIR, "ninja") 107 | cmake_args += [ 108 | "-GNinja", 109 | f"-DCMAKE_MAKE_PROGRAM:FILEPATH={ninja_executable_path}", 110 | ] 111 | except ImportError: 112 | pass 113 | 114 | else: 115 | 116 | # Single config generators are handled "normally" 117 | single_config = any(x in cmake_generator for x in {"NMake", "Ninja"}) 118 | 119 | # CMake allows an arch-in-generator style for backward compatibility 120 | contains_arch = any(x in cmake_generator for x in {"ARM", "Win64"}) 121 | 122 | # Specify the arch if using MSVC generator, but only if it doesn't 123 | # contain a backward-compatibility arch spec already in the 124 | # generator name. 125 | if not single_config and not contains_arch: 126 | cmake_args += ["-A", PLAT_TO_CMAKE[self.plat_name]] 127 | 128 | # Multi-config generators have a different way to specify configs 129 | if not single_config: 130 | cmake_args += [ 131 | f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{cfg.upper()}={extdir}" 132 | ] 133 | build_args += ["--config", cfg] 134 | 135 | if sys.platform.startswith("darwin"): 136 | # Cross-compile support for macOS - respect ARCHFLAGS if set 137 | archs = re.findall(r"-arch (\S+)", os.environ.get("ARCHFLAGS", "")) 138 | if archs: 139 | cmake_args += ["-DCMAKE_OSX_ARCHITECTURES={}".format(";".join(archs))] 140 | 141 | # Set CMAKE_BUILD_PARALLEL_LEVEL to control the parallel build level 142 | # across all generators. 143 | if "CMAKE_BUILD_PARALLEL_LEVEL" not in os.environ: 144 | # self.parallel is a Python 3 only way to set parallel jobs by hand 145 | # using -j in the build_ext call, not supported by pip or PyPA-build. 146 | if hasattr(self, "parallel") and self.parallel: 147 | # CMake 3.12+ only. 148 | build_args += [f"-j{self.parallel}"] 149 | 150 | build_temp = os.path.join(self.build_temp, ext.name) 151 | if not os.path.exists(build_temp): 152 | os.makedirs(build_temp) 153 | 154 | subprocess.check_call(["cmake", ext.sourcedir] + cmake_args, cwd=build_temp) 155 | subprocess.check_call(["cmake", "--build", "."] + build_args, cwd=build_temp) 156 | 157 | 158 | # The information here can also be placed in setup.cfg - better separation of 159 | # logic and declaration, and simpler if you include description/version in a file. 160 | setup( 161 | name="pytraversal", 162 | version="1.1", 163 | author="Christoph Rist", 164 | author_email="c.rist@posteo.de", 165 | description="python bindings for voxel traversal", 166 | long_description="", 167 | ext_modules=[CMakeExtension("pytraversal")], 168 | cmdclass={"build_ext": CMakeBuild}, 169 | zip_safe=False, 170 | extras_require={"test": ["pytest>=6.0"]}, 171 | python_requires=">=3.6", 172 | ) 173 | -------------------------------------------------------------------------------- /src/grid.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXEL_TRAVERSAL_GRID_H 2 | #define VOXEL_TRAVERSAL_GRID_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace voxel_traversal { 9 | 10 | template 11 | class Grid3DSpatialDef { 12 | public: 13 | // need to express negative voxel indices when calculating voxel traversal 14 | using int_type = int32_t; 15 | using float_t = float_type; 16 | 17 | using Vector3d = Eigen::Matrix; 18 | using Size3d = Eigen::Array; 19 | using Index3d = Eigen::Array; 20 | 21 | Grid3DSpatialDef() = default; 22 | Grid3DSpatialDef(const Vector3d& min_bound, const Vector3d& max_bound, 23 | const Index3d& num_voxels) 24 | : min_bound_{min_bound}, 25 | max_bound_{max_bound}, 26 | grid_size_{max_bound - min_bound}, 27 | num_voxels_{num_voxels}, 28 | voxel_size_{grid_size_ / num_voxels.cast()}, 29 | voxel_count_{static_cast(num_voxels.prod())} { 30 | assert((num_voxels_ > 0).all()); 31 | assert((min_bound_.array() < max_bound_.array()).all()); 32 | } 33 | virtual ~Grid3DSpatialDef() = default; 34 | 35 | Grid3DSpatialDef(const Grid3DSpatialDef& other) = default; 36 | Grid3DSpatialDef(Grid3DSpatialDef&& other) noexcept : Grid3DSpatialDef() { 37 | swap(*this, other); 38 | } 39 | //! Copy-and-swap. Handles both lvalues and rvalues 40 | Grid3DSpatialDef& operator=(Grid3DSpatialDef other) noexcept { 41 | swap(*this, other); 42 | return *this; 43 | } 44 | 45 | [[nodiscard]] const Index3d& numVoxels() const { return num_voxels_; } 46 | 47 | [[nodiscard]] const Vector3d& minBound() const { return min_bound_; } 48 | [[nodiscard]] const Vector3d& maxBound() const { return max_bound_; } 49 | [[nodiscard]] const Size3d& gridSize() const { return grid_size_; } 50 | [[nodiscard]] const Size3d& voxelSize() const { return voxel_size_; } 51 | //! Total number of voxels in this grid 52 | [[nodiscard]] uint64_t voxelCount() const noexcept { return voxel_count_; } 53 | 54 | //! Maximum number of voxels that can be traversed by a single ray 55 | [[nodiscard]] int_type upperBoundVoxelTraversal() const { 56 | return num_voxels_.sum(); 57 | } 58 | 59 | //! Get voxel index of given position 60 | [[nodiscard]] Index3d getIndex(const Vector3d& v) const noexcept { 61 | return ((v - min_bound_).array() / voxel_size_) 62 | .floor() 63 | .template cast(); 64 | } 65 | 66 | //! Check if given index falls into the voxel grid 67 | [[nodiscard]] bool isWithinGrid(const Index3d& index) const noexcept { 68 | return (index >= 0).all() && (index < num_voxels_).all(); 69 | } 70 | 71 | //! Check if given points falls into the voxel grid 72 | [[nodiscard]] bool isWithinGrid(const Vector3d& v) const noexcept { 73 | return (v.array() >= min_bound_.array()).all() && 74 | (v.array() <= max_bound_.array()).all(); 75 | } 76 | 77 | //! For copy-and-swap 78 | friend void swap(Grid3DSpatialDef& first, 79 | Grid3DSpatialDef& second) noexcept { 80 | // enable ADL 81 | using std::swap; 82 | swap(first.min_bound_, second.min_bound_); 83 | swap(first.max_bound_, second.max_bound_); 84 | swap(first.grid_size_, second.grid_size_); 85 | swap(first.num_voxels_, second.num_voxels_); 86 | swap(first.voxel_size_, second.voxel_size_); 87 | swap(first.voxel_count_, second.voxel_count_); 88 | } 89 | 90 | protected: 91 | // The minimum bound vector of the voxel grid. 92 | Vector3d min_bound_; 93 | // The maximum bound vector of the voxel grid. 94 | Vector3d max_bound_; 95 | // The grid size, determined by (max_bound_ - min_bound_). 96 | Size3d grid_size_; 97 | // The number of voxels in each of the x, y, z directions. 98 | Index3d num_voxels_; 99 | // The size of the voxel's x dimension. 100 | Size3d voxel_size_; 101 | // Number of voxels in this grid 102 | uint64_t voxel_count_; 103 | }; 104 | 105 | template 106 | class Grid3DTraversalCounter : public Grid3DSpatialDef { 107 | public: 108 | // declarations for independent base class types 109 | using typename Grid3DSpatialDef::Vector3d; 110 | using typename Grid3DSpatialDef::Index3d; 111 | 112 | using counter_type = uint64_t; 113 | using tensor_type = Eigen::Tensor; 114 | 115 | Grid3DTraversalCounter() = default; 116 | virtual ~Grid3DTraversalCounter() override = default; 117 | 118 | Grid3DTraversalCounter(const Grid3DTraversalCounter& other) = default; 119 | 120 | //! Copy-and-swap. Handles both lvalues and rvalues 121 | Grid3DTraversalCounter& operator=(Grid3DTraversalCounter other) { 122 | swap(*this, other); 123 | return *this; 124 | } 125 | Grid3DTraversalCounter(Grid3DTraversalCounter&& other) noexcept 126 | : Grid3DTraversalCounter() { 127 | swap(*this, other); 128 | } 129 | 130 | Grid3DTraversalCounter(const Vector3d& min_bound, const Vector3d& max_bound, 131 | const Index3d& num_voxels) 132 | : Grid3DSpatialDef(min_bound, max_bound, num_voxels), 133 | counter_(tensor_type(num_voxels[0], num_voxels[1], num_voxels[2])) { 134 | assert((this->num_voxels_ > 0).all()); 135 | assert((this->min_bound_.array() < this->max_bound_.array()).all()); 136 | counter_.setZero(); 137 | } 138 | 139 | [[nodiscard]] const tensor_type& getCounter() const { return counter_; } 140 | 141 | void clear() { counter_.setZero(); } 142 | void increaseAt(const Index3d& index) { counter_(index)++; } 143 | 144 | friend void swap(Grid3DTraversalCounter& first, 145 | Grid3DTraversalCounter& second) noexcept { 146 | using std::swap; 147 | // TOOD(risteon) good option to call base class swap? 148 | swap(first.min_bound_, second.min_bound_); 149 | swap(first.max_bound_, second.max_bound_); 150 | swap(first.grid_size_, second.grid_size_); 151 | swap(first.num_voxels_, second.num_voxels_); 152 | swap(first.voxel_size_, second.voxel_size_); 153 | swap(first.voxel_count_, second.voxel_count_); 154 | swap(first.counter_, second.counter_); 155 | } 156 | 157 | private: 158 | tensor_type counter_; 159 | }; 160 | 161 | } // namespace voxel_traversal 162 | #endif // VOXEL_TRAVERSAL_GRID_H 163 | -------------------------------------------------------------------------------- /src/python_bindings.cpp: -------------------------------------------------------------------------------- 1 | #include "python_bindings.h" 2 | 3 | #include 4 | 5 | #include "voxel_traversal.h" 6 | 7 | namespace py = pybind11; 8 | using namespace voxel_traversal; 9 | 10 | namespace pytraversal { 11 | 12 | py::array_t traverse( 13 | const grid_type& grid, const grid_type::Vector3d& ray_origin, 14 | const grid_type::Vector3d& ray_end) { 15 | TraversedVoxels traversed_voxels{}; 16 | const auto ray = Ray::fromOriginEnd(ray_origin, ray_end); 17 | const bool hit = traverseVoxelGrid(ray, grid, traversed_voxels); 18 | 19 | if (hit) { 20 | return py::cast(traversed_voxels); 21 | } else { 22 | // make sure to return empty array with correct shape [0, 3] 23 | const py::array::ShapeContainer shape({0, 3}); 24 | return py::array_t(shape); 25 | } 26 | } 27 | 28 | } // namespace pytraversal 29 | 30 | PYBIND11_MODULE(pytraversal, m) { 31 | using namespace pytraversal; 32 | m.doc() = "pytraversal bindings"; 33 | 34 | py::class_(m, "Grid3D") 35 | .def(py::init<>()) 36 | .def(py::init()) 38 | .def("traverse", traverse); 39 | } 40 | -------------------------------------------------------------------------------- /src/python_bindings.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXEL_TRAVERSAL_PYTHON_BINDINGS_H 2 | #define VOXEL_TRAVERSAL_PYTHON_BINDINGS_H 3 | 4 | #include 5 | #include 6 | 7 | #include "grid.h" 8 | 9 | namespace pytraversal { 10 | 11 | using grid_type = voxel_traversal::Grid3DSpatialDef; 12 | 13 | pybind11::array_t traverse( 14 | const grid_type& grid, const grid_type::Vector3d& ray_origin, 15 | const grid_type::Vector3d& ray_end); 16 | 17 | } // namespace pytraversal 18 | 19 | #endif // VOXEL_TRAVERSAL_PYTHON_BINDINGS_H 20 | -------------------------------------------------------------------------------- /src/ray.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXEL_TRAVERSAL_RAY_H 2 | #define VOXEL_TRAVERSAL_RAY_H 3 | 4 | #include 5 | 6 | namespace voxel_traversal { 7 | 8 | template 9 | class Ray { 10 | public: 11 | using Vector3d = Eigen::Matrix; 12 | 13 | static Ray fromOriginDir(const Vector3d& origin, const Vector3d& dir) { 14 | return Ray(origin, origin + dir, dir); 15 | } 16 | static Ray fromOriginEnd(const Vector3d& start, const Vector3d& end) { 17 | return Ray(start, end, end - start); 18 | } 19 | 20 | // Represents the function p(t) = origin + t * direction, 21 | // where p is a 3-dimensional position, and t is a scalar. 22 | [[nodiscard]] Vector3d at(float_type t) const { 23 | return (origin_ * (float_type{1.0} - t)) + (end_point_ * t); 24 | } 25 | 26 | [[nodiscard]] const Vector3d& origin() const noexcept { return origin_; } 27 | [[nodiscard]] const Vector3d& endPoint() const noexcept { return end_point_; } 28 | [[nodiscard]] const Vector3d& direction() const noexcept { return direction_; } 29 | 30 | private: 31 | explicit Ray(Vector3d origin, Vector3d end_point, Vector3d direction) 32 | : origin_{std::move(origin)}, 33 | end_point_{std::move(end_point)}, 34 | direction_{std::move(direction)} {} 35 | 36 | // origin and end point of the ray 37 | Vector3d origin_; 38 | Vector3d end_point_; 39 | // end - origin 40 | Vector3d direction_; 41 | }; 42 | 43 | } // namespace voxel_traversal 44 | 45 | #endif // VOXEL_TRAVERSAL_RAY_H 46 | -------------------------------------------------------------------------------- /src/voxel_traversal.cpp: -------------------------------------------------------------------------------- 1 | #include "voxel_traversal.h" 2 | 3 | #include 4 | 5 | namespace voxel_traversal { 6 | 7 | namespace detail { 8 | template 9 | bool setupTraversal(const Ray& ray, const Grid& grid, 10 | typename Grid::float_t t0, typename Grid::float_t t1, 11 | typename Grid::Size3d& delta, typename Grid::Size3d& tmax, 12 | typename Grid::Index3d& step_index, 13 | typename Grid::Index3d& current_index, 14 | typename Grid::Index3d& final_index) { 15 | using float_type = typename Grid::float_t; 16 | using int_type = typename Grid::int_type; 17 | 18 | float_type tmin_temp{}; 19 | float_type tmax_temp{}; 20 | const bool ray_intersects_grid = 21 | rayBoxIntersection(ray, grid, tmin_temp, tmax_temp, t0, t1); 22 | if (!ray_intersects_grid) return false; 23 | 24 | tmin_temp = std::max(tmin_temp, t0); 25 | tmax_temp = std::min(tmax_temp, t1); 26 | const auto ray_start = ray.at(tmin_temp); 27 | const auto ray_end = ray.at(tmax_temp); 28 | 29 | // get voxel index of start and end within grid 30 | const auto voxelIndexStartUnlimited = grid.getIndex(ray_start); 31 | current_index = 32 | voxelIndexStartUnlimited.cwiseMax(0).cwiseMin(grid.numVoxels() - 1); 33 | 34 | const auto voxelIndexEndUnlimited = grid.getIndex(ray_end); 35 | final_index = 36 | voxelIndexEndUnlimited.cwiseMax(0).cwiseMin(grid.numVoxels() - 1); 37 | 38 | // const auto tmax_xyz = grid.minBound() + 39 | const auto index_delta = 40 | (ray.direction().array() > 0.0).template cast(); 41 | const auto start_index = current_index + index_delta; 42 | const auto tmax_xyz = 43 | ((grid.minBound().array() + 44 | ((start_index.template cast() * grid.voxelSize()) - 45 | ray_start.array())) / 46 | ray.direction().array()) + 47 | tmin_temp; 48 | 49 | tmax = (ray.direction().array() == 0.0).select(tmax_temp, tmax_xyz); 50 | const auto step_float = ray.direction().template array().sign().eval(); 51 | step_index = step_float.template cast().eval(); 52 | delta = (step_index == 0) 53 | .select(tmax_temp, 54 | grid.voxelSize() / ray.direction().array() * step_float); 55 | 56 | return true; 57 | } 58 | 59 | template 60 | bool traverseSingle( 61 | typename Grid3DSpatialDef::Size3d& tmax, 62 | typename Grid3DSpatialDef::Index3d& current_index, 63 | const typename Grid3DSpatialDef::Index3d& overflow_index, 64 | const typename Grid3DSpatialDef::Index3d& step_index, 65 | const typename Grid3DSpatialDef::Size3d& delta) { 66 | if (tmax.x() < tmax.y() && tmax.x() < tmax.z()) { 67 | // X-axis traversal. 68 | current_index.x() += step_index.x(); 69 | tmax.x() += delta.x(); 70 | if (current_index.x() == overflow_index.x()) { 71 | return false; 72 | } 73 | } else if (tmax.y() < tmax.z()) { 74 | // Y-axis traversal. 75 | current_index.y() += step_index.y(); 76 | tmax.y() += delta.y(); 77 | if (current_index.y() == overflow_index.y()) { 78 | return false; 79 | } 80 | } else { 81 | // Z-axis traversal. 82 | current_index.z() += step_index.z(); 83 | tmax.z() += delta.z(); 84 | if (current_index.z() == overflow_index.z()) { 85 | return false; 86 | } 87 | } 88 | return true; 89 | } 90 | } // namespace detail 91 | 92 | // Uses the improved version of Smit's algorithm to determine if the given ray 93 | // will intersect the grid between tmin and tmax. This version causes an 94 | // additional efficiency penalty, but takes into account the negative zero case. 95 | // tMin and tmax are then updated to incorporate the new intersection values. 96 | // Returns true if the ray intersects the grid, and false otherwise. 97 | // See: http://www.cs.utah.edu/~awilliam/box/box.pdf 98 | template 99 | [[nodiscard]] bool rayBoxIntersection(const Ray& ray, 100 | const Grid3DSpatialDef& grid, 101 | float_type& tmin, float_type& tmax, 102 | float_type t0, float_type t1) noexcept { 103 | float_type tmin_temp{}; 104 | float_type tmax_temp{}; 105 | const auto inverse_direction = ray.direction().cwiseInverse(); 106 | 107 | if (inverse_direction.x() >= 0) { 108 | tmin = (grid.minBound().x() - ray.origin().x()) * inverse_direction.x(); 109 | tmax = (grid.maxBound().x() - ray.origin().x()) * inverse_direction.x(); 110 | } else { 111 | tmin = (grid.maxBound().x() - ray.origin().x()) * inverse_direction.x(); 112 | tmax = (grid.minBound().x() - ray.origin().x()) * inverse_direction.x(); 113 | } 114 | 115 | if (inverse_direction.y() >= 0) { 116 | tmin_temp = (grid.minBound().y() - ray.origin().y()) * inverse_direction.y(); 117 | tmax_temp = (grid.maxBound().y() - ray.origin().y()) * inverse_direction.y(); 118 | } else { 119 | tmin_temp = (grid.maxBound().y() - ray.origin().y()) * inverse_direction.y(); 120 | tmax_temp = (grid.minBound().y() - ray.origin().y()) * inverse_direction.y(); 121 | } 122 | 123 | if (tmin > tmax_temp || tmin_temp > tmax) return false; 124 | if (tmin_temp > tmin) tmin = tmin_temp; 125 | if (tmax_temp < tmax) tmax = tmax_temp; 126 | 127 | if (inverse_direction.z() >= 0) { 128 | tmin_temp = (grid.minBound().z() - ray.origin().z()) * inverse_direction.z(); 129 | tmax_temp = (grid.maxBound().z() - ray.origin().z()) * inverse_direction.z(); 130 | } else { 131 | tmin_temp = (grid.maxBound().z() - ray.origin().z()) * inverse_direction.z(); 132 | tmax_temp = (grid.minBound().z() - ray.origin().z()) * inverse_direction.z(); 133 | } 134 | 135 | if (tmin > tmax_temp || tmin_temp > tmax) return false; 136 | if (tmin_temp > tmin) tmin = tmin_temp; 137 | if (tmax_temp < tmax) tmax = tmax_temp; 138 | return (tmin < t1 && tmax > t0); 139 | } 140 | 141 | template 142 | bool traverseVoxelGrid(const Ray& ray, 143 | Grid3DTraversalCounter& grid, float_type t0, 144 | float_type t1) noexcept { 145 | using grid_type = Grid3DTraversalCounter; 146 | typename grid_type::Size3d delta{}; 147 | typename grid_type::Size3d tmax{}; 148 | typename grid_type::Index3d step_index{}; 149 | typename grid_type::Index3d current_index{}; 150 | typename grid_type::Index3d final_index{}; 151 | 152 | const auto intersect = detail::setupTraversal( 153 | ray, grid, t0, t1, delta, tmax, step_index, current_index, final_index); 154 | 155 | if (!intersect) return false; 156 | 157 | grid.increaseAt(current_index); 158 | 159 | // one too far in every direction. Stop as soon as we hit any of these "walls" 160 | // It can happen, that the final_index is not exacty hit (float errors) 161 | const typename grid_type::Index3d overflow_index = final_index + step_index; 162 | 163 | while (true) { 164 | if (!detail::traverseSingle(tmax, current_index, overflow_index, 165 | step_index, delta)) { 166 | break; 167 | } 168 | grid.increaseAt(current_index); 169 | } 170 | return true; 171 | } 172 | 173 | template 174 | bool traverseVoxelGrid(const Ray& ray, 175 | const Grid3DSpatialDef& grid, 176 | TraversedVoxels& traversed_voxels, 177 | float_type t0, float_type t1) noexcept { 178 | using grid_type = Grid3DSpatialDef; 179 | typename grid_type::Size3d delta{}; 180 | typename grid_type::Size3d tmax{}; 181 | typename grid_type::Index3d step_index{}; 182 | typename grid_type::Index3d current_index{}; 183 | typename grid_type::Index3d final_index{}; 184 | 185 | traversed_voxels.clear(); 186 | 187 | const auto intersect = detail::setupTraversal>( 188 | ray, grid, t0, t1, delta, tmax, step_index, current_index, final_index); 189 | if (!intersect) return false; 190 | 191 | traversed_voxels.push_back(current_index); 192 | 193 | // one too far in every direction. Stop as soon as we hit any of these "walls" 194 | // It can happen, that the final_index is not exacty hit (float errors) 195 | const typename grid_type::Index3d overflow_index = final_index + step_index; 196 | 197 | while (true) { 198 | if (!detail::traverseSingle(tmax, current_index, overflow_index, 199 | step_index, delta)) { 200 | break; 201 | } 202 | traversed_voxels.push_back(current_index); 203 | } 204 | 205 | return true; 206 | } 207 | 208 | // instantiations 209 | template bool traverseVoxelGrid(const Ray&, 210 | const Grid3DSpatialDef&, 211 | TraversedVoxels&, float, float); 212 | template bool traverseVoxelGrid(const Ray&, 213 | const Grid3DSpatialDef&, 214 | TraversedVoxels&, double, 215 | double); 216 | template bool traverseVoxelGrid( 217 | const Ray&, const Grid3DSpatialDef&, 218 | TraversedVoxels&, long double, long double); 219 | 220 | template bool traverseVoxelGrid(const Ray&, 221 | Grid3DTraversalCounter&, float t0, 222 | float t1); 223 | template bool traverseVoxelGrid(const Ray&, 224 | Grid3DTraversalCounter&, 225 | double t0, double t1); 226 | template bool traverseVoxelGrid( 227 | const Ray&, Grid3DTraversalCounter&, 228 | long double t0, long double t1); 229 | 230 | } // namespace voxel_traversal -------------------------------------------------------------------------------- /src/voxel_traversal.h: -------------------------------------------------------------------------------- 1 | #ifndef VOXEL_TRAVERSAL_H 2 | #define VOXEL_TRAVERSAL_H 3 | 4 | #include "grid.h" 5 | #include "ray.h" 6 | 7 | namespace voxel_traversal { 8 | 9 | //! Type to record the indices of traversed voxels 10 | template 11 | using TraversedVoxels = 12 | std::vector::Index3d>; 13 | 14 | /*! 15 | * Traverse a voxel grid along a ray and record all traversed voxels. 16 | * 17 | * Implements the algorithm presented in Amanatides & Woo's "A Fast Voxel 18 | * Traversal Algorithm for Ray Tracing." 19 | * If the ray origin is outside the voxel grid, uses a safer version of Smit's 20 | * ray box intersection algorithm to determine intersection. 21 | * The voxel indices in traversed_voxels are in order of traversal from 22 | * ray origin to ray end. 23 | * 24 | * @tparam float_type precision 25 | * @param ray Start at ray origin and end traversal at ray end. 26 | * @param grid Defines voxel grid. 27 | * @param traversed_voxels Output for the recorded voxels. 28 | * @param t0 ray start bound. 0 equals the ray origin. 29 | * @param t1 ray end bound. 1 equals the ray end. 30 | * @return true if the ray intersects the voxel grid 31 | */ 32 | template 33 | bool traverseVoxelGrid(const Ray& ray, 34 | const Grid3DSpatialDef& grid, 35 | TraversedVoxels& traversed_voxels, 36 | float_type t0 = float_type{0.0}, 37 | float_type t1 = float_type{1.0}) noexcept; 38 | 39 | /*! 40 | * Traverse a voxel grid along a ray and increase grid counter for traversed 41 | * voxels. 42 | * 43 | * Implements the algorithm presented in Amanatides & Woo's "A Fast Voxel 44 | * Traversal Algorithm for Ray Tracing." 45 | * If the ray origin is outside the voxel grid, uses a safer version of Smit's 46 | * ray box intersection algorithm to determine intersection. 47 | * 48 | * @tparam float_type precision 49 | * @param ray Start at ray origin and end traversal at ray end. 50 | * @param grid Defines voxel grid and holds the permanent voxel counter. 51 | * @param t0 ray start bound. 0 equals the ray origin. 52 | * @param t1 ray end bound. 1 equals the ray end. 53 | * @return true if the ray intersects the voxel grid 54 | */ 55 | template 56 | bool traverseVoxelGrid(const Ray& ray, 57 | Grid3DTraversalCounter& grid, 58 | float_type t0 = float_type{0.0}, 59 | float_type t1 = float_type{1.0}) noexcept; 60 | 61 | /*! 62 | * Get the positions t_min and t_max along a ray where it enters and exists a 63 | * voxel grid. 64 | * 65 | * @tparam float_type precision 66 | * @param ray Start at ray origin and end traversal at ray end. 67 | * @param grid Define voxel grid 68 | * @param tmin smallest position along ray that falls into the grid 69 | * @param tmax largest position along ray that falls into the grid 70 | * @param t0 ray start bound. 0 equals the ray origin. 71 | * @param t1 ray end bound. 1 equals the ray end. 72 | * @return true if the ray intersects the voxel grid 73 | */ 74 | template 75 | [[nodiscard]] bool rayBoxIntersection(const Ray& ray, 76 | const Grid3DSpatialDef& grid, 77 | float_type& tmin, float_type& tmax, 78 | float_type t0 = 0.0, 79 | float_type t1 = 1.0) noexcept; 80 | } // namespace voxel_traversal 81 | 82 | #endif // VOXEL_TRAVERSAL_H 83 | -------------------------------------------------------------------------------- /tests/test_bindings.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pytraversal 3 | 4 | 5 | def test_create_grid_without_args(): 6 | _ = pytraversal.Grid3D() 7 | 8 | 9 | def test_single_traversal(): 10 | # define grid 11 | grid = pytraversal.Grid3D([0., 0., -50.], [4., 2., -15.], [4, 2, 1]) 12 | # define expectation 13 | expected = np.asarray([[1, 0, 0], [2, 0, 0], [2, 1, 0], [3, 1, 0]], np.int64) 14 | 15 | traversed = grid.traverse([1.5, 0.8, -25.0], [6.0, 1.7, -25.0]) 16 | np.testing.assert_array_equal(expected, traversed) 17 | 18 | traversed = grid.traverse([6.0, 1.7, -25.0], [1.5, 0.8, -25.0]) 19 | np.testing.assert_array_equal(expected[::-1], traversed) 20 | -------------------------------------------------------------------------------- /tests/test_on_data.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "voxel_traversal.h" 12 | 13 | using namespace voxel_traversal; 14 | 15 | // Small voxel grid 2x2x2 16 | template 17 | class TestOnData : public ::testing::Test { 18 | protected: 19 | using V3 = typename Grid3DSpatialDef::Vector3d; 20 | using C3 = typename Grid3DSpatialDef::Index3d; 21 | using R = Ray; 22 | // parse raw points, saved as float32 23 | using V3f = Eigen::Vector3f; 24 | 25 | void SetUp() override { 26 | const V3 bound_min(0.0, -25.6, -2.0); 27 | const V3 bound_max(51.2, 25.6, 4.4); 28 | const C3 voxel_count(256, 256, 32); 29 | 30 | grid_ = Grid3DSpatialDef(bound_min, bound_max, voxel_count); 31 | grid_counter_ = 32 | Grid3DTraversalCounter(bound_min, bound_max, voxel_count); 33 | 34 | ReadPoints(points_raw_, "points.bin"); 35 | ReadPoints(origins_raw_, "ray_origins.bin"); 36 | ReadPoints(frame_counts_, "counts.bin"); 37 | 38 | TransformRawData(); 39 | // free up memory 40 | points_raw_ = decltype(points_raw_){}; 41 | origins_raw_ = decltype(origins_raw_){}; 42 | } 43 | 44 | void TransformRawData() { 45 | // check data consistency 46 | ASSERT_EQ(points_raw_.size() % 3, 0); 47 | ASSERT_EQ(origins_raw_.size() % 3, 0); 48 | const auto num_frames = origins_raw_.size() / 3; 49 | const auto num_points = points_raw_.size() / 3; 50 | ASSERT_EQ(num_frames, frame_counts_.size()); 51 | total_point_count_ = 52 | std::accumulate(frame_counts_.cbegin(), frame_counts_.cend(), 0); 53 | ASSERT_EQ(total_point_count_, num_points); 54 | 55 | origins_.resize(num_frames); 56 | points_.resize(num_frames); 57 | 58 | for (std::size_t i = 0, j = 0; j < origins_.size(); ++j, i += 3) { 59 | origins_[j] = V3f(origins_raw_.data() + i).cast(); 60 | } 61 | std::size_t batch_count = 0; 62 | std::size_t current_frame = 0; 63 | for (const auto frame_count : frame_counts_) { 64 | auto& pp = points_[current_frame]; 65 | pp.resize(frame_count); 66 | for (std::size_t i = 0, j = 0; j < frame_count; ++j, i += 3) { 67 | pp[j] = 68 | V3f(points_raw_.data() + batch_count * 3 + i).cast(); 69 | } 70 | batch_count += frame_count; 71 | ++current_frame; 72 | } 73 | 74 | ASSERT_EQ(points_.size(), origins_.size()); 75 | } 76 | 77 | template 78 | static void ReadPoints(std::vector& into, 79 | const std::string& filename) { 80 | const char* datadir = std::getenv("DATADIR"); 81 | ASSERT_TRUE(datadir != nullptr) << "Env variable 'DATADIR' not available."; 82 | 83 | const auto filepath = std::string(datadir) + filename; 84 | std::ifstream f_points(filepath, std::ios::binary | std::ios::in); 85 | ASSERT_TRUE(f_points.good()) << "Cannot open " << filepath; 86 | f_points.seekg(0, std::ios::end); 87 | const auto num_bytes = f_points.tellg(); 88 | ASSERT_TRUE(num_bytes % sizeof(Scalar) == 0); 89 | const auto num_values = num_bytes / sizeof(float); 90 | 91 | f_points.seekg(0); 92 | into.reserve(num_values); 93 | Scalar value{}; 94 | while (f_points.read(reinterpret_cast(&value), sizeof(Scalar))) { 95 | into.push_back(value); 96 | } 97 | into.shrink_to_fit(); 98 | ASSERT_EQ(into.size(), num_values); 99 | }; 100 | 101 | Grid3DSpatialDef grid_; 102 | Grid3DTraversalCounter grid_counter_; 103 | 104 | std::vector points_raw_; 105 | std::vector origins_raw_; 106 | std::vector frame_counts_; 107 | // multiple frames with their respective origin 108 | std::vector> points_; 109 | std::vector origins_; 110 | std::size_t total_point_count_; 111 | }; 112 | 113 | using Implementations = ::testing::Types; 114 | TYPED_TEST_SUITE(TestOnData, Implementations); 115 | 116 | TYPED_TEST(TestOnData, timeIntersectionCalculation) { 117 | using std::chrono::duration; 118 | using std::chrono::duration_cast; 119 | using std::chrono::high_resolution_clock; 120 | 121 | TypeParam t_min, t_max; 122 | uint32_t intersection_count{0}; 123 | TraversedVoxels traversedVoxels{}; 124 | traversedVoxels.reserve(1000); 125 | 126 | auto t1 = high_resolution_clock::now(); 127 | for (std::size_t frame = 0; frame < TestFixture::points_.size(); ++frame) { 128 | const auto& pp = TestFixture::points_[frame]; 129 | const typename TestFixture::V3& origin = TestFixture::origins_[frame]; 130 | 131 | if (frame % 50 == 0) { 132 | std::cout << "...processing frame #" << frame << " with #" << pp.size() 133 | << " points..." << std::endl; 134 | } 135 | 136 | for (const auto& point : pp) { 137 | const auto ray = TestFixture::R::fromOriginEnd(origin, point); 138 | const auto intersect = 139 | rayBoxIntersection(ray, TestFixture::grid_, t_min, t_max); 140 | if (intersect) { 141 | ++intersection_count; 142 | } 143 | } 144 | } 145 | auto t2 = high_resolution_clock::now(); 146 | const duration ss = t2 - t1; 147 | 148 | std::cout << "Elapsed seconds: " << ss.count() << std::endl; 149 | std::cout << "Total number of rays: " << TestFixture::total_point_count_ 150 | << std::endl; 151 | std::cout << "Total number of intersections: " << intersection_count 152 | << std::endl; 153 | 154 | const std::size_t expected_hits{4097337}; 155 | std::cout << "Expected #" << expected_hits << " hits." << std::endl; 156 | if (intersection_count != expected_hits) { 157 | std::cout << "!! Deviation of " 158 | << (static_cast(intersection_count) / 159 | static_cast(expected_hits) - 160 | 1.0) * 161 | 100.0 162 | << "%" << std::endl; 163 | } else { 164 | std::cout << "... exactly as expected." << std::endl; 165 | } 166 | } 167 | 168 | TYPED_TEST(TestOnData, timeTraversalCalculation) { 169 | using std::chrono::duration; 170 | using std::chrono::duration_cast; 171 | using std::chrono::high_resolution_clock; 172 | 173 | double t_min, t_max; 174 | uint32_t intersection_count{0}; 175 | uint64_t traversal_count{0}; 176 | 177 | TraversedVoxels traversedVoxels{}; 178 | traversedVoxels.reserve(TestFixture::grid_.upperBoundVoxelTraversal()); 179 | 180 | auto t1 = high_resolution_clock::now(); 181 | for (std::size_t frame = 0; frame < TestFixture::points_.size(); ++frame) { 182 | const auto& pp = TestFixture::points_[frame]; 183 | const typename TestFixture::V3& origin = TestFixture::origins_[frame]; 184 | 185 | if (frame % 50 == 0) { 186 | std::cout << "...processing frame #" << frame << " with #" << pp.size() 187 | << " points..." << std::endl; 188 | } 189 | 190 | for (const auto& point : pp) { 191 | const auto ray = TestFixture::R::fromOriginEnd(origin, point); 192 | 193 | const auto intersect = 194 | traverseVoxelGrid(ray, TestFixture::grid_, traversedVoxels, 195 | TypeParam{0.0}, TypeParam{1.0}); 196 | if (intersect) { 197 | ++intersection_count; 198 | } 199 | traversal_count += traversedVoxels.size(); 200 | } 201 | } 202 | auto t2 = high_resolution_clock::now(); 203 | const duration ss = t2 - t1; 204 | 205 | std::cout << "Elapsed seconds: " << ss.count() << std::endl; 206 | std::cout << "Total number of rays: " << TestFixture::total_point_count_ 207 | << std::endl; 208 | std::cout << "Total number of intersections: " << intersection_count 209 | << std::endl; 210 | // originally: 278 063 540 211 | std::cout << "Total number of traversed voxels: " << traversal_count 212 | << std::endl; 213 | } 214 | 215 | TYPED_TEST(TestOnData, timeCounterCalculation) { 216 | using std::chrono::duration; 217 | using std::chrono::duration_cast; 218 | using std::chrono::high_resolution_clock; 219 | 220 | double t_min, t_max; 221 | uint32_t intersection_count{0}; 222 | TestFixture::grid_counter_.clear(); 223 | 224 | auto t1 = high_resolution_clock::now(); 225 | for (std::size_t frame = 0; frame < TestFixture::points_.size(); ++frame) { 226 | const auto& pp = TestFixture::points_[frame]; 227 | const typename TestFixture::V3& origin = TestFixture::origins_[frame]; 228 | 229 | if (frame % 50 == 0) { 230 | std::cout << "...processing frame #" << frame << " with #" << pp.size() 231 | << " points..." << std::endl; 232 | } 233 | 234 | for (const auto& point : pp) { 235 | const auto ray = TestFixture::R::fromOriginEnd(origin, point); 236 | 237 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_counter_, 238 | TypeParam{0.0}, TypeParam{1.0}); 239 | if (intersect) { 240 | ++intersection_count; 241 | } 242 | } 243 | } 244 | auto t2 = high_resolution_clock::now(); 245 | const duration ss = t2 - t1; 246 | 247 | std::cout << "Elapsed seconds: " << ss.count() << std::endl; 248 | std::cout << "Total number of rays: " << TestFixture::total_point_count_ 249 | << std::endl; 250 | std::cout << "Total number of intersections: " << intersection_count 251 | << std::endl; 252 | // originally: 278 063 540 253 | const Eigen::Tensor::counter_type, 254 | 0> 255 | sum_count = TestFixture::grid_counter_.getCounter().sum(); 256 | std::cout << "Total number of traversed voxels: " << sum_count() << std::endl; 257 | } 258 | 259 | TYPED_TEST(TestOnData, sanityCheckTraversalCalculation) { 260 | TraversedVoxels traversedVoxels{}; 261 | traversedVoxels.reserve(1000); 262 | 263 | for (std::size_t frame = 0; frame < TestFixture::points_.size(); ++frame) { 264 | const auto& pp = TestFixture::points_[frame]; 265 | const typename TestFixture::V3& origin = TestFixture::origins_[frame]; 266 | 267 | if (frame % 50 == 0) { 268 | std::cout << "...processing frame #" << frame << " with #" << pp.size() 269 | << " points..." << std::endl; 270 | } 271 | 272 | for (const auto& point : pp) { 273 | const auto ray = TestFixture::R::fromOriginEnd(origin, point); 274 | 275 | const auto intersect = 276 | traverseVoxelGrid(ray, TestFixture::grid_, traversedVoxels, 277 | TypeParam{0.0}, TypeParam{1.0}); 278 | for (const auto& tv : traversedVoxels) { 279 | EXPECT_TRUE((tv >= 0).all()); 280 | EXPECT_TRUE((tv < TestFixture::grid_.numVoxels()).all()); 281 | } 282 | EXPECT_LE(traversedVoxels.size(), TestFixture::grid_.upperBoundVoxelTraversal()); 283 | } 284 | } 285 | } 286 | -------------------------------------------------------------------------------- /tests/test_voxel_counter.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // 3 | #include 4 | 5 | #include 6 | 7 | #include "voxel_traversal.h" 8 | 9 | using namespace voxel_traversal; 10 | 11 | // Small voxel grid 2x2x2 12 | template 13 | class TestVoxelCounter : public ::testing::Test { 14 | protected: 15 | static const constexpr float_type SQRT2 = sqrt(2.0); 16 | static const constexpr float_type HALF_SQRT2 = 0.5 * SQRT2; 17 | static const constexpr float_type SQRT3 = sqrt(3.0); 18 | 19 | void expectCounted(const TraversedVoxels& expected, 20 | bool more_allowed = false) const { 21 | // copy 22 | typename Grid3DTraversalCounter::tensor_type counts = 23 | grid_.getCounter(); 24 | for (const auto& index : expected) { 25 | EXPECT_GE(counts(index), 1); 26 | if (counts(index) > 0) counts(index)--; 27 | } 28 | if (!more_allowed) { 29 | const Eigen::Tensor max_value = counts.maximum(); 30 | EXPECT_EQ(0, max_value()); 31 | } 32 | } 33 | 34 | Grid3DTraversalCounter grid_; 35 | }; 36 | 37 | // Small voxel grid 2x2x2 38 | template 39 | class TestVoxel2x2x2Counter : public TestVoxelCounter { 40 | protected: 41 | using V3 = typename Grid3DTraversalCounter::Vector3d; 42 | using C3 = typename Grid3DTraversalCounter::Index3d; 43 | using R = Ray; 44 | 45 | void SetUp() override { 46 | const V3 bound_min(0.0, 0.0, 0.0); 47 | const V3 bound_max(2.0, 2.0, 2.0); 48 | const C3 voxel_count(2, 2, 2); 49 | this->grid_ = 50 | Grid3DTraversalCounter(bound_min, bound_max, voxel_count); 51 | } 52 | }; 53 | 54 | // Slightly bigger grid 5x5x5 55 | template 56 | class TestVoxel5x5x5Counter : public TestVoxelCounter { 57 | protected: 58 | using V3 = typename Grid3DTraversalCounter::Vector3d; 59 | using C3 = typename Grid3DTraversalCounter::Index3d; 60 | using R = Ray; 61 | 62 | void SetUp() override { 63 | const V3 bound_min(-10.0, -10.0, -10.0); 64 | const V3 bound_max(10.0, 10.0, 10.0); 65 | const C3 voxel_count(5, 5, 5); 66 | this->grid_ = 67 | Grid3DTraversalCounter(bound_min, bound_max, voxel_count); 68 | } 69 | }; 70 | 71 | // cuboid grid 72 | template 73 | class TestVoxel4x2x1Counter : public TestVoxelCounter { 74 | protected: 75 | using V3 = typename Grid3DTraversalCounter::Vector3d; 76 | using C3 = typename Grid3DTraversalCounter::Index3d; 77 | using R = Ray; 78 | 79 | void SetUp() override { 80 | const V3 bound_min(0.0, 0.0, -50.0); 81 | const V3 bound_max(4.0, 2.0, -15.0); 82 | const C3 voxel_count(4, 2, 1); 83 | this->grid_ = 84 | Grid3DTraversalCounter(bound_min, bound_max, voxel_count); 85 | } 86 | }; 87 | 88 | // real-world edge cases 89 | template 90 | class TestVoxelGridCounter : public TestVoxelCounter { 91 | protected: 92 | using V3 = typename Grid3DTraversalCounter::Vector3d; 93 | using C3 = typename Grid3DTraversalCounter::Index3d; 94 | using R = Ray; 95 | 96 | void SetUp() override { 97 | const V3 bound_min(0.0, -25.6, -2.0); 98 | const V3 bound_max(51.2, 25.6, 4.4); 99 | const C3 voxel_count(256, 256, 32); 100 | this->grid_ = 101 | Grid3DTraversalCounter(bound_min, bound_max, voxel_count); 102 | } 103 | }; 104 | 105 | // setup typed test suites 106 | using Implementations = ::testing::Types; 107 | TYPED_TEST_SUITE(TestVoxel2x2x2Counter, Implementations); 108 | TYPED_TEST_SUITE(TestVoxel5x5x5Counter, Implementations); 109 | TYPED_TEST_SUITE(TestVoxel4x2x1Counter, Implementations); 110 | TYPED_TEST_SUITE(TestVoxelGridCounter, Implementations); 111 | 112 | TYPED_TEST(TestVoxel2x2x2Counter, MultipleRays) { 113 | TestFixture::grid_.clear(); 114 | TraversedVoxels expected{ 115 | {0, 0, 0}, {1, 0, 0}, {1, 0, 0}, {1, 1, 0}, {1, 0, 0}, {1, 0, 1}, 116 | {1, 0, 0}, {0, 0, 0}, {1, 1, 0}, {1, 0, 0}, {1, 0, 1}, {1, 0, 0}}; 117 | { 118 | const auto ray = TestFixture::R::fromOriginDir({.5, .5, .5}, {1., 0., 0.}); 119 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_); 120 | EXPECT_TRUE(intersect); 121 | } 122 | { 123 | const auto ray = TestFixture::R::fromOriginDir({1.5, .5, .5}, {0., 1., 0.}); 124 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_); 125 | EXPECT_TRUE(intersect); 126 | } 127 | { 128 | const auto ray = TestFixture::R::fromOriginDir({1.5, .5, .5}, {0., 0., 1.}); 129 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_); 130 | EXPECT_TRUE(intersect); 131 | } 132 | // Negative directions 133 | { 134 | const auto ray = 135 | TestFixture::R::fromOriginDir({1.5, .5, .5}, {-1., 0., 0.}); 136 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_); 137 | EXPECT_TRUE(intersect); 138 | } 139 | { 140 | const auto ray = 141 | TestFixture::R::fromOriginDir({1.5, 1.5, .5}, {0., -1., 0.}); 142 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_); 143 | EXPECT_TRUE(intersect); 144 | } 145 | { 146 | const auto ray = 147 | TestFixture::R::fromOriginDir({1.5, .5, 1.5}, {0., 0., -1.}); 148 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_); 149 | EXPECT_TRUE(intersect); 150 | } 151 | TestFixture::expectCounted(expected); 152 | } 153 | 154 | TYPED_TEST(TestVoxel2x2x2Counter, AllDirectionsWithinGrid) { 155 | { 156 | // should traverse two voxels in X dir. Ray completely within grid 157 | TestFixture::grid_.clear(); 158 | const auto ray = TestFixture::R::fromOriginDir({.5, .5, .5}, {1., 0., 0.}); 159 | TraversedVoxels expected{{0, 0, 0}, {1, 0, 0}}; 160 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_); 161 | EXPECT_TRUE(intersect); 162 | TestFixture::expectCounted(expected); 163 | } 164 | { 165 | // should traverse two voxels in Y dir. Ray completely within grid 166 | TestFixture::grid_.clear(); 167 | const auto ray = TestFixture::R::fromOriginDir({1.5, .5, .5}, {0., 1., 0.}); 168 | TraversedVoxels expected{{1, 0, 0}, {1, 1, 0}}; 169 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_); 170 | EXPECT_TRUE(intersect); 171 | TestFixture::expectCounted(expected); 172 | } 173 | { 174 | // should traverse two voxels in Z dir. Ray completely within grid 175 | TestFixture::grid_.clear(); 176 | const auto ray = TestFixture::R::fromOriginDir({1.5, .5, .5}, {0., 0., 1.}); 177 | TraversedVoxels expected{{1, 0, 0}, {1, 0, 1}}; 178 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_); 179 | EXPECT_TRUE(intersect); 180 | TestFixture::expectCounted(expected); 181 | } 182 | // Negative directions 183 | { 184 | // should traverse two voxels in X dir. Ray completely within grid 185 | TestFixture::grid_.clear(); 186 | const auto ray = 187 | TestFixture::R::fromOriginDir({1.5, .5, .5}, {-1., 0., 0.}); 188 | TraversedVoxels expected{{1, 0, 0}, {0, 0, 0}}; 189 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_); 190 | EXPECT_TRUE(intersect); 191 | TestFixture::expectCounted(expected); 192 | } 193 | { 194 | // should traverse two voxels in Y dir. Ray completely within grid 195 | TestFixture::grid_.clear(); 196 | const auto ray = 197 | TestFixture::R::fromOriginDir({1.5, 1.5, .5}, {0., -1., 0.}); 198 | TraversedVoxels expected{{1, 1, 0}, {1, 0, 0}}; 199 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_); 200 | EXPECT_TRUE(intersect); 201 | TestFixture::expectCounted(expected); 202 | } 203 | { 204 | // should traverse two voxels in Z dir. Ray completely within grid 205 | TestFixture::grid_.clear(); 206 | const auto ray = 207 | TestFixture::R::fromOriginDir({1.5, .5, 1.5}, {0., 0., -1.}); 208 | TraversedVoxels expected{{1, 0, 1}, {1, 0, 0}}; 209 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_); 210 | EXPECT_TRUE(intersect); 211 | TestFixture::expectCounted(expected); 212 | } 213 | } 214 | 215 | TYPED_TEST(TestVoxel2x2x2Counter, SingleVoxel) { 216 | { 217 | // only single voxel, ray too short to reach second 218 | TestFixture::grid_.clear(); 219 | const auto ray = 220 | TestFixture::R::fromOriginDir({1.5, 1.5, 1.5}, {0.4, 0., 0.}); 221 | TraversedVoxels expected{{1, 1, 1}}; 222 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_); 223 | EXPECT_TRUE(intersect); 224 | TestFixture::expectCounted(expected); 225 | } 226 | { 227 | // only single voxel, cut through corner 228 | // -> make sure that there is no infinite loop 229 | TestFixture::grid_.clear(); 230 | const auto ray = 231 | TestFixture::R::fromOriginEnd({-0.45, 0.5, 1.5}, {0.55, -0.5, 1.5}); 232 | TraversedVoxels expected{{0, 0, 1}}; 233 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_); 234 | EXPECT_TRUE(intersect); 235 | TestFixture::expectCounted(expected); 236 | } 237 | { 238 | // only single voxel, cut through corner 239 | // -> make sure that there is no infinite loop 240 | TestFixture::grid_.clear(); 241 | const auto ray = 242 | TestFixture::R::fromOriginEnd({-0.5, 1.5, 0.55}, {0.5, 1.5, -0.45}); 243 | TraversedVoxels expected{{0, 1, 0}}; 244 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_); 245 | EXPECT_TRUE(intersect); 246 | TestFixture::expectCounted(expected); 247 | } 248 | } 249 | 250 | TYPED_TEST(TestVoxel2x2x2Counter, NoVoxel) { 251 | { 252 | // only single voxel, ray too short to reach second 253 | TestFixture::grid_.clear(); 254 | const auto ray = 255 | TestFixture::R::fromOriginDir({1.5, 1.5, 2.1}, {0., 1., 0.}); 256 | TraversedVoxels expected{}; 257 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_); 258 | EXPECT_FALSE(intersect); 259 | TestFixture::expectCounted(expected); 260 | } 261 | } 262 | 263 | TYPED_TEST(TestVoxel5x5x5Counter, Diagonal) { 264 | TypeParam t_min, t_max; 265 | { 266 | // full diagonal. We do not assert specific order of off-diagonal voxels 267 | TestFixture::grid_.clear(); 268 | const auto ray = TestFixture::R::fromOriginDir({-20.0, -20.0, -20.0}, 269 | {40.0, 40.0, 40.0}); 270 | TraversedVoxels expected{ 271 | {0, 0, 0}, {1, 1, 1}, {2, 2, 2}, {3, 3, 3}, {4, 4, 4}}; 272 | // 273 | EXPECT_TRUE(rayBoxIntersection(ray, TestFixture::grid_, t_min, t_max)); 274 | EXPECT_FLOAT_EQ(0.25, t_min); 275 | EXPECT_FLOAT_EQ(0.75, t_max); 276 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_); 277 | EXPECT_TRUE(intersect); 278 | const Eigen::Tensor summed = 279 | TestFixture::grid_.getCounter().sum(); 280 | EXPECT_GE(summed(), 5); 281 | TestFixture::expectCounted(expected, true); 282 | } 283 | } 284 | 285 | TYPED_TEST(TestVoxel4x2x1Counter, StoppingStartingRay) { 286 | { 287 | TestFixture::grid_.clear(); 288 | const auto ray = 289 | TestFixture::R::fromOriginEnd({1.5, 0.8, -25.0}, {6.0, 1.7, -25.0}); 290 | TraversedVoxels expected{ 291 | {1, 0, 0}, {2, 0, 0}, {2, 1, 0}, {3, 1, 0}}; 292 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 293 | TypeParam{0.0}, TypeParam{1.0}); 294 | EXPECT_TRUE(intersect); 295 | TestFixture::expectCounted(expected); 296 | } 297 | { 298 | TestFixture::grid_.clear(); 299 | // other way around 300 | const auto ray = 301 | TestFixture::R::fromOriginEnd({6.0, 1.7, -25.0}, {1.5, 0.8, -25.0}); 302 | TraversedVoxels expected{ 303 | {3, 1, 0}, {2, 1, 0}, {2, 0, 0}, {1, 0, 0}}; 304 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 305 | TypeParam{0.0}, TypeParam{1.0}); 306 | EXPECT_TRUE(intersect); 307 | TestFixture::expectCounted(expected); 308 | } 309 | { 310 | TestFixture::grid_.clear(); 311 | // shorter 312 | const auto ray = 313 | TestFixture::R::fromOriginEnd({6.0, 1.7, -25.0}, {1.5, 0.8, -25.0}); 314 | TraversedVoxels expected{{3, 1, 0}}; 315 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 316 | TypeParam{0.0}, TypeParam{0.5}); 317 | EXPECT_TRUE(intersect); 318 | TestFixture::expectCounted(expected); 319 | } 320 | { 321 | TestFixture::grid_.clear(); 322 | // shorter ray, use t1 323 | const auto ray = 324 | TestFixture::R::fromOriginEnd({1.5, 0.8, -25.0}, {2.5, 1.0, -25.0}); 325 | TraversedVoxels expected{{1, 0, 0}, {2, 0, 0}}; 326 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 327 | TypeParam{0.0}, TypeParam{0.9}); 328 | EXPECT_TRUE(intersect); 329 | TestFixture::expectCounted(expected); 330 | } 331 | { 332 | TestFixture::grid_.clear(); 333 | // shorter ray, use t1 334 | const auto ray = 335 | TestFixture::R::fromOriginEnd({1.5, 0.8, -25.0}, {2.5, 1.0, -25.0}); 336 | TraversedVoxels expected{{1, 0, 0}, {2, 0, 0}, {2, 1, 0}}; 337 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 338 | TypeParam{0.0}, TypeParam{1.25}); 339 | EXPECT_TRUE(intersect); 340 | TestFixture::expectCounted(expected); 341 | } 342 | { 343 | TestFixture::grid_.clear(); 344 | // long ray, use t1 345 | const auto ray = 346 | TestFixture::R::fromOriginEnd({1.5, 0.8, -25.0}, {2.5, 1.0, -25.0}); 347 | TraversedVoxels expected{ 348 | {1, 0, 0}, {2, 0, 0}, {2, 1, 0}, {3, 1, 0}}; 349 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 350 | TypeParam{0.0}, TypeParam{15.0}); 351 | EXPECT_TRUE(intersect); 352 | TestFixture::expectCounted(expected); 353 | } 354 | } 355 | 356 | TYPED_TEST(TestVoxelGridCounter, EdgeCase) { 357 | { 358 | // from actual nuscenes data. Originally caused segfault. 359 | // C 360 | // byte representations of ray origin and end should be: 361 | // origin: 362 | // X = 1.371252775 ^= 0x36 85 af 3f 363 | // Y = 0.005534927361 ^= 0x56 5e b5 3b 364 | // Z = -0.02380313911 ^= 0xcd fe c2 bc 365 | // end: 366 | // X = 20.40990257f ^= 0x7b 47 a3 41 367 | // Y = -32.43192673f ^= 0x4b ba 01 c2 368 | // Z = -0.791713357f ^= 0xba ad 4a bf 369 | 370 | TestFixture::grid_.clear(); 371 | 372 | const auto ray = TestFixture::R::fromOriginEnd( 373 | {1.371252775f, 0.005534927361f, -0.02380313911f}, 374 | {20.40990257f, -32.43192673f, -0.791713357f}); 375 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.origin())); 376 | EXPECT_FALSE(TestFixture::grid_.isWithinGrid(ray.endPoint())); 377 | 378 | TraversedVoxels expected{ 379 | {1, 0, 0}, {2, 0, 0}, {2, 1, 0}, {3, 1, 0}}; 380 | 381 | // count traversed voxels implementation 382 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 383 | TypeParam{0.0}, TypeParam{1.0}); 384 | // different code path for returning traversed voxels instead of counting 385 | TraversedVoxels traversedVoxels{}; 386 | const auto ii = traverseVoxelGrid( 387 | ray, static_cast>(TestFixture::grid_), 388 | traversedVoxels, TypeParam{0.0}, TypeParam{1.0}); 389 | EXPECT_TRUE(intersect); 390 | } 391 | } 392 | -------------------------------------------------------------------------------- /tests/test_voxel_intersect.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // 3 | #include 4 | 5 | #include 6 | 7 | #include "voxel_traversal.h" 8 | 9 | using namespace voxel_traversal; 10 | 11 | // define acceptable eps for t_min, t_max calculation 12 | template 13 | class Epsilon { 14 | public: 15 | static constexpr float_type eps = 0.0; 16 | }; 17 | template <> 18 | class Epsilon { 19 | public: 20 | static constexpr float eps = 5e-5; 21 | }; 22 | template <> 23 | class Epsilon { 24 | public: 25 | static constexpr double eps = 1e-11; 26 | }; 27 | template <> 28 | class Epsilon { 29 | public: 30 | static constexpr double eps = 2e-14; 31 | }; 32 | 33 | // Small voxel grid 2x2x2 34 | template 35 | class TestVoxel2x2x2Intersect : public ::testing::Test { 36 | protected: 37 | using V3 = typename Grid3DSpatialDef::Vector3d; 38 | using C3 = typename Grid3DSpatialDef::Index3d; 39 | using R = Ray; 40 | 41 | static const constexpr float_type SQRT2 = sqrt(2.0); 42 | static const constexpr float_type HALF_SQRT2 = 0.5 * SQRT2; 43 | static const constexpr float_type SQRT3 = sqrt(3.0); 44 | 45 | void SetUp() override { 46 | const V3 bound_min(0.0, 0.0, 0.0); 47 | const V3 bound_max(2.0, 2.0, 2.0); 48 | const C3 voxel_count(2, 2, 2); 49 | grid_ = Grid3DSpatialDef(bound_min, bound_max, voxel_count); 50 | } 51 | 52 | Grid3DSpatialDef grid_; 53 | }; 54 | 55 | using Implementations = ::testing::Types; 56 | TYPED_TEST_SUITE(TestVoxel2x2x2Intersect, Implementations); 57 | 58 | TYPED_TEST(TestVoxel2x2x2Intersect, XYPlaneFixedT) { 59 | TypeParam t_min, t_max; 60 | { 61 | const auto ray = TestFixture::R::fromOriginDir({.5, .5, .5}, {1.0, 0., 0.}); 62 | const auto intersect = 63 | rayBoxIntersection(ray, TestFixture::grid_, t_min, t_max); 64 | EXPECT_TRUE(intersect); 65 | EXPECT_FLOAT_EQ(-0.5, t_min); 66 | EXPECT_FLOAT_EQ(1.5, t_max); 67 | } 68 | { 69 | const auto ray = 70 | TestFixture::R::fromOriginDir(typename TestFixture::V3(-.5, -.5, -.5), 71 | typename TestFixture::V3(1.0, 0., 0.)); 72 | const auto intersect = rayBoxIntersection( 73 | ray, TestFixture::grid_, t_min, t_max, TypeParam{0.0}, TypeParam{1.0}); 74 | EXPECT_FALSE(intersect); 75 | } 76 | { 77 | const auto ray = 78 | TestFixture::R::fromOriginDir(typename TestFixture::V3(.5, -.5, .5), 79 | typename TestFixture::V3(1.0, 0., 0.)); 80 | const auto intersect = rayBoxIntersection( 81 | ray, TestFixture::grid_, t_min, t_max, TypeParam{0.0}, TypeParam{1.0}); 82 | EXPECT_FALSE(intersect); 83 | } 84 | { 85 | // miss corner 86 | const auto ray = 87 | TestFixture::R::fromOriginDir(typename TestFixture::V3(.5, -.55, 1.0), 88 | typename TestFixture::V3(-1.0, 1.0, 0.)); 89 | const auto intersect = rayBoxIntersection( 90 | ray, TestFixture::grid_, t_min, t_max, TypeParam{0.0}, TypeParam{1.0}); 91 | EXPECT_FALSE(intersect); 92 | } 93 | { 94 | // Non-unit direction vector 95 | const auto ray = 96 | TestFixture::R::fromOriginDir(typename TestFixture::V3(.5, -.4, 1.0), 97 | typename TestFixture::V3(-1.0, 1.0, 0.)); 98 | const auto intersect = rayBoxIntersection( 99 | ray, TestFixture::grid_, t_min, t_max, TypeParam{0.0}, TypeParam{1.0}); 100 | EXPECT_TRUE(intersect); 101 | } 102 | { 103 | // Unit direction vector 104 | const auto ray = TestFixture::R::fromOriginDir( 105 | {1.0, 1.0, 1.0}, 106 | {-TestFixture::HALF_SQRT2, -TestFixture::HALF_SQRT2, 0.}); 107 | const auto intersect = rayBoxIntersection( 108 | ray, TestFixture::grid_, t_min, t_max, TypeParam{0.0}, TypeParam{1.0}); 109 | EXPECT_TRUE(intersect); 110 | EXPECT_FLOAT_EQ(-TestFixture::SQRT2, t_min); 111 | EXPECT_FLOAT_EQ(TestFixture::SQRT2, t_max); 112 | } 113 | { 114 | // Non-unit direction vector 115 | const auto ray = 116 | TestFixture::R::fromOriginDir({1.0, 1.0, 1.0}, {-1.0, -1.0, 0.}); 117 | const auto intersect = 118 | rayBoxIntersection(ray, TestFixture::grid_, t_min, t_max); 119 | EXPECT_TRUE(intersect); 120 | EXPECT_FLOAT_EQ(-1.0, t_min); 121 | EXPECT_FLOAT_EQ(1.0, t_max); 122 | } 123 | { 124 | // positive direction 125 | const auto ray = 126 | TestFixture::R::fromOriginDir({1.0, 1.0, 1.0}, {1.0, 1.0, 0.}); 127 | const auto intersect = rayBoxIntersection( 128 | ray, TestFixture::grid_, t_min, t_max, TypeParam{0.0}, TypeParam{1.0}); 129 | EXPECT_TRUE(intersect); 130 | EXPECT_FLOAT_EQ(-1.0, t_min); 131 | EXPECT_FLOAT_EQ(1.0, t_max); 132 | } 133 | { 134 | // too short, stops before grid 135 | const auto ray = 136 | TestFixture::R::fromOriginDir({-1.0, 3.0, 1.0}, {1.1, -0.9, 0.}); 137 | const auto intersect = rayBoxIntersection( 138 | ray, TestFixture::grid_, t_min, t_max, TypeParam{0.0}, TypeParam{1.0}); 139 | EXPECT_FALSE(intersect); 140 | EXPECT_GE(t_min, 1.0); 141 | EXPECT_GE(t_max, 1.0); 142 | } 143 | { 144 | // long enough, reaches into grid and ends within 145 | const auto ray = 146 | TestFixture::R::fromOriginDir({-1.0, 3.0, 1.0}, {1.1, -1.1, 0.}); 147 | const auto intersect = rayBoxIntersection( 148 | ray, TestFixture::grid_, t_min, t_max, TypeParam{0.0}, TypeParam{1.0}); 149 | EXPECT_TRUE(intersect); 150 | EXPECT_LE(t_min, 1.0); 151 | EXPECT_GE(t_max, 1.0); 152 | } 153 | { 154 | // parallel to grid, does never intersect 155 | const auto ray = 156 | TestFixture::R::fromOriginEnd({-0.5, 0.5, 0.5}, {-0.5, 1.5, 0.5}); 157 | const auto intersect = rayBoxIntersection( 158 | ray, TestFixture::grid_, t_min, t_max, TypeParam{0.0}, TypeParam{1.0}); 159 | EXPECT_FALSE(intersect); 160 | EXPECT_FLOAT_EQ(std::numeric_limits::infinity(), 161 | std::fabs(t_min)); 162 | EXPECT_FLOAT_EQ(std::numeric_limits::infinity(), 163 | std::fabs(t_max)); 164 | } 165 | { 166 | // parallel to grid, does intersect 167 | const auto ray = 168 | TestFixture::R::fromOriginEnd({-0.5, 0.5, 0.5}, {0.5, 0.5, 0.5}); 169 | const auto intersect = rayBoxIntersection( 170 | ray, TestFixture::grid_, t_min, t_max, TypeParam{0.0}, TypeParam{1.0}); 171 | EXPECT_TRUE(intersect); 172 | EXPECT_FLOAT_EQ(0.5, t_min); 173 | EXPECT_FLOAT_EQ(2.5, t_max); 174 | } 175 | } 176 | 177 | TYPED_TEST(TestVoxel2x2x2Intersect, RayOutsideXYPlane) { 178 | TypeParam t_min, t_max; 179 | { 180 | // 181 | const auto ray = 182 | TestFixture::R::fromOriginEnd({-0.5, -0.5, -0.5}, {2.0, 2.0, 2.0}); 183 | const auto intersect = 184 | rayBoxIntersection(ray, TestFixture::grid_, t_min, t_max); 185 | EXPECT_TRUE(intersect); 186 | EXPECT_GE(t_min, TypeParam{0.1}); 187 | EXPECT_NEAR(t_max, TypeParam{1.0}, Epsilon::eps); 188 | } 189 | { 190 | // outside of grid 191 | const auto ray = 192 | TestFixture::R::fromOriginEnd({2.1, 2.1, 2.1}, {2.2, 2.2, 2.2}); 193 | const auto intersect = 194 | rayBoxIntersection(ray, TestFixture::grid_, t_min, t_max); 195 | EXPECT_FALSE(intersect); 196 | 197 | EXPECT_NEAR(t_min, TypeParam{-21.0}, Epsilon::eps); 198 | EXPECT_NEAR(t_max, TypeParam{-1.0}, Epsilon::eps); 199 | } 200 | } 201 | 202 | TYPED_TEST(TestVoxel2x2x2Intersect, DegenerateCase) { 203 | TypeParam t_min, t_max; 204 | { 205 | // zero direction within grid -> does intersect with single point 206 | const auto ray = 207 | TestFixture::R::fromOriginDir({.5, .5, .5}, TestFixture::V3::Zero()); 208 | const auto intersect = 209 | rayBoxIntersection(ray, TestFixture::grid_, t_min, t_max); 210 | EXPECT_TRUE(intersect); 211 | EXPECT_FLOAT_EQ(std::numeric_limits::infinity(), 212 | std::fabs(t_min)); 213 | EXPECT_FLOAT_EQ(std::numeric_limits::infinity(), 214 | std::fabs(t_max)); 215 | } 216 | { 217 | // zero direction within grid -> does intersect with single point 218 | // different position near corner 219 | const auto ray = TestFixture::R::fromOriginDir({1.99, 1.99, 0.01}, 220 | TestFixture::V3::Zero()); 221 | const auto intersect = 222 | rayBoxIntersection(ray, TestFixture::grid_, t_min, t_max); 223 | EXPECT_TRUE(intersect); 224 | EXPECT_FLOAT_EQ(std::numeric_limits::infinity(), 225 | std::fabs(t_min)); 226 | EXPECT_FLOAT_EQ(std::numeric_limits::infinity(), 227 | std::fabs(t_max)); 228 | } 229 | { 230 | // zero direction outside of grid -> does not intersect 231 | const auto ray = 232 | TestFixture::R::fromOriginDir({.5, .5, -0.1}, TestFixture::V3::Zero()); 233 | const auto intersect = 234 | rayBoxIntersection(ray, TestFixture::grid_, t_min, t_max); 235 | EXPECT_FALSE(intersect); 236 | EXPECT_FLOAT_EQ(std::numeric_limits::infinity(), 237 | std::fabs(t_min)); 238 | EXPECT_FLOAT_EQ(std::numeric_limits::infinity(), 239 | std::fabs(t_max)); 240 | } 241 | { 242 | // zero direction outside of grid -> does not intersect 243 | // different position 244 | const auto ray = 245 | TestFixture::R::fromOriginDir({.5, -0.01, .5}, TestFixture::V3::Zero()); 246 | const auto intersect = 247 | rayBoxIntersection(ray, TestFixture::grid_, t_min, t_max); 248 | EXPECT_FALSE(intersect); 249 | EXPECT_FLOAT_EQ(std::numeric_limits::infinity(), 250 | std::fabs(t_min)); 251 | EXPECT_FLOAT_EQ(std::numeric_limits::infinity(), 252 | std::fabs(t_max)); 253 | } 254 | } 255 | -------------------------------------------------------------------------------- /tests/test_voxel_traversal.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // 3 | #include 4 | 5 | #include 6 | 7 | #include "voxel_traversal.h" 8 | 9 | using namespace voxel_traversal; 10 | 11 | // Small voxel grid 2x2x2 12 | template 13 | class TestVoxelTraversal : public ::testing::Test { 14 | protected: 15 | static const constexpr float_type SQRT2 = sqrt(2.0); 16 | static const constexpr float_type HALF_SQRT2 = 0.5 * SQRT2; 17 | static const constexpr float_type SQRT3 = sqrt(3.0); 18 | 19 | void expectTraversed(const TraversedVoxels& expected, 20 | const TraversedVoxels& actual) { 21 | EXPECT_EQ(expected.size(), actual.size()); 22 | EXPECT_TRUE(std::equal( 23 | expected.cbegin(), expected.cend(), actual.cbegin(), 24 | [](const auto& a, const auto& b) { return (a == b).all(); })); 25 | // check if all expected voxels are actually considered to be within grid 26 | EXPECT_TRUE(std::all_of( 27 | expected.cbegin(), expected.cend(), 28 | [this](const auto& index) { return grid_.isWithinGrid(index); })); 29 | } 30 | void expectTraversedInOrderWithGaps( 31 | const TraversedVoxels& expected, 32 | const TraversedVoxels& actual) { 33 | auto it_traversed = actual.cbegin(); 34 | 35 | for (const auto& exp : expected) { 36 | const auto fp = 37 | std::find_if(it_traversed, actual.cend(), 38 | [&exp](const auto& a) { return (a == exp).all(); }); 39 | if (fp == actual.cend()) { 40 | ADD_FAILURE(); 41 | break; 42 | } 43 | it_traversed = fp; 44 | } 45 | } 46 | 47 | Grid3DSpatialDef grid_; 48 | TraversedVoxels traversed_voxels_; 49 | }; 50 | 51 | // Small voxel grid 2x2x2 52 | template 53 | class TestVoxel2x2x2Traversal : public TestVoxelTraversal { 54 | protected: 55 | using V3 = typename Grid3DSpatialDef::Vector3d; 56 | using C3 = typename Grid3DSpatialDef::Index3d; 57 | using R = Ray; 58 | 59 | void SetUp() override { 60 | const V3 bound_min(0.0, 0.0, 0.0); 61 | const V3 bound_max(2.0, 2.0, 2.0); 62 | const C3 voxel_count(2, 2, 2); 63 | this->grid_ = 64 | Grid3DSpatialDef(bound_min, bound_max, voxel_count); 65 | this->traversed_voxels_.reserve(1000); 66 | } 67 | }; 68 | 69 | // Slightly bigger grid 5x5x5 70 | template 71 | class TestVoxel5x5x5Traversal : public TestVoxelTraversal { 72 | protected: 73 | using V3 = typename Grid3DSpatialDef::Vector3d; 74 | using C3 = typename Grid3DSpatialDef::Index3d; 75 | using R = Ray; 76 | 77 | void SetUp() override { 78 | const V3 bound_min(-10.0, -10.0, -10.0); 79 | const V3 bound_max(10.0, 10.0, 10.0); 80 | const C3 voxel_count(5, 5, 5); 81 | this->grid_ = 82 | Grid3DSpatialDef(bound_min, bound_max, voxel_count); 83 | this->traversed_voxels_.reserve(1000); 84 | } 85 | }; 86 | 87 | // cuboid grid 88 | template 89 | class TestVoxel4x2x1Traversal : public TestVoxelTraversal { 90 | protected: 91 | using V3 = typename Grid3DSpatialDef::Vector3d; 92 | using C3 = typename Grid3DSpatialDef::Index3d; 93 | using R = Ray; 94 | 95 | void SetUp() override { 96 | const V3 bound_min(0.0, 0.0, -50.0); 97 | const V3 bound_max(4.0, 2.0, -15.0); 98 | const C3 voxel_count(4, 2, 1); 99 | this->grid_ = 100 | Grid3DSpatialDef(bound_min, bound_max, voxel_count); 101 | this->traversed_voxels_.reserve(1000); 102 | } 103 | }; 104 | 105 | // setup typed test suites 106 | using Implementations = ::testing::Types; 107 | TYPED_TEST_SUITE(TestVoxel2x2x2Traversal, Implementations); 108 | TYPED_TEST_SUITE(TestVoxel5x5x5Traversal, Implementations); 109 | TYPED_TEST_SUITE(TestVoxel4x2x1Traversal, Implementations); 110 | 111 | TYPED_TEST(TestVoxel2x2x2Traversal, GridProperties) { 112 | EXPECT_EQ(8ul, TestFixture::grid_.voxelCount()); 113 | const Grid3DSpatialDef grid_copy = TestFixture::grid_; 114 | EXPECT_EQ(8ul, grid_copy.voxelCount()); 115 | EXPECT_TRUE((TestFixture::grid_.gridSize() == grid_copy.gridSize()).all()); 116 | EXPECT_TRUE((TestFixture::grid_.voxelSize() == grid_copy.voxelSize()).all()); 117 | EXPECT_TRUE((TestFixture::grid_.numVoxels() == grid_copy.numVoxels()).all()); 118 | EXPECT_EQ(TestFixture::grid_.minBound(), grid_copy.minBound()); 119 | EXPECT_EQ(TestFixture::grid_.maxBound(), grid_copy.maxBound()); 120 | EXPECT_TRUE((TestFixture::grid_.gridSize() == grid_copy.gridSize()).all()); 121 | } 122 | 123 | TYPED_TEST(TestVoxel2x2x2Traversal, AllDirectionsWithinGrid) { 124 | { 125 | // should traverse two voxels in X dir. Ray completely within grid 126 | const auto ray = TestFixture::R::fromOriginDir({.5, .5, .5}, {1., 0., 0.}); 127 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.origin())); 128 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.endPoint())); 129 | TraversedVoxels expected{{0, 0, 0}, {1, 0, 0}}; 130 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 131 | TestFixture::traversed_voxels_); 132 | EXPECT_TRUE(intersect); 133 | TestFixture::expectTraversed(expected, TestFixture::traversed_voxels_); 134 | } 135 | { 136 | // should traverse two voxels in Y dir. Ray completely within grid 137 | const auto ray = TestFixture::R::fromOriginDir({1.5, .5, .5}, {0., 1., 0.}); 138 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.origin())); 139 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.endPoint())); 140 | TraversedVoxels expected{{1, 0, 0}, {1, 1, 0}}; 141 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 142 | TestFixture::traversed_voxels_); 143 | EXPECT_TRUE(intersect); 144 | TestFixture::expectTraversed(expected, TestFixture::traversed_voxels_); 145 | } 146 | { 147 | // should traverse two voxels in Z dir. Ray completely within grid 148 | const auto ray = TestFixture::R::fromOriginDir({1.5, .5, .5}, {0., 0., 1.}); 149 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.origin())); 150 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.endPoint())); 151 | TraversedVoxels expected{{1, 0, 0}, {1, 0, 1}}; 152 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 153 | TestFixture::traversed_voxels_); 154 | EXPECT_TRUE(intersect); 155 | TestFixture::expectTraversed(expected, TestFixture::traversed_voxels_); 156 | } 157 | // Negative directions 158 | { 159 | // should traverse two voxels in X dir. Ray completely within grid 160 | const auto ray = 161 | TestFixture::R::fromOriginDir({1.5, .5, .5}, {-1., 0., 0.}); 162 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.origin())); 163 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.endPoint())); 164 | TraversedVoxels expected{{1, 0, 0}, {0, 0, 0}}; 165 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 166 | TestFixture::traversed_voxels_); 167 | EXPECT_TRUE(intersect); 168 | TestFixture::expectTraversed(expected, TestFixture::traversed_voxels_); 169 | } 170 | { 171 | // should traverse two voxels in Y dir. Ray completely within grid 172 | const auto ray = 173 | TestFixture::R::fromOriginDir({1.5, 1.5, .5}, {0., -1., 0.}); 174 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.origin())); 175 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.endPoint())); 176 | TraversedVoxels expected{{1, 1, 0}, {1, 0, 0}}; 177 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 178 | TestFixture::traversed_voxels_); 179 | EXPECT_TRUE(intersect); 180 | TestFixture::expectTraversed(expected, TestFixture::traversed_voxels_); 181 | } 182 | { 183 | // should traverse two voxels in Z dir. Ray completely within grid 184 | const auto ray = 185 | TestFixture::R::fromOriginDir({1.5, .5, 1.5}, {0., 0., -1.}); 186 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.origin())); 187 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.endPoint())); 188 | TraversedVoxels expected{{1, 0, 1}, {1, 0, 0}}; 189 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 190 | TestFixture::traversed_voxels_); 191 | EXPECT_TRUE(intersect); 192 | TestFixture::expectTraversed(expected, TestFixture::traversed_voxels_); 193 | } 194 | } 195 | 196 | TYPED_TEST(TestVoxel2x2x2Traversal, SingleVoxel) { 197 | { 198 | // only single voxel, ray too short to reach second 199 | const auto ray = 200 | TestFixture::R::fromOriginDir({1.5, 1.5, 1.5}, {0.4, 0., 0.}); 201 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.origin())); 202 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.endPoint())); 203 | TraversedVoxels expected{{1, 1, 1}}; 204 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 205 | TestFixture::traversed_voxels_); 206 | EXPECT_TRUE(intersect); 207 | TestFixture::expectTraversed(expected, TestFixture::traversed_voxels_); 208 | } 209 | { 210 | // only single voxel, cut through corner 211 | // -> make sure that there is no infinite loop 212 | const auto ray = 213 | TestFixture::R::fromOriginEnd({-0.45, 0.5, 1.5}, {0.55, -0.5, 1.5}); 214 | EXPECT_FALSE(TestFixture::grid_.isWithinGrid(ray.origin())); 215 | EXPECT_FALSE(TestFixture::grid_.isWithinGrid(ray.endPoint())); 216 | TraversedVoxels expected{{0, 0, 1}}; 217 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 218 | TestFixture::traversed_voxels_); 219 | EXPECT_TRUE(intersect); 220 | TestFixture::expectTraversed(expected, TestFixture::traversed_voxels_); 221 | } 222 | { 223 | // only single voxel, cut through corner 224 | // -> make sure that there is no infinite loop 225 | const auto ray = 226 | TestFixture::R::fromOriginEnd({-0.5, 1.5, 0.55}, {0.5, 1.5, -0.45}); 227 | EXPECT_FALSE(TestFixture::grid_.isWithinGrid(ray.origin())); 228 | EXPECT_FALSE(TestFixture::grid_.isWithinGrid(ray.endPoint())); 229 | TraversedVoxels expected{{0, 1, 0}}; 230 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 231 | TestFixture::traversed_voxels_); 232 | EXPECT_TRUE(intersect); 233 | TestFixture::expectTraversed(expected, TestFixture::traversed_voxels_); 234 | } 235 | } 236 | 237 | TYPED_TEST(TestVoxel2x2x2Traversal, NoVoxel) { 238 | { 239 | // only single voxel, ray parallel and outside of grid 240 | const auto ray = 241 | TestFixture::R::fromOriginDir({1.5, 1.5, 2.1}, {0., 1., 0.}); 242 | EXPECT_FALSE(TestFixture::grid_.isWithinGrid(ray.origin())); 243 | EXPECT_FALSE(TestFixture::grid_.isWithinGrid(ray.endPoint())); 244 | TraversedVoxels expected{}; 245 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 246 | TestFixture::traversed_voxels_); 247 | EXPECT_FALSE(intersect); 248 | TestFixture::expectTraversed(expected, TestFixture::traversed_voxels_); 249 | } 250 | } 251 | 252 | TYPED_TEST(TestVoxel5x5x5Traversal, GridProperties) { 253 | EXPECT_EQ(125ul, TestFixture::grid_.voxelCount()); 254 | const Grid3DSpatialDef grid_copy = TestFixture::grid_; 255 | EXPECT_EQ(125ul, grid_copy.voxelCount()); 256 | EXPECT_TRUE((TestFixture::grid_.gridSize() == grid_copy.gridSize()).all()); 257 | EXPECT_TRUE((TestFixture::grid_.voxelSize() == grid_copy.voxelSize()).all()); 258 | EXPECT_TRUE((TestFixture::grid_.numVoxels() == grid_copy.numVoxels()).all()); 259 | EXPECT_EQ(TestFixture::grid_.minBound(), grid_copy.minBound()); 260 | EXPECT_EQ(TestFixture::grid_.maxBound(), grid_copy.maxBound()); 261 | EXPECT_TRUE((TestFixture::grid_.gridSize() == grid_copy.gridSize()).all()); 262 | } 263 | 264 | TYPED_TEST(TestVoxel5x5x5Traversal, Diagonal) { 265 | TypeParam t_min, t_max; 266 | { 267 | // full diagonal. We do not assert specific order of off-diagonal voxels 268 | const auto ray = TestFixture::R::fromOriginDir({-20.0, -20.0, -20.0}, 269 | {40.0, 40.0, 40.0}); 270 | EXPECT_FALSE(TestFixture::grid_.isWithinGrid(ray.origin())); 271 | EXPECT_FALSE(TestFixture::grid_.isWithinGrid(ray.endPoint())); 272 | TraversedVoxels expected{ 273 | {0, 0, 0}, {1, 1, 1}, {2, 2, 2}, {3, 3, 3}, {4, 4, 4}}; 274 | // 275 | EXPECT_TRUE(rayBoxIntersection(ray, TestFixture::grid_, t_min, t_max)); 276 | EXPECT_FLOAT_EQ(0.25, t_min); 277 | EXPECT_FLOAT_EQ(0.75, t_max); 278 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 279 | TestFixture::traversed_voxels_); 280 | EXPECT_TRUE(intersect); 281 | EXPECT_GE(this->traversed_voxels_.size(), 5); 282 | TestFixture::expectTraversedInOrderWithGaps(expected, 283 | TestFixture::traversed_voxels_); 284 | EXPECT_TRUE((this->traversed_voxels_.front() == expected.front()).all()); 285 | EXPECT_TRUE((this->traversed_voxels_.back() == expected.back()).all()); 286 | } 287 | } 288 | 289 | TYPED_TEST(TestVoxel4x2x1Traversal, GridProperties) { 290 | EXPECT_EQ(8ul, TestFixture::grid_.voxelCount()); 291 | const Grid3DSpatialDef grid_copy = TestFixture::grid_; 292 | EXPECT_EQ(8ul, grid_copy.voxelCount()); 293 | EXPECT_TRUE((TestFixture::grid_.gridSize() == grid_copy.gridSize()).all()); 294 | EXPECT_TRUE((TestFixture::grid_.voxelSize() == grid_copy.voxelSize()).all()); 295 | EXPECT_TRUE((TestFixture::grid_.numVoxels() == grid_copy.numVoxels()).all()); 296 | EXPECT_EQ(TestFixture::grid_.minBound(), grid_copy.minBound()); 297 | EXPECT_EQ(TestFixture::grid_.maxBound(), grid_copy.maxBound()); 298 | EXPECT_TRUE((TestFixture::grid_.gridSize() == grid_copy.gridSize()).all()); 299 | } 300 | 301 | TYPED_TEST(TestVoxel4x2x1Traversal, StoppingStartingRay) { 302 | { 303 | const auto ray = 304 | TestFixture::R::fromOriginEnd({1.5, 0.8, -25.0}, {6.0, 1.7, -25.0}); 305 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.origin())); 306 | EXPECT_FALSE(TestFixture::grid_.isWithinGrid(ray.endPoint())); 307 | TraversedVoxels expected{ 308 | {1, 0, 0}, {2, 0, 0}, {2, 1, 0}, {3, 1, 0}}; 309 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 310 | TestFixture::traversed_voxels_, 311 | TypeParam{0.0}, TypeParam{1.0}); 312 | EXPECT_TRUE(intersect); 313 | TestFixture::expectTraversed(expected, TestFixture::traversed_voxels_); 314 | } 315 | { 316 | // other way around 317 | const auto ray = 318 | TestFixture::R::fromOriginEnd({6.0, 1.7, -25.0}, {1.5, 0.8, -25.0}); 319 | EXPECT_FALSE(TestFixture::grid_.isWithinGrid(ray.origin())); 320 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.endPoint())); 321 | TraversedVoxels expected{ 322 | {3, 1, 0}, {2, 1, 0}, {2, 0, 0}, {1, 0, 0}}; 323 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 324 | TestFixture::traversed_voxels_, 325 | TypeParam{0.0}, TypeParam{1.0}); 326 | EXPECT_TRUE(intersect); 327 | TestFixture::expectTraversed(expected, TestFixture::traversed_voxels_); 328 | } 329 | { 330 | // shorter 331 | const auto ray = 332 | TestFixture::R::fromOriginEnd({6.0, 1.7, -25.0}, {1.5, 0.8, -25.0}); 333 | TraversedVoxels expected{{3, 1, 0}}; 334 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 335 | TestFixture::traversed_voxels_, 336 | TypeParam{0.0}, TypeParam{0.5}); 337 | EXPECT_TRUE(intersect); 338 | TestFixture::expectTraversed(expected, TestFixture::traversed_voxels_); 339 | } 340 | { 341 | // shorter ray, use t1 342 | const auto ray = 343 | TestFixture::R::fromOriginEnd({1.5, 0.8, -25.0}, {2.5, 1.0, -25.0}); 344 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.origin())); 345 | EXPECT_TRUE(TestFixture::grid_.isWithinGrid(ray.endPoint())); 346 | TraversedVoxels expected{{1, 0, 0}, {2, 0, 0}}; 347 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 348 | TestFixture::traversed_voxels_, 349 | TypeParam{0.0}, TypeParam{0.9}); 350 | EXPECT_TRUE(intersect); 351 | TestFixture::expectTraversed(expected, TestFixture::traversed_voxels_); 352 | } 353 | { 354 | // shorter ray, use t1 355 | const auto ray = 356 | TestFixture::R::fromOriginEnd({1.5, 0.8, -25.0}, {2.5, 1.0, -25.0}); 357 | TraversedVoxels expected{{1, 0, 0}, {2, 0, 0}, {2, 1, 0}}; 358 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 359 | TestFixture::traversed_voxels_, 360 | TypeParam{0.0}, TypeParam{1.25}); 361 | EXPECT_TRUE(intersect); 362 | TestFixture::expectTraversed(expected, TestFixture::traversed_voxels_); 363 | } 364 | { 365 | // long ray, use t1 366 | const auto ray = 367 | TestFixture::R::fromOriginEnd({1.5, 0.8, -25.0}, {2.5, 1.0, -25.0}); 368 | TraversedVoxels expected{ 369 | {1, 0, 0}, {2, 0, 0}, {2, 1, 0}, {3, 1, 0}}; 370 | const auto intersect = traverseVoxelGrid(ray, TestFixture::grid_, 371 | TestFixture::traversed_voxels_, 372 | TypeParam{0.0}, TypeParam{15.0}); 373 | EXPECT_TRUE(intersect); 374 | TestFixture::expectTraversed(expected, TestFixture::traversed_voxels_); 375 | } 376 | } 377 | -------------------------------------------------------------------------------- /voxel_traversal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/risteon/voxel-traversal/b63d16e14f12474e5881ab70c548412db6d2ad38/voxel_traversal.png --------------------------------------------------------------------------------