├── .clang-format ├── .github └── workflows │ └── continuous.yml ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── TightInclusionOptions.cmake.sample ├── app ├── CMakeLists.txt ├── config.hpp.in └── main.cpp ├── cmake ├── find │ └── FindAVX.cmake ├── recipes │ ├── CPM.cmake │ ├── ccd_query_io.cmake │ ├── cli11.cmake │ ├── eigen.cmake │ ├── pbar.cmake │ ├── rational_cpp.cmake │ └── spdlog.cmake └── tight_inclusion │ ├── tight_inclusion_cpm_cache.cmake │ ├── tight_inclusion_filter_flags.cmake │ ├── tight_inclusion_use_colors.cmake │ └── tight_inclusion_warnings.cmake └── src └── tight_inclusion ├── CMakeLists.txt ├── avx.cpp ├── avx.hpp ├── ccd.cpp ├── ccd.hpp ├── config.hpp.in ├── interval.cpp ├── interval.hpp ├── interval_root_finder.cpp ├── interval_root_finder.hpp ├── logger.cpp ├── logger.hpp ├── rational ├── CMakeLists.txt ├── ccd.cpp ├── ccd.hpp ├── interval_root_finder.cpp └── interval_root_finder.hpp ├── timer.hpp └── types.hpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | # https://clang.llvm.org/docs/ClangFormatStyleOptions.html 3 | Language: Cpp 4 | ColumnLimit: 80 5 | UseTab: Never 6 | IndentWidth: 4 7 | TabWidth: 4 8 | BreakBeforeBraces: WebKit 9 | AllowShortIfStatementsOnASingleLine: false 10 | IndentCaseLabels: false 11 | SortIncludes: false 12 | BreakStringLiterals: false 13 | ReflowComments: false 14 | AlignTrailingComments: true 15 | FixNamespaceComments: true 16 | ContinuationIndentWidth: 4 17 | NamespaceIndentation: All 18 | AccessModifierOffset: -4 19 | BreakBeforeBinaryOperators: NonAssignment 20 | BinPackParameters: false 21 | AllowAllParametersOfDeclarationOnNextLine: true 22 | AlignAfterOpenBracket: AlwaysBreak 23 | ... 24 | -------------------------------------------------------------------------------- /.github/workflows/continuous.yml: -------------------------------------------------------------------------------- 1 | name: Build 2 | 3 | on: 4 | push: 5 | branches: [main] 6 | pull_request: 7 | 8 | env: 9 | CTEST_OUTPUT_ON_FAILURE: ON 10 | CTEST_PARALLEL_LEVEL: 2 11 | 12 | jobs: 13 | #################### 14 | # Linux / macOS 15 | #################### 16 | 17 | Unix: 18 | name: ${{ matrix.name }} (${{ matrix.config }}) 19 | runs-on: ${{ matrix.os }} 20 | strategy: 21 | fail-fast: false 22 | matrix: 23 | os: [ubuntu-latest, macos-latest] 24 | config: [Debug, Release] 25 | include: 26 | - os: macos-latest 27 | name: macOS 28 | - os: ubuntu-latest 29 | name: Linux 30 | steps: 31 | - name: Checkout repository 32 | uses: actions/checkout@v4 33 | with: 34 | fetch-depth: 10 35 | 36 | - name: Dependencies (Linux) 37 | if: runner.os == 'Linux' 38 | run: | 39 | sudo apt-get update 40 | sudo apt-get -o Acquire::Retries=3 install ccache libgmp-dev 41 | echo 'CACHE_PATH=~/.cache/ccache' >> "$GITHUB_ENV" 42 | 43 | - name: Dependencies (macOS) 44 | if: runner.os == 'macOS' 45 | run: | 46 | brew install ccache gmp 47 | echo 'CACHE_PATH=~/Library/Caches/ccache' >> "$GITHUB_ENV" 48 | 49 | - name: Cache Build 50 | id: cache-build 51 | uses: actions/cache@v4 52 | with: 53 | path: ${{ env.CACHE_PATH }} 54 | key: ${{ runner.os }}-${{ matrix.config }}-cache 55 | 56 | - name: Prepare ccache 57 | run: | 58 | ccache --max-size=1.0G 59 | ccache -V && ccache --show-stats && ccache --zero-stats 60 | 61 | - name: Configure (Debug) 62 | if: matrix.config == 'Debug' 63 | run: | 64 | mkdir -p build 65 | cd build 66 | cmake .. \ 67 | -DCMAKE_CXX_COMPILER_LAUNCHER=ccache \ 68 | -DCMAKE_BUILD_TYPE=${{ matrix.config }} 69 | 70 | - name: Configure (Release) 71 | if: matrix.config == 'Release' 72 | run: | 73 | mkdir -p build 74 | cd build 75 | cmake .. \ 76 | -DCMAKE_CXX_COMPILER_LAUNCHER=ccache \ 77 | -DCMAKE_BUILD_TYPE=${{ matrix.config }} \ 78 | -DTIGHT_INCLUSION_WITH_GMP=ON \ 79 | -DTIGHT_INCLUSION_WITH_SAMPLE_QUERIES=ON 80 | 81 | - name: Build 82 | run: cd build; make -j2; ccache --show-stats 83 | 84 | - name: Tests 85 | run: cd build; ./app/Tight_Inclusion_bin --no-pbar 86 | 87 | #################### 88 | # Windows 89 | #################### 90 | 91 | Windows: 92 | runs-on: windows-latest 93 | strategy: 94 | fail-fast: false 95 | matrix: 96 | config: [Debug, Release] 97 | steps: 98 | - name: Checkout repository 99 | uses: actions/checkout@v4 100 | with: 101 | fetch-depth: 10 102 | 103 | - name: Install Ninja 104 | uses: seanmiddleditch/gha-setup-ninja@master 105 | 106 | - name: Set env 107 | run: | 108 | echo "appdata=$env:LOCALAPPDATA" >> ${env:GITHUB_ENV} 109 | 110 | - name: Cache build 111 | id: cache-build 112 | uses: actions/cache@v4 113 | with: 114 | path: ${{ env.appdata }}\Mozilla\sccache 115 | key: ${{ runner.os }}-${{ matrix.config }}-cache-${{ github.sha }} 116 | restore-keys: ${{ runner.os }}-${{ matrix.config }}-cache 117 | 118 | - name: Prepare sccache 119 | run: | 120 | iwr -useb 'https://raw.githubusercontent.com/scoopinstaller/install/master/install.ps1' -outfile 'install.ps1' 121 | .\install.ps1 -RunAsAdmin 122 | scoop install sccache --global 123 | # Scoop modifies the PATH so we make it available for the next steps of the job 124 | echo "${env:PATH}" >> ${env:GITHUB_PATH} 125 | 126 | - name: Configure and build 127 | shell: cmd 128 | run: | 129 | call "C:\Program Files\Microsoft Visual Studio\2022\Enterprise\Common7\Tools\VsDevCmd.bat" -arch=x64 130 | cmake -G Ninja ^ 131 | -DCMAKE_CXX_COMPILER_LAUNCHER=sccache ^ 132 | -DCMAKE_BUILD_TYPE=${{ matrix.config }} ^ 133 | -B build ^ 134 | -S . 135 | cmake --build build -j1 136 | 137 | - name: Tests 138 | run: | 139 | cd build 140 | ./app/Tight_Inclusion_bin --no-pbar 141 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | ## macOS 2 | default.profraw 3 | 4 | # General 5 | .DS_Store 6 | .AppleDouble 7 | .LSOverride 8 | 9 | # Icon must end with two \r 10 | Icon 11 | 12 | 13 | # Thumbnails 14 | ._* 15 | 16 | # Files that might appear in the root of a volume 17 | .DocumentRevisions-V100 18 | .fseventsd 19 | .Spotlight-V100 20 | .TemporaryItems 21 | .Trashes 22 | .VolumeIcon.icns 23 | .com.apple.timemachine.donotpresent 24 | 25 | # Directories potentially created on remote AFP share 26 | .AppleDB 27 | .AppleDesktop 28 | Network Trash Folder 29 | Temporary Items 30 | .apdisk 31 | 32 | 33 | ## C++ 34 | 35 | # Prerequisites 36 | *.d 37 | 38 | # Compiled Object files 39 | *.slo 40 | *.lo 41 | *.o 42 | *.obj 43 | 44 | # Precompiled Headers 45 | *.gch 46 | *.pch 47 | 48 | # Compiled Dynamic libraries 49 | *.so 50 | *.dylib 51 | *.dll 52 | 53 | # Fortran module files 54 | *.mod 55 | *.smod 56 | 57 | # Compiled Static libraries 58 | *.lai 59 | *.la 60 | *.a 61 | *.lib 62 | 63 | # Executables 64 | *.exe 65 | *.out 66 | *.app 67 | 68 | 69 | ## CMake 70 | 71 | CMakeLists.txt.user 72 | CMakeCache.txt 73 | CMakeFiles 74 | CMakeScripts 75 | Testing 76 | Makefile 77 | cmake_install.cmake 78 | install_manifest.txt 79 | compile_commands.json 80 | CTestTestfile.cmake 81 | _deps 82 | 83 | 84 | ## Build 85 | build* 86 | bin* 87 | 88 | ## IDES 89 | *.autosave 90 | .idea/ 91 | *.code-workspace 92 | *.vscode 93 | 94 | ## python 95 | # Byte-compiled / optimized / DLL files 96 | __pycache__/ 97 | *.py[cod] 98 | *$py.class 99 | 100 | # C extensions 101 | *.so 102 | 103 | # Distribution / packaging 104 | .Python 105 | build/ 106 | develop-eggs/ 107 | dist/ 108 | downloads/ 109 | eggs/ 110 | .eggs/ 111 | lib/ 112 | lib64/ 113 | parts/ 114 | sdist/ 115 | var/ 116 | wheels/ 117 | pip-wheel-metadata/ 118 | share/python-wheels/ 119 | *.egg-info/ 120 | .installed.cfg 121 | *.egg 122 | MANIFEST 123 | 124 | # PyInstaller 125 | # Usually these files are written by a python script from a template 126 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 127 | *.manifest 128 | *.spec 129 | 130 | # Installer logs 131 | pip-log.txt 132 | pip-delete-this-directory.txt 133 | 134 | # Unit test / coverage reports 135 | htmlcov/ 136 | .tox/ 137 | .nox/ 138 | .coverage 139 | .coverage.* 140 | .cache 141 | nosetests.xml 142 | coverage.xml 143 | *.cover 144 | *.py,cover 145 | .hypothesis/ 146 | .pytest_cache/ 147 | 148 | # Translations 149 | *.mo 150 | *.pot 151 | 152 | # Django stuff: 153 | *.log 154 | local_settings.py 155 | db.sqlite3 156 | db.sqlite3-journal 157 | 158 | # Flask stuff: 159 | instance/ 160 | .webassets-cache 161 | 162 | # Scrapy stuff: 163 | .scrapy 164 | 165 | # Sphinx documentation 166 | docs/_build/ 167 | 168 | # PyBuilder 169 | target/ 170 | 171 | # Jupyter Notebook 172 | .ipynb_checkpoints 173 | 174 | # IPython 175 | profile_default/ 176 | ipython_config.py 177 | 178 | # pyenv 179 | .python-version 180 | 181 | # pipenv 182 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 183 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 184 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 185 | # install all needed dependencies. 186 | #Pipfile.lock 187 | 188 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 189 | __pypackages__/ 190 | 191 | # Celery stuff 192 | celerybeat-schedule 193 | celerybeat.pid 194 | 195 | # SageMath parsed files 196 | *.sage.py 197 | 198 | # Environments 199 | .env 200 | .venv 201 | env/ 202 | venv/ 203 | ENV/ 204 | env.bak/ 205 | venv.bak/ 206 | 207 | # Spyder project settings 208 | .spyderproject 209 | .spyproject 210 | 211 | # Rope project settings 212 | .ropeproject 213 | 214 | # mkdocs documentation 215 | /site 216 | 217 | # mypy 218 | .mypy_cache/ 219 | .dmypy.json 220 | dmypy.json 221 | 222 | # Pyre type checker 223 | .pyre/ 224 | 225 | ## Documentation 226 | docs 227 | 228 | ## External dependancies 229 | external 230 | sample-queries 231 | 232 | *.nosync 233 | 234 | ############################################################################### 235 | 236 | paper/ 237 | TightInclusionOptions.cmake 238 | src/tight_inclusion/config.hpp 239 | app/config.hpp -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Detects whether this is a top-level project 2 | get_directory_property(HAS_PARENT PARENT_DIRECTORY) 3 | if(HAS_PARENT) 4 | set(TIGHT_INCLUSION_TOPLEVEL_PROJECT OFF) 5 | else() 6 | set(TIGHT_INCLUSION_TOPLEVEL_PROJECT ON) 7 | endif() 8 | 9 | # Check required CMake version 10 | set(REQUIRED_CMAKE_VERSION "3.18.0") 11 | if(TIGHT_INCLUSION_TOPLEVEL_PROJECT) 12 | cmake_minimum_required(VERSION ${REQUIRED_CMAKE_VERSION}) 13 | else() 14 | # Don't use cmake_minimum_required here to avoid implicitly overriding parent policies 15 | if(${CMAKE_VERSION} VERSION_LESS ${REQUIRED_CMAKE_VERSION}) 16 | message(FATAL_ERROR "CMake required version to build Tight Inclusion is ${REQUIRED_CMAKE_VERSION}") 17 | endif() 18 | endif() 19 | 20 | # Include user-provided default options if available. We do that before the main 21 | # `project()` so that we can define the C/C++ compilers from the option file. 22 | if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/TightInclusionOptions.cmake) 23 | message(STATUS "Using local options file: ${CMAKE_CURRENT_SOURCE_DIR}/TightInclusionOptions.cmake") 24 | include(${CMAKE_CURRENT_SOURCE_DIR}/TightInclusionOptions.cmake) 25 | endif() 26 | 27 | # Enable ccache if available 28 | find_program(CCACHE_PROGRAM ccache) 29 | if(CCACHE_PROGRAM) 30 | option(TIGHT_INCLUSION_WITH_CCACHE "Enable ccache when building Tight Inclusion" ${TIGHT_INCLUSION_TOPLEVEL_PROJECT}) 31 | else() 32 | option(TIGHT_INCLUSION_WITH_CCACHE "Enable ccache when building Tight Inclusion" OFF) 33 | endif() 34 | if(TIGHT_INCLUSION_WITH_CCACHE AND CCACHE_PROGRAM) 35 | message(STATUS "Enabling Ccache support") 36 | set(ccacheEnv 37 | CCACHE_BASEDIR=${CMAKE_BINARY_DIR} 38 | CCACHE_SLOPPINESS=clang_index_store,include_file_ctime,include_file_mtime,locale,pch_defines,time_macros 39 | ) 40 | foreach(lang IN ITEMS C CXX) 41 | set(CMAKE_${lang}_COMPILER_LAUNCHER 42 | ${CMAKE_COMMAND} -E env ${ccacheEnv} ${CCACHE_PROGRAM} 43 | ) 44 | endforeach() 45 | endif() 46 | 47 | ################################################################################ 48 | # CMake Policies 49 | ################################################################################ 50 | 51 | cmake_policy(SET CMP0054 NEW) # Only interpret if() arguments as variables or keywords when unquoted. 52 | cmake_policy(SET CMP0076 NEW) # target_sources() command converts relative paths to absolute. 53 | if(CMAKE_VERSION VERSION_GREATER_EQUAL "3.24") 54 | cmake_policy(SET CMP0135 NEW) # Set the timestamps of all extracted contents to the time of the extraction. 55 | endif() 56 | 57 | ################################################################################ 58 | 59 | project(TightInclusion 60 | DESCRIPTION "Tight Inclusion CCD" 61 | LANGUAGES CXX 62 | VERSION "1.0.4") 63 | 64 | option(TIGHT_INCLUSION_WITH_RATIONAL "Enable rational based predicates (for debugging)" OFF) 65 | option(TIGHT_INCLUSION_WITH_TIMER "Enable profiling timers (for debugging)" OFF) 66 | option(TIGHT_INCLUSION_WITH_DOUBLE_PRECISION "Enable double precision floating point numbers as input" ON) 67 | option(TIGHT_INCLUSION_LIMIT_QUEUE_SIZE "Enable limitation of maximal queue size" OFF) 68 | 69 | include(CMakeDependentOption) 70 | cmake_dependent_option(TIGHT_INCLUSION_FLOAT_WITH_DOUBLE_INPUT "Enable converting double queries to float" OFF "TIGHT_INCLUSION_WITH_DOUBLE_PRECISION" ON) 71 | 72 | # Set default minimum C++ standard 73 | if(TIGHT_INCLUSION_TOPLEVEL_PROJECT) 74 | set(CMAKE_CXX_STANDARD 17) 75 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 76 | set(CMAKE_CXX_EXTENSIONS OFF) 77 | endif() 78 | 79 | ### Configuration 80 | set(TIGHT_INCLUSION_SOURCE_DIR "${PROJECT_SOURCE_DIR}/src/tight_inclusion") 81 | set(TIGHT_INCLUSION_INCLUDE_DIR "${PROJECT_SOURCE_DIR}/src") 82 | 83 | list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/tight_inclusion/") 84 | list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/recipes/") 85 | list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/find/") 86 | 87 | # General CMake utils 88 | include(tight_inclusion_cpm_cache) 89 | include(tight_inclusion_use_colors) 90 | 91 | # Generate position-independent code by default 92 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 93 | 94 | ################################################################################ 95 | # Tight Inclusion Library 96 | ################################################################################ 97 | 98 | # Add an empty library and fill in the list of sources in `src/CMakeLists.txt`. 99 | add_library(tight_inclusion) 100 | add_library(tight_inclusion::tight_inclusion ALIAS tight_inclusion) 101 | 102 | # Fill in configuration options 103 | configure_file( 104 | "${TIGHT_INCLUSION_SOURCE_DIR}/config.hpp.in" 105 | "${TIGHT_INCLUSION_SOURCE_DIR}/config.hpp") 106 | 107 | # Add source and header files to tight_inclusion 108 | add_subdirectory("${TIGHT_INCLUSION_SOURCE_DIR}") 109 | 110 | # Public include directory for Tight Inclusion 111 | target_include_directories(tight_inclusion PUBLIC "${TIGHT_INCLUSION_INCLUDE_DIR}") 112 | 113 | ################################################################################ 114 | # Optional Definitions 115 | ################################################################################ 116 | 117 | # For MSVC, do not use the min and max macros. 118 | target_compile_definitions(tight_inclusion PUBLIC NOMINMAX) 119 | 120 | ################################################################################ 121 | # Dependencies 122 | ################################################################################ 123 | 124 | # Eigen 125 | include(eigen) 126 | target_link_libraries(tight_inclusion PUBLIC Eigen3::Eigen) 127 | 128 | # Logger 129 | include(spdlog) 130 | target_link_libraries(tight_inclusion PUBLIC spdlog::spdlog) 131 | 132 | # rational-cpp (optional) 133 | if(TIGHT_INCLUSION_WITH_RATIONAL) 134 | include(rational_cpp) 135 | target_link_libraries(tight_inclusion PUBLIC rational::rational) 136 | endif() 137 | 138 | # Extra warnings (link last for highest priority) 139 | include(tight_inclusion_warnings) 140 | target_link_libraries(tight_inclusion PRIVATE tight_inclusion::warnings) 141 | 142 | ################################################################################ 143 | # Compiler options 144 | ################################################################################ 145 | 146 | # Figure out AVX level support 147 | message(STATUS "Searching for AVX...") 148 | find_package(AVX) 149 | string(REPLACE " " ";" SIMD_FLAGS "${AVX_FLAGS}") 150 | target_compile_options(tight_inclusion PRIVATE ${SIMD_FLAGS}) 151 | 152 | # Use C++17 153 | target_compile_features(tight_inclusion PUBLIC cxx_std_17) 154 | 155 | ################################################################################ 156 | # App 157 | ################################################################################ 158 | 159 | if(TIGHT_INCLUSION_TOPLEVEL_PROJECT) 160 | add_subdirectory(app) 161 | endif() 162 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Bolun Wang, Zachary Ferguson, Teseo Schneider, Xin Jiang, Marco Attene, and Daniele Panozzo 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Tight-Inclusion Continuous Collision Detection 2 | 3 | [![Build](https://github.com/Continuous-Collision-Detection/Tight-Inclusion/actions/workflows/continuous.yml/badge.svg)](https://github.com/Continuous-Collision-Detection/Tight-Inclusion/actions/workflows/continuous.yml) 4 | 5 | ![](https://continuous-collision-detection.github.io/assets/tight-inclusion-teaser.png) 6 | 7 | A conservative continuous collision detection (CCD) method with support for minimum separation. 8 | 9 | To know more about this work, please read our ACM Transactions on Graphics paper:
10 | ["A Large Scale Benchmark and an Inclusion-Based Algorithm for Continuous Collision Detection"](https://continuous-collision-detection.github.io/tight_inclusion/) and watch our [SIGGRAPH 2022 presentation](https://www.youtube.com/watch?v=7cRg52cWL8c). 11 | 12 | ## Build 13 | 14 | To compile the code, first, make sure CMake is installed. 15 | 16 | To build the library on Linux or macOS: 17 | 18 | ```sh 19 | mkdir build 20 | cd build 21 | cmake .. -DCMAKE_BUILD_TYPE=Release 22 | make -j4 23 | ``` 24 | 25 | Then you can run a CCD example: 26 | 27 | ```bash 28 | ./app/Tight_Inclusion_bin 29 | ``` 30 | 31 | ### Optional 32 | 33 | We also provide an example that tests [sample queries](https://github.com/Continuous-Collision-Detection/Sample-Queries) using our CCD method. This requires installing `gmp` on your system before compiling the code. Set the CMake option `TIGHT_INCLUSION_WITH_SAMPLE_QUERIES` to `ON` when compiling: 34 | 35 | ```sh 36 | cmake .. -DCMAKE_BUILD_TYPE=Release -DTIGHT_INCLUSION_WITH_SAMPLE_QUERIES=ON 37 | make -j4 38 | ``` 39 | 40 | Then you can run `./app/Tight_Inclusion_bin` to test the handcrafted and simulation queries in the Sample Queries. 41 | 42 | ## Usage 43 | 44 | ### Overview 45 | 46 | * Include: `#include ` 47 | * Check vertex-face CCD: `bool ticcd::vertexFaceCCD(...)` 48 | * Check edge-edge CCD: `bool ticcd::edgeEdgeCCD(...)` 49 | 50 | ### Details 51 | 52 | :bulb: Each CCD function returns a boolean result corresponding to if a collision is detected. Because our method is *conservative*, we guarantee a result of `false` implies no collision occurs. If the result is `true`, there may not be a collision but we falsely report a collision. However, we can guarantee that this happens only if the minimal distance between the two primitives in this time step is no larger than `tolerance + ms + err` (see below for a description of these parameters). 53 | 54 | #### Parameters 55 | 56 | For both vertex-face and edge-edge CCD, the input query is given by eight vertices which are in the format of `Eigen::Vector3d`. Please read our code in `tight_inclusion/ccd.hpp` for the correct input order of the vertices. 57 | 58 | Besides the input vertices, there are some input and output parameters for users to tune the performance or to get more information from the CCD. 59 | 60 | Here is a list of the explanations of the parameters: 61 | 62 | ##### Input 63 | * `err`: The numerical filters of the $x$, $y$ and $z$ coordinates. It measures the errors introduced by floating-point calculation when solving inclusion functions. 64 | * `ms`: A minimum separation distance (no less than 0). We guarantee a collision will be reported if the distance between the two primitives is less than `ms`. 65 | * `tolerance`: User-specific solving precision. It is the target maximal $x$, $y$, and $z$ length of the inclusion function. We suggest the to use `1e-6`. 66 | * `t_max`: The time range $[0, t_{\max}]$ where we detect collisions. Since the input query implies the motion is in time interval $[0, 1]$, `t_max` should not be larger than 1. 67 | * `max_itr`: The maximum number of iterations our inclusion-based root-finding algorithm can take. This enables early termination of the algorithm. If you set `max_itr < 0`, early termination will be disabled, but this may cause longer running times. We suggest setting `max_itr = 1e6`. 68 | * `no_zero_toi`: For simulators which use non-zero minimum separation distance (`ms > 0`) to make sure intersection-free for each time-step, we have the option `no_zero_toi` to avoid returning a collision time `toi` of 0. The code will continue the refinement in higher precision if the output `toi` is 0 under the given `tolerance`, so the eventual `toi` will not be 0. 69 | * `CCD_TYPE`: Enumeration of possible CCD schemes. The default and recommended type is `BREADTH_FIRST_SEARCH`. If set `DEPTH_FIRST_SEARCH`, the code will switch to a naive conservative CCD algorithm but lacks our advanced features. 70 | 71 | ##### Output 72 | 73 | * `toi`: The time of impact. If multiple collisions happen in this time step, it will return the earliest collision time. If there is no collision, the returned `toi` value will be `std::numeric_limits::infinity()`. 74 | * `output_tolerance`: The resulting solve's precision. If early termination is enabled, the solving precision may not reach the target precision. This parameter will return the resulting solving precision when the code is terminated. 75 | 76 | ### Tips 77 | 78 | :bulb: The input parameter `err` is crucial to guarantee our algorithm is a conservative method not affected by floating-point rounding errors. To run a single query, you can set `err = Eigen::Array3d(-1, -1, -1)` to enable a sub-function to calculate the real numerical filters when solving CCD. If you are integrating our CCD in simulators, you need to: 79 | 80 | * Include the headler: `#include `. 81 | * Call 82 | ``` 83 | std::array err_vf = ticcd::get_numerical_error() 84 | ``` 85 | and 86 | ``` 87 | std::array err_ee = ticcd::get_numerical_error() 88 | ``` 89 | * Use the parameter `err_ee` each time you call `bool ticcd::edgeEdgeCCD()` and `err_vf` when you call `bool ticcd::vertexFaceCCD()`. 90 | 91 | The parameters for function `ticcd::get_numerical_error()` are: 92 | * `vertices`: Vertices of the axis-aligned bounding box of the simulation scene. Before you run the simulation, you need to conservatively estimate the axis-aligned bounding box in which the meshes will be located during the whole simulation process, and the vertices should be the corners of the AABB. 93 | * `is_vertex_face`: A boolean flag corresponding to if you are checking vertex-face or edge-edge CCD. 94 | * `using_minimum_separation`: A boolean flag corresponding to if you are using minimum-separation CCD (the input parameter `ms > 0`). 95 | 96 | To better understand or to get more details of our Tight-Inclusion CCD algorithm, please refer to our paper. 97 | 98 | ## Citation 99 | 100 | If you use this work in your project, please consider citing the original paper: 101 | 102 | ```bibtex 103 | @article{Wang:2021:Benchmark, 104 | title = {A Large Scale Benchmark and an Inclusion-Based Algorithm for Continuous Collision Detection}, 105 | author = {Bolun Wang and Zachary Ferguson and Teseo Schneider and Xin Jiang and Marco Attene and Daniele Panozzo}, 106 | year = 2021, 107 | month = oct, 108 | journal = {ACM Transactions on Graphics}, 109 | volume = 40, 110 | number = 5, 111 | articleno = 188, 112 | numpages = 16 113 | } 114 | ``` 115 | -------------------------------------------------------------------------------- /TightInclusionOptions.cmake.sample: -------------------------------------------------------------------------------- 1 | # In order to persistently set default options for your project, copy this file 2 | # and remove the '.sample' suffix. Then uncomment the relevant options for your 3 | # project. Note that this file is included before `project(IPCToolkit)` is defined, 4 | # so we can use it to define the C and C++ compilers, but some variables such as 5 | # PROJECT_SOURCE_DIR will not be defined yet. You can use CMAKE_SOURCE_DIR instead. 6 | 7 | ################################################################################ 8 | # CMake Options 9 | ################################################################################ 10 | 11 | # Specify a custom install prefix path 12 | # set(CMAKE_INSTALL_PREFIX ${CMAKE_SOURCE_DIR}/install CACHE STRING "Install directory used by install().") 13 | 14 | # Generates a `compile_commands.json` that can be used for autocompletion 15 | # set(CMAKE_EXPORT_COMPILE_COMMANDS ON CACHE BOOL "Enable/Disable output of compile commands during generation.") 16 | 17 | # Use a specific C/C++ compiler, e.g. llvm-clang on macOS (so we can use clangd) 18 | # set(CMAKE_C_COMPILER "/usr/local/opt/llvm/bin/clang" CACHE STRING "C compiler") 19 | # set(CMAKE_CXX_COMPILER "/usr/local/opt/llvm/bin/clang++" CACHE STRING "C++ compiler") 20 | 21 | # Use a specific Cuda compiler 22 | # set(CMAKE_CUDA_COMPILER "/usr/local/cuda/bin/nvcc" CACHE STRING "C++ compiler") 23 | 24 | # Set deployment platform for macOS 25 | # set(CMAKE_OSX_DEPLOYMENT_TARGET 10.12 CACHE STRING "macOS deployment target") 26 | 27 | ################################################################################ 28 | # IPC Toolkit Options 29 | ################################################################################ 30 | 31 | # option(TIGHT_INCLUSION_WITH_RATIONAL "Enable rational based predicates (for debugging)" OFF) 32 | # option(TIGHT_INCLUSION_WITH_TIMER "Enable profiling timers (for debugging)" OFF) 33 | # option(TIGHT_INCLUSION_WITH_DOUBLE_PRECISION "Enable double precision floating point numbers as input" ON) 34 | # option(TIGHT_INCLUSION_LIMIT_QUEUE_SIZE "Enable limitation of maximal queue size" OFF) 35 | # option(TIGHT_INCLUSION_WITH_SAMPLE_QUERIES "Enable sample queries in the application" OFF) -------------------------------------------------------------------------------- /app/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # App options 2 | option(TIGHT_INCLUSION_WITH_SAMPLE_QUERIES "Enable sample queries in the application" OFF) 3 | 4 | # Add the executable 5 | add_executable(Tight_Inclusion_bin "main.cpp") 6 | 7 | # Link to the library 8 | target_link_libraries(Tight_Inclusion_bin PUBLIC tight_inclusion) 9 | 10 | # Fill in configuration options 11 | configure_file("${CMAKE_CURRENT_SOURCE_DIR}/config.hpp.in" "${CMAKE_CURRENT_SOURCE_DIR}/config.hpp") 12 | 13 | # Link to the required libraries 14 | if(TIGHT_INCLUSION_WITH_SAMPLE_QUERIES) 15 | include(ccd_query_io) 16 | target_link_libraries(Tight_Inclusion_bin PUBLIC ccd_io::ccd_io) 17 | endif() 18 | 19 | include(pbar) 20 | target_link_libraries(Tight_Inclusion_bin PUBLIC pbar::pbar) 21 | 22 | include(cli11) 23 | target_link_libraries(Tight_Inclusion_bin PUBLIC CLI11::CLI11) -------------------------------------------------------------------------------- /app/config.hpp.in: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // WARNING: Do not modify config.hpp directly. Instead, modify config.hpp.in. 4 | 5 | #cmakedefine TIGHT_INCLUSION_WITH_SAMPLE_QUERIES -------------------------------------------------------------------------------- /app/main.cpp: -------------------------------------------------------------------------------- 1 | #include "config.hpp" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #ifdef TIGHT_INCLUSION_WITH_SAMPLE_QUERIES 9 | #include 10 | #endif 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | namespace fs = std::filesystem; 18 | 19 | using namespace ticcd; 20 | 21 | #ifdef TIGHT_INCLUSION_WITH_SAMPLE_QUERIES 22 | 23 | static const std::string root_path(CCD_IO_SAMPLE_QUERIES_DIR); 24 | 25 | static const std::vector simulation_folders = {{ 26 | "chain", 27 | "cow-heads", 28 | "golf-ball", 29 | "mat-twist", 30 | }}; 31 | 32 | static const std::vector handcrafted_folders = {{ 33 | "erleben-sliding-spike", 34 | "erleben-spike-wedge", 35 | "erleben-sliding-wedge", 36 | "erleben-wedge-crack", 37 | "erleben-spike-crack", 38 | "erleben-wedges", 39 | "erleben-cube-cliff-edges", 40 | "erleben-spike-hole", 41 | "erleben-cube-internal-edges", 42 | "erleben-spikes", 43 | "unit-tests", 44 | }}; 45 | 46 | void check_sample_queries( 47 | const bool is_edge_edge, 48 | const bool is_simulation_data, 49 | const double minimum_seperation, 50 | const double tolerance, 51 | const long max_itr, 52 | const bool print_progress = true) 53 | { 54 | using Matrix8x3 = Eigen::Matrix; 55 | 56 | const Eigen::Array3d err(-1, -1, -1); 57 | constexpr double t_max = 1; 58 | constexpr bool no_zero_toi = false; 59 | constexpr CCDRootFindingMethod ccd_method = 60 | CCDRootFindingMethod::BREADTH_FIRST_SEARCH; 61 | 62 | int total_positives = 0; 63 | int total_false_positives = 0; 64 | int total_false_negatives = 0; 65 | 66 | Timer timer; 67 | double total_time_in_micro_sec = 0.0; 68 | 69 | const auto folders = 70 | is_simulation_data ? simulation_folders : handcrafted_folders; 71 | const std::string sub_folder = is_edge_edge ? "edge-edge" : "vertex-face"; 72 | 73 | size_t total_number_of_queries = 0; 74 | pbar::pbar bar(-1); 75 | if (print_progress) { 76 | logger().trace("Determining total number of queries..."); 77 | for (const std::string &folder : folders) { 78 | fs::path dir = fs::path(root_path) / folder / sub_folder; 79 | for (const auto &csv : fs::directory_iterator(dir)) { 80 | std::ifstream in_stream(csv.path().string()); 81 | unsigned int line_count = std::count_if( 82 | std::istreambuf_iterator{in_stream}, {}, 83 | [](char c) { return c == '\n'; }); 84 | assert(line_count % 8 == 0); 85 | total_number_of_queries += line_count / 8; 86 | } 87 | } 88 | logger().trace("Total number of queries: {}", total_number_of_queries); 89 | 90 | bar = pbar::pbar(total_number_of_queries); 91 | bar.enable_recalc_console_width(1); // check console width every tick 92 | bar.init(); 93 | } 94 | 95 | for (const std::string &folder : folders) { 96 | fs::path dir = fs::path(root_path) / folder / sub_folder; 97 | for (const auto &csv : fs::directory_iterator(dir)) { 98 | 99 | const std::vector queries = 100 | ccd_io::read_ccd_queries(csv.path().string()); 101 | 102 | for (int i = 0; i < queries.size(); i++) { 103 | Eigen::Map V(&queries[i].vertices[0][0]); 104 | const bool expected_result = queries[i].ground_truth; 105 | total_positives += expected_result; 106 | 107 | // Output of CCD 108 | bool result; 109 | double toi; 110 | double output_tolerance = tolerance; 111 | 112 | timer.start(); 113 | if (is_edge_edge) { 114 | result = edgeEdgeCCD( 115 | V.row(0), V.row(1), V.row(2), V.row(3), V.row(4), 116 | V.row(5), V.row(6), V.row(7), err, minimum_seperation, 117 | toi, tolerance, t_max, max_itr, output_tolerance, 118 | no_zero_toi, ccd_method); 119 | // result = rational::edgeEdgeCCD( 120 | // V.row(0), V.row(1), V.row(2), V.row(3), V.row(4), 121 | // V.row(5), V.row(6), V.row(7), err, minimum_seperation, 122 | // toi); 123 | } else { 124 | result = vertexFaceCCD( 125 | V.row(0), V.row(1), V.row(2), V.row(3), V.row(4), 126 | V.row(5), V.row(6), V.row(7), err, minimum_seperation, 127 | toi, tolerance, t_max, max_itr, output_tolerance, 128 | no_zero_toi, ccd_method); 129 | // result = rational::vertexFaceCCD( 130 | // V.row(0), V.row(1), V.row(2), V.row(3), V.row(4), 131 | // V.row(5), V.row(6), V.row(7), err, minimum_seperation, 132 | // toi); 133 | } 134 | timer.stop(); 135 | total_time_in_micro_sec += timer.getElapsedTimeInMicroSec(); 136 | 137 | if (result != expected_result) { 138 | if (result) { 139 | total_false_positives++; 140 | } else { 141 | total_false_negatives++; 142 | 143 | logger().error( 144 | "False negative encountered in file \"{}\" (query #{})!", 145 | csv.path().string(), i); 146 | for (int j = 0; j < 8; j++) { 147 | logger().debug( 148 | "V{}: {:.17f} {:.17f} {:.17f}", // 149 | j, V(j, 0), V(j, 1), V(j, 2)); 150 | } 151 | logger().debug("Is edge: {}", is_edge_edge); 152 | assert(false); 153 | // exit(1); 154 | } 155 | } 156 | 157 | if (print_progress) { 158 | ++bar; // total_number_of_queries already computed 159 | } else { 160 | ++total_number_of_queries; 161 | } 162 | } 163 | } 164 | } 165 | 166 | logger().info("total # of queries: {:7d}", total_number_of_queries); 167 | logger().info("total positives: {:7d}", total_positives); 168 | logger().info("# of false positives: {:7d}", total_false_positives); 169 | logger().info("# of false negatives: {:7d}", total_false_negatives); 170 | logger().info( 171 | "total time: {:g} s", total_time_in_micro_sec / 1e6); 172 | logger().info( 173 | "average time: {:g} μs", 174 | total_time_in_micro_sec / double(total_number_of_queries)); 175 | } 176 | 177 | void check_all_sample_queries(const bool print_progress = true) 178 | { 179 | constexpr double ms = 0.0; 180 | constexpr double tolerance = 1e-6; 181 | constexpr long max_itr = 1e6; 182 | 183 | for (const bool is_simulation : std::array{{true, false}}) { 184 | for (const bool is_edge_edge : std::array{{false, true}}) { 185 | fmt::print( 186 | "\nRunning {} {} data:\n", 187 | is_simulation ? "simulation" : "handcrafted", 188 | is_edge_edge ? "edge-edge" : "vertex-face"); 189 | check_sample_queries( 190 | is_edge_edge, is_simulation, ms, tolerance, max_itr, 191 | print_progress); 192 | } 193 | } 194 | } 195 | #endif 196 | 197 | void check_single_case() 198 | { 199 | const Vector3 ea0_t0(0.1, 0.1, 0.1); 200 | const Vector3 ea1_t0(0, 0, 1); 201 | const Vector3 ea0_t1(1, 0, 1); 202 | const Vector3 ea1_t1(0, 1, 1); 203 | const Vector3 eb0_t0(0.1, 0.1, 0.1); 204 | const Vector3 eb1_t0(0, 0, 0); 205 | const Vector3 eb0_t1(0, 1, 0); 206 | const Vector3 eb1_t1(1, 0, 0); 207 | 208 | const Array3 err(-1, -1, -1); 209 | constexpr double ms = 1e-8; 210 | constexpr double tolerance = 1e-6; 211 | constexpr double t_max = 1; 212 | constexpr long max_itr = 1e6; 213 | 214 | double toi, output_tolerance; 215 | const bool res = edgeEdgeCCD( 216 | ea0_t0, ea1_t0, ea0_t1, ea1_t1, eb0_t0, eb1_t0, eb0_t1, eb1_t1, err, ms, 217 | toi, tolerance, t_max, max_itr, output_tolerance); 218 | 219 | logger().info("Double CCD result: {}", res); 220 | #ifdef TIGHT_INCLUSION_CHECK_QUEUE_SIZE 221 | logger().info("queue size max {}", return_queue_size()); 222 | #endif 223 | } 224 | 225 | int main(int argc, char *argv[]) 226 | { 227 | logger().set_level(spdlog::level::trace); 228 | 229 | CLI::App app{"Tight Inclusion CCD"}; 230 | 231 | bool print_progress = true; 232 | app.add_flag("--pbar,!--no-pbar", print_progress, "Hide progress bar"); 233 | 234 | CLI11_PARSE(app, argc, argv); 235 | logger().debug("Progress bar: {}", print_progress); 236 | 237 | logger().debug( 238 | "Using {} precision floating point numbers", 239 | #ifdef TIGHT_INCLUSION_WITH_DOUBLE_PRECISION 240 | "double" 241 | #else 242 | "single" 243 | #endif 244 | ); 245 | 246 | #ifdef TIGHT_INCLUSION_WITH_SAMPLE_QUERIES 247 | check_all_sample_queries(print_progress); 248 | #else 249 | check_single_case(); 250 | #endif 251 | 252 | return 0; 253 | } 254 | -------------------------------------------------------------------------------- /cmake/find/FindAVX.cmake: -------------------------------------------------------------------------------- 1 | # This script checks for the highest level of AVX support on the host 2 | # by compiling and running small C++ programs that use AVX intrinsics. 3 | # 4 | # You can invoke this module using the following command: 5 | # 6 | # FIND_PACKAGE(AVX [major[.minor]] [EXACT] [QUIET|REQUIRED]) 7 | # 8 | # where the version string is one of: 9 | # 10 | # 1.0 for AVX support 11 | # 2.0 for AVX2 support 12 | # 13 | # Note that any ".0" in the above version string is optional. 14 | # 15 | # If any AVX support is detected, the following variables are set: 16 | # 17 | # AVX_FOUND = 1 18 | # AVX_VERSION = the requested version, if EXACT is true, or 19 | # the highest AVX version found. 20 | # AVX_FLAGS = compile flags for the version of AVX found 21 | # 22 | # If AVX is not supported on the host platform, these variables are 23 | # not set. If QUIET is true, the module does not print a message if 24 | # AVX if missing. If REQUIRED is true, the module produces a fatal 25 | # error if AVX support is missing. 26 | # 27 | set(AVX_FLAGS) 28 | set(AVX_FOUND) 29 | set(DETECTED_AVX_10) 30 | set(DETECTED_AVX_20) 31 | 32 | if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX) 33 | execute_process(COMMAND ${CMAKE_CXX_COMPILER} "-dumpversion" OUTPUT_VARIABLE GCC_VERSION_STRING) 34 | if(GCC_VERSION_STRING VERSION_GREATER 4.2 AND NOT APPLE AND NOT CMAKE_CROSSCOMPILING) 35 | SET(AVX_FLAGS "${AVX_FLAGS} -march=native") 36 | message(STATUS "Using CPU native flags for AVX optimization: ${AVX_FLAGS}") 37 | endif() 38 | endif() 39 | 40 | include(CheckCXXSourceRuns) 41 | set(CMAKE_REQUIRED_FLAGS) 42 | 43 | 44 | # Generate a list of AVX versions to test. 45 | if(AVX_FIND_VERSION_EXACT) 46 | if(AVX_FIND_VERSION VERSION_EQUAL "2.0") 47 | set(_AVX_TEST_20 1) 48 | elseif(AVX_FIND_VERSION VERSION_EQUAL "1.0") 49 | set(_AVX_TEST_10 1) 50 | endif() 51 | else() 52 | if(NOT AVX_FIND_VERSION VERSION_GREATER "2.0") 53 | set(_AVX_TEST_20 1) 54 | endif() 55 | if(NOT AVX_FIND_VERSION VERSION_GREATER "1.0") 56 | set(_AVX_TEST_10 1) 57 | endif() 58 | endif() 59 | 60 | # Check for AVX2 support. 61 | if(_AVX_TEST_20) 62 | if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 63 | set(CMAKE_REQUIRED_FLAGS "-mavx2") 64 | elseif(CMAKE_CXX_COMPILER_ID MATCHES "Intel") 65 | set(CMAKE_REQUIRED_FLAGS "-xHost") 66 | elseif(MSVC AND NOT CMAKE_CL_64) 67 | set(CMAKE_REQUIRED_FLAGS "/arch:AVX2") 68 | endif() 69 | check_cxx_source_runs(" 70 | #include 71 | int main() 72 | { 73 | __m256i a = _mm256_set_epi32 (-1, 2, -3, 4, -1, 2, -3, 4); 74 | __m256i result = _mm256_abs_epi32 (a); 75 | return 0; 76 | }" DETECTED_AVX_20) 77 | endif() 78 | 79 | # Check for AVX support. 80 | if(_AVX_TEST_10) 81 | if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 82 | set(CMAKE_REQUIRED_FLAGS "-mavx") 83 | elseif(CMAKE_CXX_COMPILER_ID MATCHES "Intel") 84 | set(CMAKE_REQUIRED_FLAGS "-xHost") 85 | elseif(MSVC AND NOT CMAKE_CL_64) 86 | set(CMAKE_REQUIRED_FLAGS "/arch:AVX") 87 | endif() 88 | check_cxx_source_runs(" 89 | #include 90 | int main() 91 | { 92 | __m256 a = _mm256_set_ps (-1.0f, 2.0f, -3.0f, 4.0f, -1.0f, 2.0f, -3.0f, 4.0f); 93 | __m256 b = _mm256_set_ps (1.0f, 2.0f, 3.0f, 4.0f, 1.0f, 2.0f, 3.0f, 4.0f); 94 | __m256 result = _mm256_add_ps (a, b); 95 | return 0; 96 | }" DETECTED_AVX_10) 97 | endif() 98 | 99 | set(CMAKE_REQUIRED_FLAGS) 100 | 101 | 102 | if(DETECTED_AVX_20) 103 | set(AVX_VERSION "2.0") 104 | set(AVX_STR "2_0") 105 | set(AVX_FOUND 1) 106 | elseif(DETECTED_AVX_10) 107 | set(AVX_VERSION "1.0") 108 | set(AVX_STR "1_0") 109 | set(AVX_FOUND 1) 110 | endif() 111 | 112 | 113 | if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 114 | if(DETECTED_AVX_20) 115 | SET(AVX_FLAGS "${AVX_FLAGS} -mavx2") 116 | elseif(DETECTED_AVX_10) 117 | SET(AVX_FLAGS "${AVX_FLAGS} -mavx") 118 | endif() 119 | # Only add -mno-avx512* if the compiler accepts them 120 | foreach(flag -mno-avx512f -mno-avx512pf -mno-avx512er -mno-avx512cd) 121 | string(REPLACE "-" "_" safe_flag ${flag}) 122 | check_cxx_compiler_flag("${flag}" HAS_FLAG_${safe_flag}) 123 | if(HAS_FLAG_${safe_flag}) 124 | set(AVX_FLAGS "${AVX_FLAGS} ${flag}") 125 | endif() 126 | endforeach() 127 | elseif(CMAKE_CXX_COMPILER_ID MATCHES "Intel") 128 | set(AVX_FLAGS "-xHost") 129 | elseif(MSVC) 130 | if(DETECTED_AVX_20) 131 | SET(AVX_FLAGS "${AVX_FLAGS} /arch:AVX2") 132 | elseif(DETECTED_AVX_10) 133 | SET(AVX_FLAGS "${AVX_FLAGS} /arch:AVX") 134 | endif() 135 | endif() 136 | 137 | if(AVX_FOUND) 138 | message(STATUS " Found AVX ${AVX_VERSION} extensions, using flags: ${AVX_FLAGS}") 139 | else() 140 | message(STATUS " No AVX support found") 141 | set(AVX_FLAGS "") 142 | endif() 143 | 144 | set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${AVX_FLAGS}") 145 | set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} ${AVX_FLAGS}") 146 | set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${AVX_FLAGS}") 147 | 148 | return() 149 | #------------------------------------- 150 | 151 | # If no AVX support is found, print an error message. 152 | if(AVX_FIND_VERSION) 153 | set(_AVX_ERROR_MESSAGE "AVX ${AVX_FIND_VERSION} support is not found on this architecture") 154 | else() 155 | set(_AVX_ERROR_MESSAGE "AVX support is not found on this architecture") 156 | endif() 157 | 158 | if(AVX_FIND_REQUIRED) 159 | message(FATAL_ERROR "${_AVX_ERROR_MESSAGE}") 160 | elseif(NOT AVX_FIND_QUIETLY) 161 | message(STATUS "${_AVX_ERROR_MESSAGE}") 162 | endif() 163 | -------------------------------------------------------------------------------- /cmake/recipes/CPM.cmake: -------------------------------------------------------------------------------- 1 | set(CPM_DOWNLOAD_VERSION 0.38.1) 2 | 3 | if(CPM_SOURCE_CACHE) 4 | set(CPM_DOWNLOAD_LOCATION "${CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake") 5 | elseif(DEFINED ENV{CPM_SOURCE_CACHE}) 6 | set(CPM_DOWNLOAD_LOCATION "$ENV{CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake") 7 | else() 8 | set(CPM_DOWNLOAD_LOCATION "${CMAKE_BINARY_DIR}/cmake/CPM_${CPM_DOWNLOAD_VERSION}.cmake") 9 | endif() 10 | 11 | # Expand relative path. This is important if the provided path contains a tilde (~) 12 | get_filename_component(CPM_DOWNLOAD_LOCATION ${CPM_DOWNLOAD_LOCATION} ABSOLUTE) 13 | 14 | function(download_cpm) 15 | message(STATUS "Downloading CPM.cmake to ${CPM_DOWNLOAD_LOCATION}") 16 | file(DOWNLOAD 17 | https://github.com/cpm-cmake/CPM.cmake/releases/download/v${CPM_DOWNLOAD_VERSION}/CPM.cmake 18 | ${CPM_DOWNLOAD_LOCATION} 19 | ) 20 | endfunction() 21 | 22 | if(NOT (EXISTS ${CPM_DOWNLOAD_LOCATION})) 23 | download_cpm() 24 | else() 25 | # resume download if it previously failed 26 | file(READ ${CPM_DOWNLOAD_LOCATION} check) 27 | if("${check}" STREQUAL "") 28 | download_cpm() 29 | endif() 30 | unset(check) 31 | endif() 32 | 33 | include(${CPM_DOWNLOAD_LOCATION}) 34 | -------------------------------------------------------------------------------- /cmake/recipes/ccd_query_io.cmake: -------------------------------------------------------------------------------- 1 | if(TARGET ccd_io::ccd_io) 2 | return() 3 | endif() 4 | 5 | message(STATUS "Third-party: creating target 'ccd_io::ccd_io'") 6 | 7 | set(CCD_IO_DOWNLOAD_SAMPLE_QUERIES ON CACHE BOOL "Download sample CCD queries" FORCE) 8 | set(CCD_IO_SAMPLE_QUERIES_DIR "${PROJECT_SOURCE_DIR}/sample-queries/" CACHE PATH "Where should we download sample queries?") 9 | 10 | include(CPM) 11 | CPMAddPackage("gh:Continuous-Collision-Detection/CCD-Query-IO#efca80cda21d95d74a1477ed22d42db8aabb5835") -------------------------------------------------------------------------------- /cmake/recipes/cli11.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 2020 Adobe. All rights reserved. 3 | # This file is licensed to you under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. You may obtain a copy 5 | # of the License at http://www.apache.org/licenses/LICENSE-2.0 6 | # 7 | # Unless required by applicable law or agreed to in writing, software distributed under 8 | # the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR REPRESENTATIONS 9 | # OF ANY KIND, either express or implied. See the License for the specific language 10 | # governing permissions and limitations under the License. 11 | # 12 | if(TARGET CLI11::CLI11) 13 | return() 14 | endif() 15 | 16 | message(STATUS "Third-party: creating target 'CLI11::CLI11'") 17 | 18 | include(CPM) 19 | CPMAddPackage("gh:CLIUtils/CLI11@2.3.2") -------------------------------------------------------------------------------- /cmake/recipes/eigen.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 2020 Adobe. All rights reserved. 3 | # This file is licensed to you under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. You may obtain a copy 5 | # of the License at http://www.apache.org/licenses/LICENSE-2.0 6 | # 7 | # Unless required by applicable law or agreed to in writing, software distributed under 8 | # the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR REPRESENTATIONS 9 | # OF ANY KIND, either express or implied. See the License for the specific language 10 | # governing permissions and limitations under the License. 11 | # 12 | if(TARGET Eigen3::Eigen) 13 | return() 14 | endif() 15 | 16 | option(EIGEN_WITH_MKL "Use Eigen with MKL" OFF) 17 | option(EIGEN_DONT_VECTORIZE "Disable Eigen vectorization" OFF) 18 | option(EIGEN_MPL2_ONLY "Enable Eigen MPL2 license only" OFF) 19 | 20 | message(STATUS "Third-party: creating target 'Eigen3::Eigen'") 21 | 22 | include(CPM) 23 | CPMAddPackage( 24 | NAME eigen 25 | GITLAB_REPOSITORY libeigen/eigen 26 | GIT_TAG 3.4.0 27 | DOWNLOAD_ONLY ON 28 | ) 29 | 30 | add_library(Eigen3_Eigen INTERFACE) 31 | add_library(Eigen3::Eigen ALIAS Eigen3_Eigen) 32 | 33 | include(GNUInstallDirs) 34 | target_include_directories(Eigen3_Eigen SYSTEM INTERFACE 35 | $ 36 | $ 37 | ) 38 | 39 | if(EIGEN_MPL2_ONLY) 40 | target_compile_definitions(Eigen3_Eigen INTERFACE EIGEN_MPL2_ONLY) 41 | endif() 42 | 43 | if(EIGEN_DONT_VECTORIZE) 44 | target_compile_definitions(Eigen3_Eigen INTERFACE EIGEN_DONT_VECTORIZE) 45 | endif() 46 | 47 | if(EIGEN_WITH_MKL) 48 | # TODO: Checks that, on 64bits systems, `mkl::mkl` is using the LP64 interface 49 | # (by looking at the compile definition of the target) 50 | include(mkl) 51 | target_link_libraries(Eigen3_Eigen INTERFACE mkl::mkl) 52 | target_compile_definitions(Eigen3_Eigen INTERFACE 53 | EIGEN_USE_MKL_ALL 54 | EIGEN_USE_LAPACKE_STRICT 55 | ) 56 | endif() 57 | 58 | # On Windows, enable natvis files to improve debugging experience 59 | if(WIN32 AND eigen_SOURCE_DIR) 60 | target_sources(Eigen3_Eigen INTERFACE $) 61 | endif() 62 | 63 | # Install rules 64 | set(CMAKE_INSTALL_DEFAULT_COMPONENT_NAME eigen) 65 | set_target_properties(Eigen3_Eigen PROPERTIES EXPORT_NAME Eigen) 66 | install(DIRECTORY ${eigen_SOURCE_DIR} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) 67 | install(TARGETS Eigen3_Eigen EXPORT Eigen_Targets) 68 | install(EXPORT Eigen_Targets DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/eigen NAMESPACE Eigen3::) -------------------------------------------------------------------------------- /cmake/recipes/pbar.cmake: -------------------------------------------------------------------------------- 1 | if(TARGET pbar::pbar) 2 | return() 3 | endif() 4 | 5 | message(STATUS "Third-party: creating target 'pbar::pbar'") 6 | 7 | include(CPM) 8 | CPMAddPackage( 9 | NAME pbar 10 | GITHUB_REPOSITORY estshorter/pbar 11 | GIT_TAG 226b9ad291d72c6859456d5188f33c932e2e730b 12 | DOWNLOAD_ONLY ON 13 | ) 14 | 15 | add_library(pbar INTERFACE) 16 | add_library(pbar::pbar ALIAS pbar) 17 | 18 | target_include_directories(pbar INTERFACE "${pbar_SOURCE_DIR}") -------------------------------------------------------------------------------- /cmake/recipes/rational_cpp.cmake: -------------------------------------------------------------------------------- 1 | if(TARGET rational::rational) 2 | return() 3 | endif() 4 | 5 | message(STATUS "Third-party: creating target 'rational::rational'") 6 | 7 | include(CPM) 8 | CPMAddPackage("gh:zfergus/rational-cpp#1e91438315389db5987732f464c36614b31bcadf") -------------------------------------------------------------------------------- /cmake/recipes/spdlog.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 2020 Adobe. All rights reserved. 3 | # This file is licensed to you under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. You may obtain a copy 5 | # of the License at http://www.apache.org/licenses/LICENSE-2.0 6 | # 7 | # Unless required by applicable law or agreed to in writing, software distributed under 8 | # the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR REPRESENTATIONS 9 | # OF ANY KIND, either express or implied. See the License for the specific language 10 | # governing permissions and limitations under the License. 11 | # 12 | if(TARGET spdlog::spdlog) 13 | return() 14 | endif() 15 | 16 | message(STATUS "Third-party: creating target 'spdlog::spdlog'") 17 | 18 | option(SPDLOG_INSTALL "Generate the install target" ON) 19 | set(CMAKE_INSTALL_DEFAULT_COMPONENT_NAME "spdlog") 20 | 21 | include(CPM) 22 | CPMAddPackage("gh:gabime/spdlog@1.11.0") 23 | 24 | set_target_properties(spdlog PROPERTIES POSITION_INDEPENDENT_CODE ON) 25 | 26 | set_target_properties(spdlog PROPERTIES FOLDER external) 27 | 28 | if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "AppleClang" OR 29 | "${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") 30 | target_compile_options(spdlog PRIVATE 31 | "-Wno-sign-conversion" 32 | ) 33 | endif() 34 | -------------------------------------------------------------------------------- /cmake/tight_inclusion/tight_inclusion_cpm_cache.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 2021 Adobe. All rights reserved. 3 | # This file is licensed to you under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. You may obtain a copy 5 | # of the License at http://www.apache.org/licenses/LICENSE-2.0 6 | # 7 | # Unless required by applicable law or agreed to in writing, software distributed under 8 | # the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR REPRESENTATIONS 9 | # OF ANY KIND, either express or implied. See the License for the specific language 10 | # governing permissions and limitations under the License. 11 | # 12 | 13 | if(DEFINED ENV{CPM_SOURCE_CACHE}) 14 | set(CPM_SOURCE_CACHE_DEFAULT $ENV{CPM_SOURCE_CACHE}) 15 | else() 16 | # Set CPM cache folder if unset 17 | file(REAL_PATH "~/.cache/CPM" CPM_SOURCE_CACHE_DEFAULT EXPAND_TILDE) 18 | endif() 19 | 20 | set(CPM_SOURCE_CACHE 21 | ${CPM_SOURCE_CACHE_DEFAULT} 22 | CACHE PATH "Directory to download CPM dependencies" 23 | ) 24 | message(STATUS "Using CPM cache folder: ${CPM_SOURCE_CACHE}") -------------------------------------------------------------------------------- /cmake/tight_inclusion/tight_inclusion_filter_flags.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 2021 Adobe. All rights reserved. 3 | # This file is licensed to you under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. You may obtain a copy 5 | # of the License at http://www.apache.org/licenses/LICENSE-2.0 6 | # 7 | # Unless required by applicable law or agreed to in writing, software distributed under 8 | # the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR REPRESENTATIONS 9 | # OF ANY KIND, either express or implied. See the License for the specific language 10 | # governing permissions and limitations under the License. 11 | # 12 | function(tight_inclusion_filter_flags flags) 13 | include(CheckCXXCompilerFlag) 14 | set(output_flags) 15 | foreach(FLAG IN ITEMS ${${flags}}) 16 | string(REPLACE "=" "-" FLAG_VAR "${FLAG}") 17 | if(NOT DEFINED IS_SUPPORTED_${FLAG_VAR}) 18 | check_cxx_compiler_flag("${FLAG}" IS_SUPPORTED_${FLAG_VAR}) 19 | endif() 20 | if(IS_SUPPORTED_${FLAG_VAR}) 21 | list(APPEND output_flags $<$:${FLAG}>) 22 | endif() 23 | endforeach() 24 | set(${flags} ${output_flags} PARENT_SCOPE) 25 | endfunction() -------------------------------------------------------------------------------- /cmake/tight_inclusion/tight_inclusion_use_colors.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 2020 Adobe. All rights reserved. 3 | # This file is licensed to you under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. You may obtain a copy 5 | # of the License at http://www.apache.org/licenses/LICENSE-2.0 6 | # 7 | # Unless required by applicable law or agreed to in writing, software distributed under 8 | # the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR REPRESENTATIONS 9 | # OF ANY KIND, either express or implied. See the License for the specific language 10 | # governing permissions and limitations under the License. 11 | # 12 | include_guard(GLOBAL) 13 | 14 | # options 15 | if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC") 16 | # Reduce the warning level of external files to the selected value (W1 - only major). 17 | # Requires Visual Studio 2017 version 15.7 18 | # https://blogs.msdn.microsoft.com/vcblog/2017/12/13/broken-warnings-theory/ 19 | 20 | # There is an issue in using these flags in earlier versions of MSVC: 21 | # https://developercommunity.visualstudio.com/content/problem/220812/experimentalexternal-generates-a-lot-of-c4193-warn.html 22 | if(MSVC_VERSION GREATER 1920) 23 | add_compile_options(/experimental:external) 24 | add_compile_options(/external:W1) 25 | endif() 26 | 27 | # When building in parallel, MSVC sometimes fails with the following error: 28 | # > fatal error C1090: PDB API call failed, error code '23' 29 | # To avoid this problem, we force PDB write to be synchronous with /FS. 30 | # https://developercommunity.visualstudio.com/content/problem/48897/c1090-pdb-api-call-failed-error-code-23.html 31 | add_compile_options(/FS) 32 | else() 33 | include(tight_inclusion_filter_flags) 34 | set(TIGHT_INCLUSION_GLOBAL_FLAGS 35 | -fdiagnostics-color=always # GCC 36 | -fcolor-diagnostics # Clang 37 | ) 38 | tight_inclusion_filter_flags(TIGHT_INCLUSION_GLOBAL_FLAGS) 39 | message(STATUS "Adding global flags: ${TIGHT_INCLUSION_GLOBAL_FLAGS}") 40 | add_compile_options(${TIGHT_INCLUSION_GLOBAL_FLAGS}) 41 | endif() 42 | -------------------------------------------------------------------------------- /cmake/tight_inclusion/tight_inclusion_warnings.cmake: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # See comments and discussions here: 3 | # http://stackoverflow.com/questions/5088460/flags-to-enable-thorough-and-verbose-g-warnings 4 | ################################################################################ 5 | 6 | if(TARGET tight_inclusion::warnings) 7 | return() 8 | endif() 9 | 10 | set(TIGHT_INCLUSION_WARNING_FLAGS 11 | -Wall 12 | -Wextra 13 | -pedantic 14 | 15 | # -Wconversion 16 | #-Wunsafe-loop-optimizations # broken with C++11 loops 17 | -Wunused 18 | 19 | -Wno-long-long 20 | -Wpointer-arith 21 | -Wformat=2 22 | -Wno-maybe-uninitialized 23 | -Wuninitialized 24 | -Wcast-qual 25 | -Wmissing-noreturn 26 | -Wmissing-format-attribute 27 | -Wredundant-decls 28 | 29 | -Werror=implicit 30 | -Werror=nonnull 31 | -Werror=init-self 32 | -Werror=main 33 | -Werror=missing-braces 34 | -Werror=sequence-point 35 | -Werror=return-type 36 | -Werror=trigraphs 37 | -Werror=array-bounds 38 | -Werror=write-strings 39 | -Werror=address 40 | -Werror=int-to-pointer-cast 41 | -Werror=pointer-to-int-cast 42 | 43 | -Wno-unused-variable 44 | -Wno-unused-but-set-variable 45 | -Wno-unused-parameter 46 | 47 | #-Weffc++ 48 | -Wno-old-style-cast 49 | # -Wno-sign-conversion 50 | #-Wsign-conversion 51 | 52 | -Wshadow 53 | 54 | -Wstrict-null-sentinel 55 | -Woverloaded-virtual 56 | -Wsign-promo 57 | -Wstack-protector 58 | -Wstrict-aliasing 59 | -Wstrict-aliasing=2 60 | 61 | # Warn whenever a switch statement has an index of enumerated type and 62 | # lacks a case for one or more of the named codes of that enumeration. 63 | -Wswitch 64 | # This is annoying if all cases are already covered. 65 | # -Wswitch-default 66 | # This is annoying if there is a default that covers the rest. 67 | # -Wswitch-enum 68 | -Wswitch-unreachable 69 | # -Wcovered-switch-default # Annoying warnings from nlohmann::json 70 | 71 | -Wcast-align 72 | -Wdisabled-optimization 73 | #-Winline # produces warning on default implicit destructor 74 | -Winvalid-pch 75 | # -Wmissing-include-dirs 76 | -Wpacked 77 | -Wno-padded 78 | -Wstrict-overflow 79 | -Wstrict-overflow=2 80 | 81 | -Wctor-dtor-privacy 82 | -Wlogical-op 83 | -Wnoexcept 84 | -Woverloaded-virtual 85 | # -Wundef 86 | 87 | -Wnon-virtual-dtor 88 | -Wdelete-non-virtual-dtor 89 | -Werror=non-virtual-dtor 90 | -Werror=delete-non-virtual-dtor 91 | 92 | -Wno-sign-compare 93 | 94 | ########### 95 | # GCC 6.1 # 96 | ########### 97 | 98 | -Wnull-dereference 99 | -fdelete-null-pointer-checks 100 | -Wduplicated-cond 101 | -Wmisleading-indentation 102 | 103 | #-Weverything 104 | 105 | ########################### 106 | # Enabled by -Weverything # 107 | ########################### 108 | 109 | #-Wdocumentation 110 | #-Wdocumentation-unknown-command 111 | #-Wfloat-equal 112 | 113 | #-Wglobal-constructors 114 | #-Wexit-time-destructors 115 | #-Wmissing-variable-declarations 116 | #-Wextra-semi 117 | #-Wweak-vtables 118 | #-Wno-source-uses-openmp 119 | #-Wdeprecated 120 | #-Wnewline-eof 121 | #-Wmissing-prototypes 122 | 123 | #-Wno-c++98-compat 124 | #-Wno-c++98-compat-pedantic 125 | 126 | ################################################ 127 | # Need to check if those are still valid today # 128 | ################################################ 129 | 130 | #-Wimplicit-atomic-properties 131 | #-Wmissing-declarations 132 | #-Wmissing-prototypes 133 | #-Wstrict-selector-match 134 | #-Wundeclared-selector 135 | #-Wunreachable-code 136 | 137 | # Not a warning, but enable link-time-optimization 138 | # TODO: Check out modern CMake version of setting this flag 139 | # https://cmake.org/cmake/help/latest/module/CheckIPOSupported.html 140 | #-flto 141 | 142 | # Gives meaningful stack traces 143 | -fno-omit-frame-pointer 144 | -fno-optimize-sibling-calls 145 | ) 146 | 147 | # Flags above don't make sense for MSVC 148 | if(MSVC) 149 | set(TIGHT_INCLUSION_WARNING_FLAGS) 150 | endif() 151 | 152 | add_library(tight_inclusion_warnings INTERFACE) 153 | add_library(tight_inclusion::warnings ALIAS tight_inclusion_warnings) 154 | 155 | include(tight_inclusion_filter_flags) 156 | tight_inclusion_filter_flags(TIGHT_INCLUSION_WARNING_FLAGS) 157 | target_compile_options(tight_inclusion_warnings INTERFACE ${TIGHT_INCLUSION_WARNING_FLAGS}) -------------------------------------------------------------------------------- /src/tight_inclusion/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(SOURCES 2 | avx.hpp 3 | avx.cpp 4 | ccd.cpp 5 | ccd.hpp 6 | interval_root_finder.cpp 7 | interval_root_finder.hpp 8 | interval.cpp 9 | interval.hpp 10 | logger.cpp 11 | logger.hpp 12 | timer.hpp 13 | types.hpp 14 | ) 15 | 16 | source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "Source Files" FILES ${SOURCES}) 17 | target_sources(tight_inclusion PRIVATE ${SOURCES}) 18 | 19 | ################################################################################ 20 | # Subfolders 21 | ################################################################################ 22 | 23 | if(TIGHT_INCLUSION_WITH_RATIONAL) 24 | add_subdirectory(rational) 25 | endif() 26 | -------------------------------------------------------------------------------- /src/tight_inclusion/avx.cpp: -------------------------------------------------------------------------------- 1 | #include "avx.hpp" 2 | 3 | //#include 4 | 5 | namespace ticcd { 6 | 7 | // __m512d function_f_ee_vector( 8 | // __m512d ea0_t0, 9 | // __m512d ea1_t0, 10 | // __m512d eb0_t0, 11 | // __m512d eb1_t0, 12 | // __m512d ea0_t1, 13 | // __m512d ea1_t1, 14 | // __m512d eb0_t1, 15 | // __m512d eb1_t1, 16 | // __m512d t_up, 17 | // __m512d t_dw, 18 | // __m512d u_up, 19 | // __m512d u_dw, 20 | // __m512d v_up, 21 | // __m512d v_dw) 22 | // { 23 | 24 | // __m512d ea0 = _mm512_sub_pd(ea0_t1, ea0_t0); 25 | // ea0 = _mm512_mul_pd(ea0, t_up); 26 | // ea0 = _mm512_div_pd(ea0, t_dw); 27 | // ea0 = _mm512_add_pd(ea0, ea0_t0); 28 | 29 | // __m512d ea1 = _mm512_sub_pd(ea1_t1, ea1_t0); 30 | // ea1 = _mm512_mul_pd(ea1, t_up); 31 | // ea1 = _mm512_div_pd(ea1, t_dw); 32 | // ea1 = _mm512_add_pd(ea1, ea1_t0); 33 | 34 | // __m512d eb0 = _mm512_sub_pd(eb0_t1, eb0_t0); 35 | // eb0 = _mm512_mul_pd(eb0, t_up); 36 | // eb0 = _mm512_div_pd(eb0, t_dw); 37 | // eb0 = _mm512_add_pd(eb0, eb0_t0); 38 | 39 | // __m512d eb1 = _mm512_sub_pd(eb1_t1, eb1_t0); 40 | // eb1 = _mm512_mul_pd(eb1, t_up); 41 | // eb1 = _mm512_div_pd(eb1, t_dw); 42 | // eb1 = _mm512_add_pd(eb1, eb1_t0); 43 | 44 | // __m512d va = _mm512_sub_pd(ea1, ea0); 45 | // va = _mm512_mul_pd(va, u_up); 46 | // va = _mm512_div_pd(va, u_dw); 47 | // va = _mm512_add_pd(va, ea0); 48 | 49 | // __m512d vb = _mm512_sub_pd(eb1, eb0); 50 | // vb = _mm512_mul_pd(vb, v_up); 51 | // vb = _mm512_div_pd(vb, v_dw); 52 | // vb = _mm512_add_pd(vb, eb0); 53 | 54 | // return _mm512_sub_pd(va, vb); 55 | // } 56 | 57 | std::array function_ee( 58 | const Scalar &ea0_t0, 59 | const Scalar &ea1_t0, 60 | const Scalar &eb0_t0, 61 | const Scalar &eb1_t0, 62 | const Scalar &ea0_t1, 63 | const Scalar &ea1_t1, 64 | const Scalar &eb0_t1, 65 | const Scalar &eb1_t1, 66 | const std::array &t_up, 67 | const std::array &t_dw, 68 | const std::array &u_up, 69 | const std::array &u_dw, 70 | const std::array &v_up, 71 | const std::array &v_dw) 72 | { 73 | std::array rst; 74 | for (int i = 0; i < 8; i++) { 75 | const Scalar ea0 = (ea0_t1 - ea0_t0) * t_up[i] / t_dw[i] + ea0_t0; 76 | const Scalar ea1 = (ea1_t1 - ea1_t0) * t_up[i] / t_dw[i] + ea1_t0; 77 | const Scalar eb0 = (eb0_t1 - eb0_t0) * t_up[i] / t_dw[i] + eb0_t0; 78 | const Scalar eb1 = (eb1_t1 - eb1_t0) * t_up[i] / t_dw[i] + eb1_t0; 79 | 80 | const Scalar va = (ea1 - ea0) * u_up[i] / u_dw[i] + ea0; 81 | const Scalar vb = (eb1 - eb0) * v_up[i] / v_dw[i] + eb0; 82 | rst[i] = va - vb; 83 | } 84 | return rst; 85 | } 86 | 87 | std::array function_vf( 88 | const Scalar &v_t0, 89 | const Scalar &f0_t0, 90 | const Scalar &f1_t0, 91 | const Scalar &f2_t0, 92 | const Scalar &v_t1, 93 | const Scalar &f0_t1, 94 | const Scalar &f1_t1, 95 | const Scalar &f2_t1, 96 | const std::array &t_up, 97 | const std::array &t_dw, 98 | const std::array &u_up, 99 | const std::array &u_dw, 100 | const std::array &v_up, 101 | const std::array &v_dw) 102 | { 103 | std::array rst; 104 | for (int i = 0; i < 8; i++) { 105 | const Scalar v = (v_t1 - v_t0) * t_up[i] / t_dw[i] + v_t0; 106 | const Scalar f0 = (f0_t1 - f0_t0) * t_up[i] / t_dw[i] + f0_t0; 107 | const Scalar f1 = (f1_t1 - f1_t0) * t_up[i] / t_dw[i] + f1_t0; 108 | const Scalar f2 = (f2_t1 - f2_t0) * t_up[i] / t_dw[i] + f2_t0; 109 | const Scalar pt = (f1 - f0) * u_up[i] / u_dw[i] 110 | + (f2 - f0) * v_up[i] / v_dw[i] + f0; 111 | rst[i] = v - pt; 112 | } 113 | return rst; 114 | } 115 | 116 | void convert_tuv_to_array( 117 | const Interval3 &itv, 118 | std::array &t_up, 119 | std::array &t_dw, 120 | std::array &u_up, 121 | std::array &u_dw, 122 | std::array &v_up, 123 | std::array &v_dw) 124 | { 125 | // t order: 0,0,0,0,1,1,1,1 126 | // u order: 0,0,1,1,0,0,1,1 127 | // v order: 0,1,0,1,0,1,0,1 128 | const Scalar t0_up = itv[0].lower.numerator; 129 | const Scalar t0_dw = itv[0].lower.denominator(); 130 | const Scalar t1_up = itv[0].upper.numerator; 131 | const Scalar t1_dw = itv[0].upper.denominator(); 132 | const Scalar u0_up = itv[1].lower.numerator; 133 | const Scalar u0_dw = itv[1].lower.denominator(); 134 | const Scalar u1_up = itv[1].upper.numerator; 135 | const Scalar u1_dw = itv[1].upper.denominator(); 136 | const Scalar v0_up = itv[2].lower.numerator; 137 | const Scalar v0_dw = itv[2].lower.denominator(); 138 | const Scalar v1_up = itv[2].upper.numerator; 139 | const Scalar v1_dw = itv[2].upper.denominator(); 140 | t_up = {{t0_up, t0_up, t0_up, t0_up, t1_up, t1_up, t1_up, t1_up}}; 141 | t_dw = {{t0_dw, t0_dw, t0_dw, t0_dw, t1_dw, t1_dw, t1_dw, t1_dw}}; 142 | u_up = {{u0_up, u0_up, u1_up, u1_up, u0_up, u0_up, u1_up, u1_up}}; 143 | u_dw = {{u0_dw, u0_dw, u1_dw, u1_dw, u0_dw, u0_dw, u1_dw, u1_dw}}; 144 | v_up = {{v0_up, v1_up, v0_up, v1_up, v0_up, v1_up, v0_up, v1_up}}; 145 | v_dw = {{v0_dw, v1_dw, v0_dw, v1_dw, v0_dw, v1_dw, v0_dw, v1_dw}}; 146 | } 147 | 148 | // __m512d function_f_vf_vector( 149 | // __m512d v_t0, 150 | // __m512d t0_t0, 151 | // __m512d t1_t0, 152 | // __m512d t2_t0, 153 | // __m512d v_t1, 154 | // __m512d t0_t1, 155 | // __m512d t1_t1, 156 | // __m512d t2_t1, 157 | // __m512d t_up, 158 | // __m512d t_dw, 159 | // __m512d u_up, 160 | // __m512d u_dw, 161 | // __m512d v_up, 162 | // __m512d v_dw) 163 | // { 164 | // __m512d v = _mm512_sub_pd(v_t1, v_t0); 165 | // v = _mm512_mul_pd(v, t_up); 166 | // v = _mm512_div_pd(v, t_dw); 167 | // v = _mm512_add_pd(v, v_t0); 168 | 169 | // __m512d t0 = _mm512_sub_pd(f0_t1, f0_t0); 170 | // t0 = _mm512_mul_pd(t0, t_up); 171 | // t0 = _mm512_div_pd(t0, t_dw); 172 | // t0 = _mm512_add_pd(t0, f0_t0); 173 | 174 | // __m512d t1 = _mm512_sub_pd(f1_t1, f1_t0); 175 | // t1 = _mm512_mul_pd(t1, t_up); 176 | // t1 = _mm512_div_pd(t1, t_dw); 177 | // t1 = _mm512_add_pd(t1, f1_t0); 178 | 179 | // __m512d t2 = _mm512_sub_pd(f2_t1, f2_t0); 180 | // t2 = _mm512_mul_pd(t2, t_up); 181 | // t2 = _mm512_div_pd(t2, t_dw); 182 | // t2 = _mm512_add_pd(t2, f2_t0); 183 | 184 | // __m512d t01 = _mm512_sub_pd(t1, t0); 185 | // t01 = _mm512_mul_pd(t01, u_up); 186 | // t01 = _mm512_div_pd(t01, u_dw); 187 | 188 | // __m512d t02 = _mm512_sub_pd(t2, t0); 189 | // t02 = _mm512_mul_pd(t02, v_up); 190 | // t02 = _mm512_div_pd(t02, v_dw); 191 | 192 | // __m512d pt = _mm512_add_pd(t01, t02); 193 | // pt = _mm512_add_pd(pt, t0); 194 | 195 | // return _mm512_sub_pd(v, pt); 196 | // } 197 | 198 | // void convert_to_vector_pts( 199 | // const Scalar asd, 200 | // const Scalar bsd, 201 | // const Scalar csd, 202 | // const Scalar dsd, 203 | // const Scalar aed, 204 | // const Scalar bed, 205 | // const Scalar ced, 206 | // const Scalar ded, 207 | 208 | // __m512d &as, 209 | // __m512d &bs, 210 | // __m512d &cs, 211 | // __m512d &ds, 212 | // __m512d &ae, 213 | // __m512d &be, 214 | // __m512d &ce, 215 | // __m512d &de 216 | 217 | // ) 218 | // { 219 | // as = _mm512_setr_pd(asd, asd, asd, asd, asd, asd, asd, asd); 220 | // bs = _mm512_setr_pd(bsd, bsd, bsd, bsd, bsd, bsd, bsd, bsd); 221 | // cs = _mm512_setr_pd(csd, csd, csd, csd, csd, csd, csd, csd); 222 | // ds = _mm512_setr_pd(dsd, dsd, dsd, dsd, dsd, dsd, dsd, dsd); 223 | 224 | // ae = _mm512_setr_pd(aed, aed, aed, aed, aed, aed, aed, aed); 225 | // be = _mm512_setr_pd(bed, bed, bed, bed, bed, bed, bed, bed); 226 | // ce = _mm512_setr_pd(ced, ced, ced, ced, ced, ced, ced, ced); 227 | // de = _mm512_setr_pd(ded, ded, ded, ded, ded, ded, ded, ded); 228 | // } 229 | // void convert_to_vector_pts_uvt( 230 | // const std::array &t_up, 231 | // const std::array &t_dw, 232 | // const std::array &u_up, 233 | // const std::array &u_dw, 234 | // const std::array &v_up, 235 | // const std::array &v_dw, 236 | 237 | // __m512d &tu, 238 | // __m512d &td, 239 | // __m512d &uu, 240 | // __m512d &ud, 241 | // __m512d &vu, 242 | // __m512d &vd) 243 | // { 244 | // tu = _mm512_setr_pd( 245 | // t_up[0], t_up[1], t_up[2], t_up[3], t_up[4], t_up[5], t_up[6], 246 | // t_up[7]); 247 | // td = _mm512_setr_pd( 248 | // t_dw[0], t_dw[1], t_dw[2], t_dw[3], t_dw[4], t_dw[5], t_dw[6], 249 | // t_dw[7]); 250 | 251 | // uu = _mm512_setr_pd( 252 | // u_up[0], u_up[1], u_up[2], u_up[3], u_up[4], u_up[5], u_up[6], 253 | // u_up[7]); 254 | // ud = _mm512_setr_pd( 255 | // u_dw[0], u_dw[1], u_dw[2], u_dw[3], u_dw[4], u_dw[5], u_dw[6], 256 | // u_dw[7]); 257 | 258 | // vu = _mm512_setr_pd( 259 | // v_up[0], v_up[1], v_up[2], v_up[3], v_up[4], v_up[5], v_up[6], 260 | // v_up[7]); 261 | // vd = _mm512_setr_pd( 262 | // v_dw[0], v_dw[1], v_dw[2], v_dw[3], v_dw[4], v_dw[5], v_dw[6], 263 | // v_dw[7]); 264 | // } 265 | // //(a-b)/c 266 | // __m512d function_test(__m512d a, __m512d b, __m512d c) 267 | // { 268 | // __m512d v = _mm512_sub_pd(a, b); 269 | // v = _mm512_div_pd(v, c); 270 | // return v; 271 | // } 272 | 273 | } // namespace ticcd 274 | -------------------------------------------------------------------------------- /src/tight_inclusion/avx.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace ticcd { 8 | void convert_tuv_to_array( 9 | const Interval3 &itv, 10 | std::array &t_up, 11 | std::array &t_dw, 12 | std::array &u_up, 13 | std::array &u_dw, 14 | std::array &v_up, 15 | std::array &v_dw); 16 | 17 | std::array function_vf( 18 | const Scalar &v_t0, 19 | const Scalar &f0_t0, 20 | const Scalar &f1_t0, 21 | const Scalar &f2_t0, 22 | const Scalar &v_t1, 23 | const Scalar &f0_t1, 24 | const Scalar &f1_t1, 25 | const Scalar &f2_t1, 26 | const std::array &t_up, 27 | const std::array &t_dw, 28 | const std::array &u_up, 29 | const std::array &u_dw, 30 | const std::array &v_up, 31 | const std::array &v_dw); 32 | 33 | std::array function_ee( 34 | const Scalar &ea0_t0, 35 | const Scalar &ea1_t0, 36 | const Scalar &eb0_t0, 37 | const Scalar &eb1_t0, 38 | const Scalar &ea0_t1, 39 | const Scalar &ea1_t1, 40 | const Scalar &eb0_t1, 41 | const Scalar &eb1_t1, 42 | const std::array &t_up, 43 | const std::array &t_dw, 44 | const std::array &u_up, 45 | const std::array &u_dw, 46 | const std::array &v_up, 47 | const std::array &v_dw); 48 | } // namespace ticcd 49 | -------------------------------------------------------------------------------- /src/tight_inclusion/ccd.cpp: -------------------------------------------------------------------------------- 1 | #include "ccd.hpp" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace ticcd { 10 | 11 | #ifdef TIGHT_INCLUSION_USE_MAX_ABS_TOL 12 | static constexpr Scalar CCD_MAX_TIME_TOL = 1e-3; 13 | static constexpr Scalar CCD_MAX_COORD_TOL = 1e-2; 14 | #else 15 | static constexpr Scalar CCD_MAX_TIME_TOL = 16 | std::numeric_limits::infinity(); 17 | static constexpr Scalar CCD_MAX_COORD_TOL = 18 | std::numeric_limits::infinity(); 19 | #endif 20 | 21 | inline std::array bbd_4_pts( 22 | const Vector3 &p0, 23 | const Vector3 &p1, 24 | const Vector3 &p2, 25 | const Vector3 &p3) 26 | { 27 | return { 28 | {p0.cwiseMin(p1).cwiseMin(p2).cwiseMin(p3), 29 | p0.cwiseMax(p1).cwiseMax(p2).cwiseMax(p3)}}; 30 | } 31 | 32 | // calculate maximum x, y and z diff 33 | Scalar get_max_axis_diff( 34 | const std::array &b1, const std::array &b2) 35 | { 36 | return std::max({ 37 | (b1[1] - b1[0]).maxCoeff(), 38 | (b2[1] - b2[0]).maxCoeff(), 39 | (b2[0] - b1[1]).cwiseAbs().maxCoeff(), 40 | (b1[0] - b2[1]).cwiseAbs().maxCoeff(), 41 | }); 42 | } 43 | 44 | inline Scalar max_linf_4( 45 | const Vector3 &p1, 46 | const Vector3 &p2, 47 | const Vector3 &p3, 48 | const Vector3 &p4, 49 | const Vector3 &p1e, 50 | const Vector3 &p2e, 51 | const Vector3 &p3e, 52 | const Vector3 &p4e) 53 | { 54 | return std::max( 55 | {(p1e - p1).lpNorm(), 56 | (p2e - p2).lpNorm(), 57 | (p3e - p3).lpNorm(), 58 | (p4e - p4).lpNorm()}); 59 | } 60 | 61 | /// @brief Clamp a/b to [-∞, max_val] 62 | /// @param a numerator 63 | /// @param b denominator 64 | /// @param max_val 65 | /// @return a/b if b != 0, max_val if b == 0 66 | inline Scalar 67 | clamp_div(const Scalar a, const Scalar b, const Scalar max_val) 68 | { 69 | if (b == 0) { 70 | return max_val; 71 | } else { 72 | return std::min(a / b, max_val); 73 | } 74 | } 75 | 76 | Array3 compute_vertex_face_tolerances( 77 | const Vector3 &v_t0, 78 | const Vector3 &f0_t0, 79 | const Vector3 &f1_t0, 80 | const Vector3 &f2_t0, 81 | const Vector3 &v_t1, 82 | const Vector3 &f0_t1, 83 | const Vector3 &f1_t1, 84 | const Vector3 &f2_t1, 85 | const Scalar distance_tolerance) 86 | { 87 | const Vector3 p000 = v_t0 - f0_t0; 88 | const Vector3 p001 = v_t0 - f2_t0; 89 | const Vector3 p011 = v_t0 - (f1_t0 + f2_t0 - f0_t0); 90 | const Vector3 p010 = v_t0 - f1_t0; 91 | const Vector3 p100 = v_t1 - f0_t1; 92 | const Vector3 p101 = v_t1 - f2_t1; 93 | const Vector3 p111 = v_t1 - (f1_t1 + f2_t1 - f0_t1); 94 | const Vector3 p110 = v_t1 - f1_t1; 95 | 96 | const Scalar dl = 97 | 3 * max_linf_4(p000, p001, p011, p010, p100, p101, p111, p110); 98 | const Scalar edge0_length = 99 | 3 * max_linf_4(p000, p100, p101, p001, p010, p110, p111, p011); 100 | const Scalar edge1_length = 101 | 3 * max_linf_4(p000, p100, p110, p010, p001, p101, p111, p011); 102 | 103 | return Array3( 104 | clamp_div(distance_tolerance, dl, CCD_MAX_TIME_TOL), 105 | clamp_div(distance_tolerance, edge0_length, CCD_MAX_COORD_TOL), 106 | clamp_div(distance_tolerance, edge1_length, CCD_MAX_COORD_TOL)); 107 | } 108 | 109 | Array3 compute_edge_edge_tolerances( 110 | const Vector3 &ea0_t0, 111 | const Vector3 &ea1_t0, 112 | const Vector3 &eb0_t0, 113 | const Vector3 &eb1_t0, 114 | const Vector3 &ea0_t1, 115 | const Vector3 &ea1_t1, 116 | const Vector3 &eb0_t1, 117 | const Vector3 &eb1_t1, 118 | const Scalar distance_tolerance) 119 | { 120 | 121 | const Vector3 p000 = ea0_t0 - eb0_t0; 122 | const Vector3 p001 = ea0_t0 - eb1_t0; 123 | const Vector3 p010 = ea1_t0 - eb0_t0; 124 | const Vector3 p011 = ea1_t0 - eb1_t0; 125 | const Vector3 p100 = ea0_t1 - eb0_t1; 126 | const Vector3 p101 = ea0_t1 - eb1_t1; 127 | const Vector3 p110 = ea1_t1 - eb0_t1; 128 | const Vector3 p111 = ea1_t1 - eb1_t1; 129 | 130 | const Scalar dl = 131 | 3 * max_linf_4(p000, p001, p011, p010, p100, p101, p111, p110); 132 | const Scalar edge0_length = 133 | 3 * max_linf_4(p000, p100, p101, p001, p010, p110, p111, p011); 134 | const Scalar edge1_length = 135 | 3 * max_linf_4(p000, p100, p110, p010, p001, p101, p111, p011); 136 | 137 | return Array3( 138 | clamp_div(distance_tolerance, dl, CCD_MAX_TIME_TOL), 139 | clamp_div(distance_tolerance, edge0_length, CCD_MAX_COORD_TOL), 140 | clamp_div(distance_tolerance, edge1_length, CCD_MAX_COORD_TOL)); 141 | } 142 | 143 | template 144 | bool 145 | CCD(const Vector3 &a_t0, 146 | const Vector3 &b_t0, 147 | const Vector3 &c_t0, 148 | const Vector3 &d_t0, 149 | const Vector3 &a_t1, 150 | const Vector3 &b_t1, 151 | const Vector3 &c_t1, 152 | const Vector3 &d_t1, 153 | const Array3 &err_in, 154 | const Scalar ms_in, 155 | Scalar &toi, 156 | const Scalar tolerance_in, 157 | const Scalar t_max_in, 158 | const long max_itr, 159 | Scalar &output_tolerance, 160 | bool no_zero_toi, 161 | const CCDRootFindingMethod ccd_method) 162 | { 163 | constexpr int MAX_NO_ZERO_TOI_ITER = std::numeric_limits::max(); 164 | // unsigned so can be larger than MAX_NO_ZERO_TOI_ITER 165 | unsigned int no_zero_toi_iter = 0; 166 | 167 | bool is_impacting, tmp_is_impacting; 168 | 169 | // Mutable copies for no_zero_toi 170 | Scalar t_max = t_max_in; 171 | Scalar tolerance = tolerance_in; 172 | Scalar ms = ms_in; 173 | 174 | Array3 tol; 175 | if constexpr (is_vertex_face) { 176 | tol = compute_vertex_face_tolerances( 177 | a_t0, b_t0, c_t0, d_t0, a_t1, b_t1, c_t1, d_t1, tolerance_in); 178 | } else { 179 | tol = compute_edge_edge_tolerances( 180 | a_t0, b_t0, c_t0, d_t0, a_t1, b_t1, c_t1, d_t1, tolerance_in); 181 | } 182 | 183 | ////////////////////////////////////////////////////////// 184 | // this should be the error of the whole mesh 185 | Array3 err; 186 | // if error[0] < 0, means we need to calculate error here 187 | if (err_in[0] < 0) { 188 | err = get_numerical_error( 189 | std::vector{ 190 | {a_t0, b_t0, c_t0, d_t0, a_t1, b_t1, c_t1, d_t1}}, 191 | is_vertex_face, ms > 0); 192 | } else { 193 | err = err_in; 194 | } 195 | ////////////////////////////////////////////////////////// 196 | 197 | do { 198 | switch (ccd_method) { 199 | case CCDRootFindingMethod::DEPTH_FIRST_SEARCH: 200 | // no handling for zero toi 201 | return interval_root_finder_DFS( 202 | a_t0, b_t0, c_t0, d_t0, a_t1, b_t1, c_t1, d_t1, tol, err, 203 | ms, toi); 204 | case CCDRootFindingMethod::BREADTH_FIRST_SEARCH: 205 | assert(t_max >= 0 && t_max <= 1); 206 | tmp_is_impacting = interval_root_finder_BFS( 207 | a_t0, b_t0, c_t0, d_t0, a_t1, b_t1, c_t1, d_t1, tol, 208 | tolerance, err, ms, t_max, max_itr, toi, output_tolerance); 209 | break; 210 | } 211 | assert(!tmp_is_impacting || toi >= 0); 212 | 213 | if (t_max == t_max_in) { 214 | // This will be the final output because we might need to 215 | // perform CCD again if the toi is zero. In which case we will 216 | // use a smaller t_max for more time resolution. 217 | is_impacting = tmp_is_impacting; 218 | } else { 219 | toi = tmp_is_impacting ? toi : t_max; 220 | } 221 | 222 | // This modification is for CCD-filtered line-search (e.g., IPC) 223 | // strategies for dealing with toi = 0: 224 | // 1. shrink t_max (when reaches max_itr), 225 | // 2. shrink tolerance (when not reach max_itr and tolerance is big) or 226 | // ms (when tolerance is too small comparing with ms) 227 | if (tmp_is_impacting && toi == 0 && no_zero_toi) { 228 | if (output_tolerance > tolerance) { 229 | // reaches max_itr, so shrink t_max to return a more accurate result to reach target tolerance. 230 | t_max *= 0.9; 231 | } else if (10 * tolerance < ms) { 232 | ms *= 0.5; // ms is too large, shrink it 233 | } else { 234 | tolerance *= 0.5; // tolerance is too large, shrink it 235 | 236 | if constexpr (is_vertex_face) { 237 | tol = compute_vertex_face_tolerances( 238 | a_t0, b_t0, c_t0, d_t0, a_t1, b_t1, c_t1, d_t1, 239 | tolerance); 240 | } else { 241 | tol = compute_edge_edge_tolerances( 242 | a_t0, b_t0, c_t0, d_t0, a_t1, b_t1, c_t1, d_t1, 243 | tolerance); 244 | } 245 | } 246 | } 247 | 248 | // Only perform a second iteration if toi == 0. 249 | // WARNING: This option assumes the initial distance is not zero. 250 | } while (no_zero_toi && ++no_zero_toi_iter < MAX_NO_ZERO_TOI_ITER 251 | && tmp_is_impacting && toi == 0); 252 | assert(!no_zero_toi || !is_impacting || toi != 0); 253 | 254 | return is_impacting; 255 | } 256 | 257 | bool edgeEdgeCCD( 258 | const Vector3 &ea0_t0, 259 | const Vector3 &ea1_t0, 260 | const Vector3 &eb0_t0, 261 | const Vector3 &eb1_t0, 262 | const Vector3 &ea0_t1, 263 | const Vector3 &ea1_t1, 264 | const Vector3 &eb0_t1, 265 | const Vector3 &eb1_t1, 266 | const Array3 &err_in, 267 | const Scalar ms_in, 268 | Scalar &toi, 269 | const Scalar tolerance_in, 270 | const Scalar t_max_in, 271 | const long max_itr, 272 | Scalar &output_tolerance, 273 | bool no_zero_toi, 274 | const CCDRootFindingMethod ccd_method) 275 | { 276 | return CCD( 277 | ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1, eb1_t1, 278 | err_in, ms_in, toi, tolerance_in, t_max_in, max_itr, 279 | output_tolerance, no_zero_toi, ccd_method); 280 | } 281 | 282 | bool vertexFaceCCD( 283 | const Vector3 &v_t0, 284 | const Vector3 &f0_t0, 285 | const Vector3 &f1_t0, 286 | const Vector3 &f2_t0, 287 | const Vector3 &v_t1, 288 | const Vector3 &f0_t1, 289 | const Vector3 &f1_t1, 290 | const Vector3 &f2_t1, 291 | const Array3 &err_in, 292 | const Scalar ms_in, 293 | Scalar &toi, 294 | const Scalar tolerance_in, 295 | const Scalar t_max_in, 296 | const long max_itr, 297 | Scalar &output_tolerance, 298 | bool no_zero_toi, 299 | const CCDRootFindingMethod ccd_method) 300 | { 301 | return CCD( 302 | v_t0, f0_t0, f1_t0, f2_t0, v_t1, f0_t1, f1_t1, f2_t1, err_in, ms_in, 303 | toi, tolerance_in, t_max_in, max_itr, output_tolerance, no_zero_toi, 304 | ccd_method); 305 | } 306 | 307 | #ifdef TIGHT_INCLUSION_FLOAT_WITH_DOUBLE_INPUT 308 | // these function are designed to test the performance of floating point vertion but with double inputs 309 | bool edgeEdgeCCD( 310 | const Eigen::Vector3d &ea0_t0, 311 | const Eigen::Vector3d &ea1_t0, 312 | const Eigen::Vector3d &eb0_t0, 313 | const Eigen::Vector3d &eb1_t0, 314 | const Eigen::Vector3d &ea0_t1, 315 | const Eigen::Vector3d &ea1_t1, 316 | const Eigen::Vector3d &eb0_t1, 317 | const Eigen::Vector3d &eb1_t1, 318 | const Eigen::Array3d &err, 319 | const double ms, 320 | double &toi, 321 | const double tolerance, 322 | const double t_max, 323 | const long max_itr, 324 | double &output_tolerance, 325 | bool no_zero_toi, 326 | const CCDRootFindingMethod ccd_method) 327 | { 328 | Scalar _toi = toi; 329 | Scalar _output_tolerance = output_tolerance; 330 | 331 | const bool result = edgeEdgeCCD( 332 | ea0_t0.cast(), ea1_t0.cast(), eb0_t0.cast(), 333 | eb1_t0.cast(), ea0_t1.cast(), ea1_t1.cast(), 334 | eb0_t1.cast(), eb1_t1.cast(), err.cast(), 335 | static_cast(ms), _toi, static_cast(tolerance), 336 | static_cast(t_max), max_itr, _output_tolerance, no_zero_toi, 337 | ccd_method); 338 | 339 | toi = _toi; 340 | output_tolerance = _output_tolerance; 341 | 342 | return result; 343 | } 344 | 345 | bool vertexFaceCCD( 346 | const Eigen::Vector3d &v_t0, 347 | const Eigen::Vector3d &f0_t0, 348 | const Eigen::Vector3d &f1_t0, 349 | const Eigen::Vector3d &f2_t0, 350 | const Eigen::Vector3d &v_t1, 351 | const Eigen::Vector3d &f0_t1, 352 | const Eigen::Vector3d &f1_t1, 353 | const Eigen::Vector3d &f2_t1, 354 | const Eigen::Array3d &err, 355 | const double ms, 356 | double &toi, 357 | const double tolerance, 358 | const double t_max, 359 | const long max_itr, 360 | double &output_tolerance, 361 | bool no_zero_toi, 362 | const CCDRootFindingMethod ccd_method) 363 | { 364 | Scalar _toi = toi; 365 | Scalar _output_tolerance = output_tolerance; 366 | 367 | const bool result = vertexFaceCCD( 368 | v_t0.cast(), f0_t0.cast(), f1_t0.cast(), 369 | f2_t0.cast(), v_t1.cast(), f0_t1.cast(), 370 | f1_t1.cast(), f2_t1.cast(), err.cast(), 371 | static_cast(ms), _toi, static_cast(tolerance), 372 | static_cast(t_max), max_itr, _output_tolerance, no_zero_toi, 373 | ccd_method); 374 | 375 | toi = _toi; 376 | output_tolerance = _output_tolerance; 377 | 378 | return result; 379 | } 380 | #endif 381 | } // namespace ticcd 382 | -------------------------------------------------------------------------------- /src/tight_inclusion/ccd.hpp: -------------------------------------------------------------------------------- 1 | // Time-of-impact computation for rigid bodies with angular trajectories. 2 | #pragma once 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace ticcd { 9 | static constexpr bool DEFAULT_NO_ZERO_TOI = false; 10 | static constexpr Scalar DEFAULT_CCD_DISTANCE_TOL = 1e-6; 11 | 12 | enum class CCDRootFindingMethod { 13 | DEPTH_FIRST_SEARCH, 14 | BREADTH_FIRST_SEARCH, 15 | }; 16 | 17 | /// @brief This function can give you the answer of continuous collision detection with minimum 18 | /// separation, and the earliest collision time if collision happens. 19 | /// 20 | /// @param[in] ea0_t0 The start position of the first vertex of the first edge. 21 | /// @param[in] ea1_t0 The start position of the second vertex of the first edge. 22 | /// @param[in] eb0_t0 The start position of the first vertex of the second edge. 23 | /// @param[in] eb1_t0 The start position of the second vertex of the second edge. 24 | /// @param[in] ea0_t1 The end position of the first vertex of the first edge. 25 | /// @param[in] ea1_t1 The end position of the second vertex of the first edge. 26 | /// @param[in] eb0_t1 The end position of the first vertex of the second edge. 27 | /// @param[in] eb1_t1 The end position of the second vertex of the second edge. 28 | /// @param[in] err The filters calculated using the bounding box of the simulation scene. 29 | /// If you are checking a single query without a scene, please set it as {-1,-1,-1}. 30 | /// @param[in] ms The minimum separation. should set: ms < max(abs(x),1), ms < max(abs(y),1), ms < max(abs(z),1) of the QUERY (NOT THE SCENE!). 31 | /// @param[out] toi The earliest time of collision if collision happens. If there is no collision, toi will be infinite. 32 | /// @param[in] tolerance A user - input solving precision. We suggest to use 1e-6. 33 | /// @param[in] t_max The upper bound of the time interval [0,t_max] to be checked. 0<=t_max<=1 34 | /// @param[in] max_itr A user-defined value to terminate the algorithm earlier, and return a result under current 35 | /// precision. please set max_itr either a big number like 1e7, or -1 which means it will not be terminated 36 | /// earlier and the precision will be user-defined precision -- tolerance. 37 | /// @param[out] output_tolerance The precision under max_itr ( > 0). if max_itr < 0, output_tolerance = tolerance; 38 | /// @param[in] no_zero_toi Refine further if a zero toi is produced (assumes not initially in contact). 39 | /// @return True if there is a collision, false otherwise. 40 | bool edgeEdgeCCD( 41 | const Vector3 &ea0_t0, 42 | const Vector3 &ea1_t0, 43 | const Vector3 &eb0_t0, 44 | const Vector3 &eb1_t0, 45 | const Vector3 &ea0_t1, 46 | const Vector3 &ea1_t1, 47 | const Vector3 &eb0_t1, 48 | const Vector3 &eb1_t1, 49 | const Array3 &err, 50 | const Scalar ms, 51 | Scalar &toi, 52 | const Scalar tolerance, 53 | const Scalar t_max, 54 | const long max_itr, 55 | Scalar &output_tolerance, 56 | bool no_zero_toi = DEFAULT_NO_ZERO_TOI, 57 | const CCDRootFindingMethod ccd_method = 58 | CCDRootFindingMethod::BREADTH_FIRST_SEARCH); 59 | 60 | /// This function can give you the answer of continuous collision detection with minimum 61 | /// separation, and the earliest collision time if collision happens. 62 | /// 63 | /// @param[in] v_t0 The start position of the vertex. 64 | /// @param[in] f0_t0 The start position of the first vertex of the face. 65 | /// @param[in] f1_t0 The start position of the second vertex of the face. 66 | /// @param[in] f2_t0 The start position of the third vertex of the face. 67 | /// @param[in] v_t1 The end position of the vertex. 68 | /// @param[in] f0_t1 The end position of the first vertex of the face. 69 | /// @param[in] f1_t1 The end position of the second vertex of the face. 70 | /// @param[in] f2_t1 The end position of the third vertex of the face. 71 | /// @param[in] err The filters calculated using the bounding box of the simulation scene. 72 | /// If you are checking a single query without a scene, please set it as {-1,-1,-1}. 73 | /// @param[in] ms The minimum separation. should set: ms < max(abs(x),1), ms < max(abs(y),1), ms < max(abs(z),1) of the QUERY (NOT THE SCENE!). 74 | /// @param[out] toi The earliest time of collision if collision happens. If there is no collision, toi will be infinite. 75 | /// @param[in] tolerance A user - input solving precision. We suggest to use 1e-6. 76 | /// @param[in] t_max The upper bound of the time interval [0,t_max] to be checked. 0<=t_max<=1 77 | /// @param[in] max_itr A user-defined value to terminate the algorithm earlier, and return a result under current 78 | /// precision. please set max_itr either a big number like 1e7, or -1 which means it will not be terminated 79 | /// earlier and the precision will be user-defined precision -- tolerance. 80 | /// @param[out] output_tolerance The precision under max_itr ( > 0). if max_itr < 0, output_tolerance = tolerance; 81 | /// @param[in] no_zero_toi Refine further if a zero toi is produced (assumes not initially in contact). 82 | /// @return True if there is a collision, false otherwise. 83 | bool vertexFaceCCD( 84 | const Vector3 &v_t0, 85 | const Vector3 &f0_t0, 86 | const Vector3 &f1_t0, 87 | const Vector3 &f2_t0, 88 | const Vector3 &v_t1, 89 | const Vector3 &f0_t1, 90 | const Vector3 &f1_t1, 91 | const Vector3 &f2_t1, 92 | const Array3 &err, 93 | const Scalar ms, 94 | Scalar &toi, 95 | const Scalar tolerance, 96 | const Scalar t_max, 97 | const long max_itr, 98 | Scalar &output_tolerance, 99 | bool no_zero_toi = DEFAULT_NO_ZERO_TOI, 100 | const CCDRootFindingMethod ccd_method = 101 | CCDRootFindingMethod::BREADTH_FIRST_SEARCH); 102 | 103 | Array3 compute_vertex_face_tolerances( 104 | const Vector3 &v_t0, 105 | const Vector3 &f0_t0, 106 | const Vector3 &f1_t0, 107 | const Vector3 &f2_t0, 108 | const Vector3 &v_t1, 109 | const Vector3 &f0_t1, 110 | const Vector3 &f1_t1, 111 | const Vector3 &f2_t1, 112 | const Scalar distance_tolerance = DEFAULT_CCD_DISTANCE_TOL); 113 | 114 | Array3 compute_edge_edge_tolerances( 115 | const Vector3 &ea0_t0, 116 | const Vector3 &ea1_t0, 117 | const Vector3 &eb0_t0, 118 | const Vector3 &eb1_t0, 119 | const Vector3 &ea0_t1, 120 | const Vector3 &ea1_t1, 121 | const Vector3 &eb0_t1, 122 | const Vector3 &eb1_t1, 123 | const Scalar distance_tolerance = DEFAULT_CCD_DISTANCE_TOL); 124 | 125 | long return_queue_size(); 126 | 127 | // these function are designed to test the performance of floating point version but with double inputs 128 | #ifdef TIGHT_INCLUSION_FLOAT_WITH_DOUBLE_INPUT 129 | bool edgeEdgeCCD( 130 | const Eigen::Vector3d &ea0_t0, 131 | const Eigen::Vector3d &ea1_t0, 132 | const Eigen::Vector3d &eb0_t0, 133 | const Eigen::Vector3d &eb1_t0, 134 | const Eigen::Vector3d &ea0_t1, 135 | const Eigen::Vector3d &ea1_t1, 136 | const Eigen::Vector3d &eb0_t1, 137 | const Eigen::Vector3d &eb1_t1, 138 | const Eigen::Array3d &err, 139 | const double ms, 140 | double &toi, 141 | const double tolerance, 142 | const double t_max, 143 | const long max_itr, 144 | double &output_tolerance, 145 | bool no_zero_toi = DEFAULT_NO_ZERO_TOI, 146 | const CCDRootFindingMethod ccd_method = 147 | CCDRootFindingMethod::BREADTH_FIRST_SEARCH); 148 | 149 | bool vertexFaceCCD( 150 | const Eigen::Vector3d &v_t0, 151 | const Eigen::Vector3d &f0_t0, 152 | const Eigen::Vector3d &f1_t0, 153 | const Eigen::Vector3d &f2_t0, 154 | const Eigen::Vector3d &v_t1, 155 | const Eigen::Vector3d &f0_t1, 156 | const Eigen::Vector3d &f1_t1, 157 | const Eigen::Vector3d &f2_t1, 158 | const Eigen::Array3d &err, 159 | const double ms, 160 | double &toi, 161 | const double tolerance, 162 | const double t_max, 163 | const long max_itr, 164 | double &output_tolerance, 165 | bool no_zero_toi = DEFAULT_NO_ZERO_TOI, 166 | const CCDRootFindingMethod ccd_method = 167 | CCDRootFindingMethod::BREADTH_FIRST_SEARCH); 168 | #endif 169 | } // namespace ticcd 170 | -------------------------------------------------------------------------------- /src/tight_inclusion/config.hpp.in: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // WARNING: Do not modify config.hpp directly. Instead, modify config.hpp.in. 4 | 5 | #define TIGHT_INCLUSION_NAME "@PROJECT_NAME@" 6 | #define TIGHT_INCLUSION_VER "@PROJECT_VERSION@" 7 | #define TIGHT_INCLUSION_VER_MAJOR "@PROJECT_VERSION_MAJOR@" 8 | #define TIGHT_INCLUSION_VER_MINOR "@PROJECT_VERSION_MINOR@" 9 | #define TIGHT_INCLUSION_VER_PATCH "@PROJECT_VERSION_PATCH@" 10 | 11 | #cmakedefine TIGHT_INCLUSION_WITH_RATIONAL 12 | #cmakedefine TIGHT_INCLUSION_WITH_TIMER 13 | #cmakedefine TIGHT_INCLUSION_WITH_DOUBLE_PRECISION 14 | #cmakedefine TIGHT_INCLUSION_LIMIT_QUEUE_SIZE 15 | #cmakedefine TIGHT_INCLUSION_FLOAT_WITH_DOUBLE_INPUT 16 | 17 | // #define TIGHT_INCLUSION_CHECK_QUEUE_SIZE 18 | // #define TIGHT_INCLUSION_USE_MAX_ABS_TOL 19 | 20 | #ifdef TIGHT_INCLUSION_LIMIT_QUEUE_SIZE 21 | #define MAX_QSIZE 1000 22 | #endif -------------------------------------------------------------------------------- /src/tight_inclusion/interval.cpp: -------------------------------------------------------------------------------- 1 | // An interval object. 2 | #include "interval.hpp" 3 | 4 | #include 5 | 6 | namespace ticcd { 7 | 8 | static constexpr uint8_t MAX_DENOM_POWER = 8 * sizeof(uint64_t) - 1; 9 | 10 | // calculate a*(2^b) 11 | uint64_t power(const uint64_t a, const uint8_t b) 12 | { 13 | // The fast bit shifting power trick only works if b is not too larger. 14 | assert(b < MAX_DENOM_POWER); 15 | // WARNING: Technically this can still fail with `b < MAX_DENOM_POWER` if `a > 1`. 16 | return a << b; 17 | } 18 | 19 | // return power t. n=result*2^t 20 | uint8_t reduction(const uint64_t n, uint64_t &result) 21 | { 22 | uint8_t t = 0; 23 | result = n; 24 | while (result != 0 && (result & 1) == 0) { 25 | result >>= 1; 26 | t++; 27 | } 28 | return t; 29 | } 30 | 31 | NumCCD::NumCCD(Scalar x) 32 | { 33 | // Use bisection to find an upper bound of x. 34 | assert(x >= 0 && x <= 1); 35 | NumCCD low(0, 0), high(1, 0), mid; 36 | 37 | // Hard code these cases for better accuracy. 38 | if (x == 0) { 39 | *this = low; 40 | return; 41 | } else if (x == 1) { 42 | *this = high; 43 | return; 44 | } 45 | 46 | do { 47 | mid = low + high; 48 | mid.denom_power++; 49 | 50 | if (mid.denom_power >= MAX_DENOM_POWER) { 51 | break; 52 | } 53 | 54 | if (x > mid) { 55 | low = mid; 56 | } else if (x < mid) { 57 | high = mid; 58 | } else { 59 | break; 60 | } 61 | } while (mid.denom_power < MAX_DENOM_POWER); 62 | *this = high; 63 | assert(x <= value()); 64 | } 65 | 66 | NumCCD NumCCD::operator+(const NumCCD &other) const 67 | { 68 | const uint64_t &k1 = numerator, &k2 = other.numerator; 69 | const uint8_t &n1 = denom_power, &n2 = other.denom_power; 70 | 71 | NumCCD result; 72 | if (n1 == n2) { 73 | result.denom_power = n2 - reduction(k1 + k2, result.numerator); 74 | } else if (n2 > n1) { 75 | result.numerator = k1 * pow2(n2 - n1) + k2; 76 | assert(result.numerator % 2 == 1); 77 | result.denom_power = n2; 78 | } else { // n2 < n1 79 | result.numerator = k1 + k2 * pow2(n1 - n2); 80 | assert(result.numerator % 2 == 1); 81 | result.denom_power = n1; 82 | } 83 | return result; 84 | } 85 | 86 | bool NumCCD::operator<(const NumCCD &other) const 87 | { 88 | const uint64_t &k1 = numerator, &k2 = other.numerator; 89 | const uint8_t &n1 = denom_power, &n2 = other.denom_power; 90 | 91 | uint64_t tmp_k1 = k1, tmp_k2 = k2; 92 | if (n1 < n2) { 93 | tmp_k1 = pow2(n2 - n1) * k1; 94 | } else if (n1 > n2) { 95 | tmp_k2 = pow2(n1 - n2) * k2; 96 | } 97 | assert((value() < other.value()) == (tmp_k1 < tmp_k2)); 98 | return tmp_k1 < tmp_k2; 99 | } 100 | 101 | bool NumCCD::is_sum_leq_1(const NumCCD &num1, const NumCCD &num2) 102 | { 103 | if (num1.denom_power == num2.denom_power) { 104 | // skip the reduction in num1 + num2 105 | return num1.numerator + num2.numerator <= num1.denominator(); 106 | } 107 | NumCCD tmp = num1 + num2; 108 | return tmp.numerator <= tmp.denominator(); 109 | } 110 | 111 | std::pair Interval::bisect() const 112 | { 113 | // interval is [k1/pow2(n1), k2/pow2(n2)] 114 | NumCCD mid = upper + lower; 115 | mid.denom_power++; // ÷ 2 116 | assert(mid.value() > lower.value() && mid.value() < upper.value()); 117 | return std::make_pair(Interval(lower, mid), Interval(mid, upper)); 118 | } 119 | 120 | bool Interval::overlaps(const Scalar r1, const Scalar r2) const 121 | { 122 | return upper.value() >= r1 && lower.value() <= r2; 123 | } 124 | 125 | Array3 width(const Interval3 &x) 126 | { 127 | return Array3( 128 | x[0].upper.value() - x[0].lower.value(), 129 | x[1].upper.value() - x[1].lower.value(), 130 | x[2].upper.value() - x[2].lower.value()); 131 | } 132 | 133 | } // namespace ticcd 134 | -------------------------------------------------------------------------------- /src/tight_inclusion/interval.hpp: -------------------------------------------------------------------------------- 1 | // An interval object. 2 | #pragma once 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace ticcd { 12 | 13 | // calculate a*(2^b) 14 | uint64_t power(const uint64_t a, const uint8_t b); 15 | 16 | // calculate 2^exponent 17 | inline uint64_t pow2(const uint8_t exponent) { return power(1l, exponent); } 18 | 19 | // return power t. n=result*2^t 20 | uint8_t reduction(const uint64_t n, uint64_t &result); 21 | 22 | // pair present a number k/pow(2,n) 23 | struct NumCCD { 24 | uint64_t numerator; 25 | uint8_t denom_power; 26 | 27 | NumCCD() {} 28 | 29 | NumCCD(uint64_t p_numerator, uint8_t p_denom_power) 30 | : numerator(p_numerator), denom_power(p_denom_power) 31 | { 32 | } 33 | 34 | NumCCD(Scalar x); 35 | 36 | ~NumCCD() {} 37 | 38 | uint64_t denominator() const { return pow2(denom_power); } 39 | 40 | // convert NumCCD to double number 41 | Scalar value() const { return Scalar(numerator) / denominator(); } 42 | 43 | operator double() const { return value(); } 44 | 45 | NumCCD operator+(const NumCCD &other) const; 46 | 47 | bool operator==(const NumCCD &other) const 48 | { 49 | return numerator == other.numerator 50 | && denom_power == other.denom_power; 51 | } 52 | bool operator!=(const NumCCD &other) const { return !(*this == other); } 53 | bool operator<(const NumCCD &other) const; 54 | bool operator<=(const NumCCD &other) const 55 | { 56 | return (*this == other) || (*this < other); 57 | } 58 | bool operator>=(const NumCCD &other) const { return !(*this < other); } 59 | bool operator>(const NumCCD &other) const { return !(*this <= other); } 60 | 61 | bool operator<(const Scalar other) const { return value() < other; } 62 | bool operator>(const Scalar other) const { return value() > other; } 63 | bool operator==(const Scalar other) const { return value() == other; } 64 | 65 | static bool is_sum_leq_1(const NumCCD &num1, const NumCCD &num2); 66 | }; 67 | 68 | // an interval represented by two double numbers 69 | struct Interval { 70 | NumCCD lower; 71 | NumCCD upper; 72 | 73 | Interval() {} 74 | 75 | Interval(const NumCCD &p_lower, const NumCCD &p_upper) 76 | : lower(p_lower), upper(p_upper) 77 | { 78 | } 79 | 80 | ~Interval() {} 81 | 82 | std::pair bisect() const; 83 | 84 | bool overlaps(const Scalar r1, const Scalar r2) const; 85 | }; 86 | 87 | typedef std::array Interval3; 88 | Array3 width(const Interval3 &x); 89 | 90 | } // namespace ticcd 91 | -------------------------------------------------------------------------------- /src/tight_inclusion/interval_root_finder.cpp: -------------------------------------------------------------------------------- 1 | // A root finder using interval arithmetic. 2 | #include "interval_root_finder.hpp" 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace ticcd { 11 | double time_predicates = 0, time_width = 0, time_bisect = 0, 12 | time_eval_origin_1D = 0, time_eval_origin_tuv = 0, 13 | time_vertex_solving = 0; 14 | 15 | template 16 | inline void min_max_array(const std::array &arr, T &min, T &max) 17 | { 18 | static_assert(N > 0, "no min/max of empty array"); 19 | min = arr[0]; 20 | max = arr[0]; 21 | for (int i = 1; i < N; i++) { 22 | if (min > arr[i]) { 23 | min = arr[i]; 24 | } 25 | if (max < arr[i]) { 26 | max = arr[i]; 27 | } 28 | } 29 | } 30 | 31 | // ** this version can return the true x or y or z tolerance of the co-domain ** 32 | // eps is the interval [-eps,eps] we need to check 33 | // if [-eps,eps] overlap, return true 34 | // bbox_in_eps tell us if the box is totally in eps box 35 | // ms is the minimum seperation 36 | template 37 | bool evaluate_bbox_one_dimension_vector( 38 | std::array &t_up, 39 | std::array &t_dw, 40 | std::array &u_up, 41 | std::array &u_dw, 42 | std::array &v_up, 43 | std::array &v_dw, 44 | const Vector3 &a_t0, 45 | const Vector3 &b_t0, 46 | const Vector3 &c_t0, 47 | const Vector3 &d_t0, 48 | const Vector3 &a_t1, 49 | const Vector3 &b_t1, 50 | const Vector3 &c_t1, 51 | const Vector3 &d_t1, 52 | const int dim, 53 | const Scalar eps, 54 | bool &bbox_in_eps, 55 | const Scalar ms = 0, 56 | Scalar *tol = nullptr) 57 | { 58 | TIGHT_INCLUSION_SCOPED_TIMER(time_vertex_solving); 59 | 60 | std::array vs; 61 | if constexpr (is_vertex_face) { 62 | vs = function_vf( 63 | a_t0[dim], b_t0[dim], c_t0[dim], d_t0[dim], a_t1[dim], 64 | b_t1[dim], c_t1[dim], d_t1[dim], t_up, t_dw, u_up, u_dw, v_up, 65 | v_dw); 66 | } else { 67 | vs = function_ee( 68 | a_t0[dim], b_t0[dim], c_t0[dim], d_t0[dim], a_t1[dim], 69 | b_t1[dim], c_t1[dim], d_t1[dim], t_up, t_dw, u_up, u_dw, v_up, 70 | v_dw); 71 | } 72 | 73 | Scalar minv, maxv; 74 | min_max_array(vs, minv, maxv); 75 | 76 | if (tol != nullptr) { 77 | *tol = maxv - minv; // this is the real tolerance 78 | } 79 | 80 | bbox_in_eps = false; 81 | 82 | const Scalar eps_and_ms = eps + ms; 83 | 84 | if (minv > eps_and_ms || maxv < -eps_and_ms) { 85 | return false; 86 | } 87 | 88 | if (minv >= -eps_and_ms && maxv <= eps_and_ms) { 89 | bbox_in_eps = true; 90 | } 91 | 92 | return true; 93 | } 94 | 95 | // ** this version can return the true tolerance of the co-domain ** 96 | // give the result of if the hex overlaps the input eps box around origin 97 | // use vectorized hex-vertex-solving function for acceleration 98 | // box_in_eps shows if this hex is totally inside box. if so, no need to do further bisection 99 | template 100 | bool origin_in_function_bounding_box_vector( 101 | const Interval3 ¶s, 102 | const Vector3 &a_t0, 103 | const Vector3 &b_t0, 104 | const Vector3 &c_t0, 105 | const Vector3 &d_t0, 106 | const Vector3 &a_t1, 107 | const Vector3 &b_t1, 108 | const Vector3 &c_t1, 109 | const Vector3 &d_t1, 110 | const Array3 &eps, 111 | bool &box_in_eps, 112 | const Scalar ms = 0, 113 | Array3 *tolerance = nullptr) 114 | { 115 | box_in_eps = false; 116 | 117 | std::array t_up, t_dw, u_up, u_dw, v_up, v_dw; 118 | { 119 | TIGHT_INCLUSION_SCOPED_TIMER(time_eval_origin_tuv); 120 | convert_tuv_to_array(paras, t_up, t_dw, u_up, u_dw, v_up, v_dw); 121 | } 122 | 123 | bool box_in[3]; 124 | for (int i = 0; i < 3; i++) { 125 | TIGHT_INCLUSION_SCOPED_TIMER(time_eval_origin_1D); 126 | Scalar *tol = tolerance == nullptr ? nullptr : &((*tolerance)[i]); 127 | if (!evaluate_bbox_one_dimension_vector( 128 | t_up, t_dw, u_up, u_dw, v_up, v_dw, a_t0, b_t0, c_t0, d_t0, 129 | a_t1, b_t1, c_t1, d_t1, i, eps[i], box_in[i], ms, tol)) { 130 | return false; 131 | } 132 | } 133 | 134 | if (box_in[0] && box_in[1] && box_in[2]) { 135 | box_in_eps = true; 136 | } 137 | 138 | return true; 139 | } 140 | 141 | // find the largest width/tol dimension that is greater than its tolerance 142 | int find_next_split(const Array3 &widths, const Array3 &tols) 143 | { 144 | // assert((widths > tols).any()); 145 | Array3 tmp = 146 | (widths > tols) 147 | .select( 148 | widths / tols, -std::numeric_limits::infinity()); 149 | int max_index; 150 | tmp.maxCoeff(&max_index); 151 | return max_index; 152 | } 153 | 154 | bool split_and_push( 155 | const Interval3 &tuv, 156 | int split_i, 157 | std::function push, 158 | bool is_vertex_face, 159 | Scalar t_upper_bound = 1) 160 | { 161 | std::pair halves = tuv[split_i].bisect(); 162 | if (halves.first.lower >= halves.first.upper 163 | || halves.second.lower >= halves.second.upper) { 164 | logger().error("overflow occured when splitting intervals!"); 165 | return true; 166 | } 167 | 168 | Interval3 tmp = tuv; 169 | 170 | if (split_i == 0) { 171 | if (t_upper_bound == 1 172 | || halves.second.overlaps(0, t_upper_bound)) { 173 | tmp[split_i] = halves.second; 174 | push(tmp); 175 | } 176 | if (t_upper_bound == 1 || halves.first.overlaps(0, t_upper_bound)) { 177 | tmp[split_i] = halves.first; 178 | push(tmp); 179 | } 180 | } else if (!is_vertex_face) { // edge uv 181 | tmp[split_i] = halves.second; 182 | push(tmp); 183 | tmp[split_i] = halves.first; 184 | push(tmp); 185 | } else { 186 | assert(is_vertex_face && split_i != 0); 187 | // u + v ≤ 1 188 | if (split_i == 1) { 189 | const Interval &v = tuv[2]; 190 | if (NumCCD::is_sum_leq_1(halves.second.lower, v.lower)) { 191 | tmp[split_i] = halves.second; 192 | push(tmp); 193 | } 194 | if (NumCCD::is_sum_leq_1(halves.first.lower, v.lower)) { 195 | tmp[split_i] = halves.first; 196 | push(tmp); 197 | } 198 | } else if (split_i == 2) { 199 | const Interval &u = tuv[1]; 200 | if (NumCCD::is_sum_leq_1(u.lower, halves.second.lower)) { 201 | tmp[split_i] = halves.second; 202 | push(tmp); 203 | } 204 | if (NumCCD::is_sum_leq_1(u.lower, halves.first.lower)) { 205 | tmp[split_i] = halves.first; 206 | push(tmp); 207 | } 208 | } 209 | } 210 | return false; // no overflow 211 | } 212 | 213 | // this version cannot give the impact time at t=1, although this collision can 214 | // be detected at t=0 of the next time step, but still may cause problems in 215 | // line-search based physical simulation 216 | template 217 | bool interval_root_finder_DFS( 218 | const Vector3 &a_t0, 219 | const Vector3 &b_t0, 220 | const Vector3 &c_t0, 221 | const Vector3 &d_t0, 222 | const Vector3 &a_t1, 223 | const Vector3 &b_t1, 224 | const Vector3 &c_t1, 225 | const Vector3 &d_t1, 226 | const Array3 &tol, 227 | const Array3 &err, 228 | const Scalar ms, 229 | Scalar &toi) 230 | { 231 | auto cmp_time = [](const Interval3 &i1, const Interval3 &i2) { 232 | return i1[0].lower >= i2[0].lower; 233 | }; 234 | 235 | //build interval set [0,1]x[0,1]x[0,1] 236 | const Interval zero_to_one = Interval(NumCCD(0, 0), NumCCD(1, 0)); 237 | Interval3 iset = {{zero_to_one, zero_to_one, zero_to_one}}; 238 | 239 | // Stack of intervals and the last split dimension 240 | // std::stack> istack; 241 | std::priority_queue< 242 | Interval3, std::vector, decltype(cmp_time)> 243 | istack(cmp_time); 244 | istack.emplace(iset); 245 | 246 | Array3 err_and_ms = err + ms; 247 | 248 | int refine = 0; 249 | 250 | toi = std::numeric_limits::infinity(); 251 | NumCCD TOI(1, 0); 252 | 253 | bool collision = false; 254 | int rnbr = 0; 255 | while (!istack.empty()) { 256 | Interval3 current = istack.top(); 257 | istack.pop(); 258 | 259 | // if(rnbr>0&&less_than( current[0].first,TOI)){ 260 | // continue; 261 | // } 262 | 263 | //TOI should always be no larger than current 264 | if (current[0].lower >= TOI) { 265 | continue; 266 | } 267 | 268 | refine++; 269 | 270 | bool zero_in, box_in; 271 | { 272 | TIGHT_INCLUSION_SCOPED_TIMER(time_predicates); 273 | zero_in = 274 | origin_in_function_bounding_box_vector( 275 | current, a_t0, b_t0, c_t0, d_t0, a_t1, b_t1, c_t1, d_t1, 276 | err_and_ms, box_in); 277 | } 278 | 279 | // #ifdef TIGHT_INCLUSION_WITH_RATIONAL // this is defined in the begining of this file 280 | // zero_in = origin_in_function_bounding_box_rational( 281 | // current, a_t0, b_t0, c_t0, d_t0, a_t1, b_t1, c_t1, d_t1); 282 | // #endif 283 | 284 | if (!zero_in) { 285 | continue; 286 | } 287 | 288 | Array3 widths; 289 | { 290 | TIGHT_INCLUSION_SCOPED_TIMER(time_width); 291 | widths = width(current); 292 | } 293 | 294 | if (box_in || (widths <= tol).all()) { 295 | TOI = current[0].lower; 296 | collision = true; 297 | rnbr++; 298 | // continue; 299 | toi = TOI.value(); 300 | return true; 301 | } 302 | 303 | // find the next dimension to split 304 | int split_i = find_next_split(widths, tol); 305 | 306 | bool overflowed = split_and_push( 307 | current, split_i, 308 | [&](const Interval3 &i) { istack.emplace(i); }, is_vertex_face); 309 | if (overflowed) { 310 | logger().error("overflow occured when splitting intervals!"); 311 | return true; 312 | } 313 | } 314 | if (collision) 315 | toi = TOI.value(); 316 | return collision; 317 | } 318 | 319 | // when check_t_overlap = false, check [0,1]x[0,1]x[0,1]; otherwise, check [0, t_max]x[0,1]x[0,1] 320 | template 321 | bool interval_root_finder_BFS( 322 | const Vector3 &a_t0, 323 | const Vector3 &b_t0, 324 | const Vector3 &c_t0, 325 | const Vector3 &d_t0, 326 | const Vector3 &a_t1, 327 | const Vector3 &b_t1, 328 | const Vector3 &c_t1, 329 | const Vector3 &d_t1, 330 | const Interval3 &iset, 331 | const Array3 &tol, 332 | const Scalar co_domain_tolerance, 333 | const Array3 &err, 334 | const Scalar ms, 335 | const Scalar max_time, 336 | const long max_itr, 337 | Scalar &toi, 338 | Scalar &output_tolerance) 339 | { 340 | long queue_size = 0; 341 | // if max_itr <0, output_tolerance= co_domain_tolerance; 342 | // else, output_tolearancewill be the precision after iteration time > max_itr 343 | output_tolerance = co_domain_tolerance; 344 | 345 | // this is used to catch the tolerance for each level 346 | Scalar temp_output_tolerance = co_domain_tolerance; 347 | // return time1 >= time2 348 | // auto time_cmp = [](const std::pair &i1, 349 | // const std::pair &i2) { 350 | // return i1.first[0].lower >= i2.first[0].lower; 351 | // }; 352 | 353 | // check the tree level by level instead of going deep 354 | // (if level 1 != level 2, return level 1 >= level 2; else, return time1 >= time2) 355 | auto horiz_cmp = [](const std::pair &i1, 356 | const std::pair &i2) { 357 | if (i1.second != i2.second) { 358 | return i1.second >= i2.second; 359 | } else { 360 | return i1.first[0].lower > i2.first[0].lower; 361 | } 362 | }; 363 | 364 | // Stack of intervals and the last split dimension 365 | // std::stack> istack; 366 | const auto &cmp = horiz_cmp; 367 | std::priority_queue< 368 | std::pair, std::vector>, 369 | decltype(cmp)> 370 | istack(cmp); 371 | istack.emplace(iset, -1); 372 | 373 | // current intervals 374 | Interval3 current; 375 | int refine = 0; 376 | Scalar impact_ratio = 1; 377 | 378 | toi = std::numeric_limits::infinity(); //set toi as infinate 379 | // temp_toi is to catch the toi of each level 380 | Scalar temp_toi = toi; 381 | // set TOI to 4. this is to record the impact time of this level 382 | NumCCD TOI(4, 0); 383 | // this is to record the element that already small enough or contained in eps-box 384 | NumCCD TOI_SKIP = TOI; 385 | bool use_skip = false; // this is to record if TOI_SKIP is used. 386 | int rnbr = 0; 387 | int current_level = -2; // in the begining, current_level != level 388 | int box_in_level = -2; // this checks if all the boxes before this 389 | // level < tolerance. only true, we can return when we find one overlaps eps box and smaller than tolerance or eps-box 390 | bool this_level_less_tol = true; 391 | bool find_level_root = false; 392 | Scalar t_upper_bound = max_time; // 2*tol make it more conservative 393 | while (!istack.empty()) { 394 | #ifdef TIGHT_INCLUSION_CHECK_QUEUE_SIZE 395 | if (istack.size() > queue_size) { 396 | queue_size = istack.size(); 397 | } 398 | #endif 399 | #ifdef TIGHT_INCLUSION_LIMIT_QUEUE_SIZE 400 | if (istack.size() > MAX_QSIZE) { 401 | return true; 402 | } 403 | #endif 404 | 405 | current = istack.top().first; 406 | int level = istack.top().second; 407 | istack.pop(); 408 | 409 | // if this box is later than TOI_SKIP in time, we can skip this one. 410 | // TOI_SKIP is only updated when the box is small enough or totally contained in eps-box 411 | if (current[0].lower >= TOI_SKIP) { 412 | continue; 413 | } 414 | // before check a new level, set this_level_less_tol=true 415 | if (box_in_level != level) { 416 | box_in_level = level; 417 | this_level_less_tol = true; 418 | } 419 | 420 | refine++; 421 | bool zero_in, box_in; 422 | Array3 true_tol; 423 | { 424 | TIGHT_INCLUSION_SCOPED_TIMER(time_predicates); 425 | // #ifdef TIGHT_INCLUSION_WITH_RATIONAL // this is defined in the begining of this file 426 | // Array3 ms_3d = Array3::Constant(ms); 427 | // zero_in = origin_in_function_bounding_box_rational_return_tolerance( 428 | // current, a_t0, b_t0, c_t0, d_t0, a_t1, b_t1, c_t1, d_t1, 429 | // ms_3d, box_in, true_tol); 430 | // #else 431 | zero_in = 432 | origin_in_function_bounding_box_vector( 433 | current, a_t0, b_t0, c_t0, d_t0, a_t1, b_t1, c_t1, d_t1, 434 | err, box_in, ms, &true_tol); 435 | // #endif 436 | } 437 | 438 | if (!zero_in) 439 | continue; 440 | 441 | Array3 widths; 442 | { 443 | TIGHT_INCLUSION_SCOPED_TIMER(time_width); 444 | widths = width(current); 445 | } 446 | 447 | bool tol_condition = (true_tol <= co_domain_tolerance).all(); 448 | 449 | // Condition 1, stopping condition on t, u and v is satisfied. this is useless now since we have condition 2 450 | bool condition1 = (widths <= tol).all(); 451 | 452 | // Condition 2, zero_in = true, box inside eps-box and in this level, 453 | // no box whose zero_in is true but box size larger than tolerance, can return 454 | bool condition2 = box_in && this_level_less_tol; 455 | if (!tol_condition) { 456 | this_level_less_tol = false; 457 | // this level has at least one box whose size > tolerance, thus we 458 | // cannot directly return if find one box whose size < tolerance or box-in 459 | // TODO: think about it. maybe we can return even if this value is false, so we can terminate earlier. 460 | } 461 | 462 | // Condition 3, in this level, we find a box that zero-in and size < tolerance. 463 | // and no other boxes whose zero-in is true in this level before this one is larger than tolerance, can return 464 | bool condition3 = this_level_less_tol; 465 | if (condition1 || condition2 || condition3) { 466 | TOI = current[0].lower; 467 | rnbr++; 468 | // continue; 469 | toi = TOI.value() * impact_ratio; 470 | // we don't need to compare with TOI_SKIP because we already 471 | // continue when t >= TOI_SKIP 472 | return true; 473 | } 474 | 475 | if (max_itr > 0) { // if max_itr <= 0 ⟹ unlimited iterations 476 | if (current_level != level) { 477 | // output_tolerance=current_tolerance; 478 | // current_tolerance=0; 479 | current_level = level; 480 | find_level_root = false; 481 | } 482 | // current_tolerance=std::max( 483 | // std::max(std::max(current_tolerance,true_tol[0]),true_tol[1]),true_tol[2] 484 | // ); 485 | if (!find_level_root) { 486 | TOI = current[0].lower; 487 | // collision=true; 488 | rnbr++; 489 | // continue; 490 | temp_toi = TOI.value() * impact_ratio; 491 | 492 | // if the real tolerance is larger than input, use the real one; 493 | // if the real tolerance is smaller than input, use input 494 | temp_output_tolerance = std::max( 495 | {true_tol[0], true_tol[1], true_tol[2], 496 | co_domain_tolerance}); 497 | // this ensures always find the earlist root 498 | find_level_root = true; 499 | } 500 | if (refine > max_itr) { 501 | toi = temp_toi; 502 | output_tolerance = temp_output_tolerance; 503 | 504 | return true; 505 | } 506 | // get the time of impact down here 507 | } 508 | 509 | // if this box is small enough, or inside of eps-box, then just continue, 510 | // but we need to record the collision time 511 | if (tol_condition || box_in) { 512 | if (current[0].lower < TOI_SKIP) { 513 | TOI_SKIP = current[0].lower; 514 | } 515 | use_skip = true; 516 | continue; 517 | } 518 | 519 | // find the next dimension to split 520 | int split_i = find_next_split(widths, tol); 521 | 522 | bool overflow = split_and_push( 523 | current, split_i, 524 | [&](const Interval3 &i) { istack.emplace(i, level + 1); }, 525 | is_vertex_face, t_upper_bound); 526 | if (overflow) { 527 | logger().error("overflow occured when splitting intervals!"); 528 | return true; 529 | } 530 | } 531 | 532 | if (use_skip) { 533 | toi = TOI_SKIP.value() * impact_ratio; 534 | return true; 535 | } 536 | 537 | return false; 538 | } 539 | 540 | template 541 | bool interval_root_finder_BFS( 542 | const Vector3 &a_t0, 543 | const Vector3 &b_t0, 544 | const Vector3 &c_t0, 545 | const Vector3 &d_t0, 546 | const Vector3 &a_t1, 547 | const Vector3 &b_t1, 548 | const Vector3 &c_t1, 549 | const Vector3 &d_t1, 550 | const Array3 &tol, 551 | const Scalar co_domain_tolerance, 552 | const Array3 &err, 553 | const Scalar ms, 554 | const Scalar max_time, 555 | const long max_itr, 556 | Scalar &toi, 557 | Scalar &output_tolerance) 558 | { 559 | // build interval set [0,t_max]x[0,1]x[0,1] 560 | const Interval zero_to_one = Interval(NumCCD(0, 0), NumCCD(1, 0)); 561 | Interval3 iset = {{ 562 | // Interval(NumCCD(0, 0), NumCCD(max_time)), 563 | zero_to_one, 564 | zero_to_one, 565 | zero_to_one, 566 | }}; 567 | 568 | return interval_root_finder_BFS( 569 | a_t0, b_t0, c_t0, d_t0, a_t1, b_t1, c_t1, d_t1, iset, tol, 570 | co_domain_tolerance, err, ms, max_time, max_itr, toi, 571 | output_tolerance); 572 | } 573 | 574 | void print_times() 575 | { 576 | logger().trace("[time] origin predicates, {}", time_predicates); 577 | logger().trace("[time] width, {}", time_width); 578 | logger().trace("[time] bisect, {}", time_bisect); 579 | logger().trace( 580 | "[time] origin part1(evaluate 1 dimension), {}", 581 | time_eval_origin_1D); 582 | logger().trace( 583 | "[time] origin part2(convert tuv), {}", time_eval_origin_tuv); 584 | logger().trace( 585 | "[time] time of call the vertex solving function, {}", 586 | time_vertex_solving); 587 | } 588 | 589 | Array3 get_numerical_error( 590 | const std::vector &vertices, 591 | const bool is_vertex_face, 592 | const bool using_minimum_separation) 593 | { 594 | Scalar eefilter; 595 | Scalar vffilter; 596 | if (!using_minimum_separation) { 597 | #ifdef TIGHT_INCLUSION_WITH_DOUBLE_PRECISION 598 | eefilter = 6.217248937900877e-15; 599 | vffilter = 6.661338147750939e-15; 600 | #else 601 | eefilter = 3.337861e-06; 602 | vffilter = 3.576279e-06; 603 | #endif 604 | } else { // using minimum separation 605 | #ifdef TIGHT_INCLUSION_WITH_DOUBLE_PRECISION 606 | eefilter = 7.105427357601002e-15; 607 | vffilter = 7.549516567451064e-15; 608 | #else 609 | eefilter = 3.814698e-06; 610 | vffilter = 4.053116e-06; 611 | #endif 612 | } 613 | 614 | Vector3 max = vertices[0].cwiseAbs(); 615 | for (int i = 1; i < vertices.size(); i++) { 616 | max = max.cwiseMax(vertices[i].cwiseAbs()); 617 | } 618 | Vector3 delta = max.cwiseMin(1); 619 | Scalar filter = is_vertex_face ? vffilter : eefilter; 620 | return filter * delta.array().pow(3); 621 | } 622 | 623 | bool edge_edge_interval_root_finder_DFS( 624 | const Vector3 &ea0_t0, 625 | const Vector3 &ea1_t0, 626 | const Vector3 &eb0_t0, 627 | const Vector3 &eb1_t0, 628 | const Vector3 &ea0_t1, 629 | const Vector3 &ea1_t1, 630 | const Vector3 &eb0_t1, 631 | const Vector3 &eb1_t1, 632 | const Array3 &tol, 633 | const Array3 &err, 634 | const Scalar ms, 635 | Scalar &toi) 636 | { 637 | return interval_root_finder_DFS( 638 | ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1, eb1_t1, tol, 639 | err, ms, toi); 640 | } 641 | 642 | bool vertex_face_interval_root_finder_DFS( 643 | const Vector3 &v_t0, 644 | const Vector3 &f0_t0, 645 | const Vector3 &f1_t0, 646 | const Vector3 &f2_t0, 647 | const Vector3 &v_t1, 648 | const Vector3 &f0_t1, 649 | const Vector3 &f1_t1, 650 | const Vector3 &f2_t1, 651 | const Array3 &tol, 652 | const Array3 &err, 653 | const Scalar ms, 654 | Scalar &toi) 655 | { 656 | return interval_root_finder_DFS( 657 | v_t0, f0_t0, f1_t0, f2_t0, v_t1, f0_t1, f1_t1, f2_t1, tol, err, ms, 658 | toi); 659 | } 660 | 661 | bool edge_edge_interval_root_finder_BFS( 662 | const Vector3 &ea0_t0, 663 | const Vector3 &ea1_t0, 664 | const Vector3 &eb0_t0, 665 | const Vector3 &eb1_t0, 666 | const Vector3 &ea0_t1, 667 | const Vector3 &ea1_t1, 668 | const Vector3 &eb0_t1, 669 | const Vector3 &eb1_t1, 670 | const Array3 &tol, 671 | const Scalar co_domain_tolerance, 672 | // this is the maximum error on each axis when calculating the vertices, err, aka, filter 673 | const Array3 &err, 674 | const Scalar ms, 675 | const Scalar max_time, 676 | const long max_itr, 677 | Scalar &toi, 678 | Scalar &output_tolerance) 679 | { 680 | return interval_root_finder_BFS( 681 | ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1, eb1_t1, tol, 682 | co_domain_tolerance, err, ms, max_time, max_itr, toi, 683 | output_tolerance); 684 | } 685 | 686 | bool vertex_face_interval_root_finder_BFS( 687 | const Vector3 &v_t0, 688 | const Vector3 &f0_t0, 689 | const Vector3 &f1_t0, 690 | const Vector3 &f2_t0, 691 | const Vector3 &v_t1, 692 | const Vector3 &f0_t1, 693 | const Vector3 &f1_t1, 694 | const Vector3 &f2_t1, 695 | const Array3 &tol, 696 | const Scalar co_domain_tolerance, 697 | // this is the maximum error on each axis when calculating the vertices, err, aka, filter 698 | const Array3 &err, 699 | const Scalar ms, 700 | const Scalar max_time, 701 | const long max_itr, 702 | Scalar &toi, 703 | Scalar &output_tolerance) 704 | { 705 | return interval_root_finder_BFS( 706 | v_t0, f0_t0, f1_t0, f2_t0, v_t1, f0_t1, f1_t1, f2_t1, tol, 707 | co_domain_tolerance, err, ms, max_time, max_itr, toi, 708 | output_tolerance); 709 | } 710 | 711 | // ------------------------------------------------------------------------ 712 | // Template instantiation 713 | // ------------------------------------------------------------------------ 714 | 715 | // clang-format off 716 | template bool interval_root_finder_DFS(const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Array3 &,const Array3 &,const Scalar,Scalar &); 717 | template bool interval_root_finder_DFS(const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Array3 &,const Array3 &,const Scalar,Scalar &); 718 | template bool interval_root_finder_BFS(const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Array3 &,const Scalar,const Array3 &,const Scalar,const Scalar,const long,Scalar &,Scalar &); 719 | template bool interval_root_finder_BFS(const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Vector3 &,const Array3 &,const Scalar,const Array3 &,const Scalar,const Scalar,const long,Scalar &,Scalar &); 720 | // clang-format on 721 | 722 | } // namespace ticcd 723 | -------------------------------------------------------------------------------- /src/tight_inclusion/interval_root_finder.hpp: -------------------------------------------------------------------------------- 1 | // A root finder using interval arithmetic. 2 | #pragma once 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace ticcd { 11 | // this version cannot give the impact time at t=1, although this collision can 12 | // be detected at t=0 of the next time step, but still may cause problems in 13 | // line-search based physical simulation 14 | 15 | /// @brief Perform interval root finding for CCD using DFS. 16 | /// @param[in] a_t0 Vertex a at t=0 17 | /// @param[in] b_t0 Vertex b at t=0 18 | /// @param[in] c_t0 Vertex c at t=0 19 | /// @param[in] d_t0 Vertex d at t=0 20 | /// @param[in] a_t1 Vertex a at t=1 21 | /// @param[in] b_t1 Vertex b at t=1 22 | /// @param[in] c_t1 Vertex c at t=1 23 | /// @param[in] d_t1 Vertex d at t=1 24 | /// @param[in] tol The tolerance of the interval. 25 | /// @param[in] err The floating-point error of the interval. 26 | /// @param[in] ms The minimum separation. 27 | /// @param[out] toi The time of impact. 28 | /// @tparam is_vertex_face Whether to check vertex-face or edge-edge collision. 29 | /// @return True if there is a root (collision), false otherwise. 30 | template 31 | bool interval_root_finder_DFS( 32 | const Vector3 &a_t0, 33 | const Vector3 &b_t0, 34 | const Vector3 &c_t0, 35 | const Vector3 &d_t0, 36 | const Vector3 &a_t1, 37 | const Vector3 &b_t1, 38 | const Vector3 &c_t1, 39 | const Vector3 &d_t1, 40 | const Array3 &tol, 41 | const Array3 &err, 42 | const Scalar ms, 43 | Scalar &toi); 44 | 45 | /// @brief Perform interval root finding for edge-edge CCD using DFS. 46 | /// @param[in] ea0_t0 The start position of the first vertex of the first edge. 47 | /// @param[in] ea1_t0 The start position of the second vertex of the first edge. 48 | /// @param[in] eb0_t0 The start position of the first vertex of the second edge. 49 | /// @param[in] eb1_t0 The start position of the second vertex of the second edge. 50 | /// @param[in] ea0_t1 The end position of the first vertex of the first edge. 51 | /// @param[in] ea1_t1 The end position of the second vertex of the first edge. 52 | /// @param[in] eb0_t1 The end position of the first vertex of the second edge. 53 | /// @param[in] eb1_t1 The end position of the second vertex of the second edge. 54 | /// @param[in] tol The tolerance of the interval. 55 | /// @param[in] err The maximum error on each axis when calculating the vertices, err, aka, filter. 56 | /// @param[in] ms The minimum separation. 57 | /// @param[out] toi The time of impact. 58 | /// @return True if there is a root (collision), false otherwise. 59 | bool edge_edge_interval_root_finder_DFS( 60 | const Vector3 &ea0_t0, 61 | const Vector3 &ea1_t0, 62 | const Vector3 &eb0_t0, 63 | const Vector3 &eb1_t0, 64 | const Vector3 &ea0_t1, 65 | const Vector3 &ea1_t1, 66 | const Vector3 &eb0_t1, 67 | const Vector3 &eb1_t1, 68 | const Array3 &tol, 69 | const Array3 &err, 70 | const Scalar ms, 71 | Scalar &toi); 72 | 73 | /// @brief Perform interval root finding for vertex-face CCD using DFS. 74 | /// @param[in] v_t0 The start position of the vertex. 75 | /// @param[in] f0_t0 The start position of the first vertex of the face. 76 | /// @param[in] f1_t0 The start position of the second vertex of the face. 77 | /// @param[in] f2_t0 The start position of the third vertex of the face. 78 | /// @param[in] v_t1 The end position of the vertex. 79 | /// @param[in] f0_t1 The end position of the first vertex of the face. 80 | /// @param[in] f1_t1 The end position of the second vertex of the face. 81 | /// @param[in] f2_t1 The end position of the third vertex of the face. 82 | /// @param[in] tol The tolerance of the interval. 83 | /// @param[in] err The maximum error on each axis when calculating the vertices, err, aka, filter. 84 | /// @param[in] ms The minimum separation. 85 | /// @param[out] toi The time of impact. 86 | /// @return True if there is a root (collision), false otherwise. 87 | bool vertex_face_interval_root_finder_DFS( 88 | const Vector3 &v_t0, 89 | const Vector3 &f0_t0, 90 | const Vector3 &f1_t0, 91 | const Vector3 &f2_t0, 92 | const Vector3 &v_t1, 93 | const Vector3 &f0_t1, 94 | const Vector3 &f1_t1, 95 | const Vector3 &f2_t1, 96 | const Array3 &tol, 97 | const Array3 &err, 98 | const Scalar ms, 99 | Scalar &toi); 100 | 101 | // this version cannot give the impact time at t=1. 102 | // max_itr is a user defined maximum iteration time. if < 0, then 103 | // it will run until stack empty; otherwise the algorithm will stop when 104 | // iteration time reaches max_itr, and return a solution precision output_tolerance 105 | // it uses interval t = [0, max_time] instead of t = [0,1] 106 | // 0<=max_time <=1 107 | // tree searching order is horizontal 108 | 109 | /// @brief Perform interval root finding for CCD using DFS. 110 | /// @param[in] a_t0 Vertex a at t=0 111 | /// @param[in] b_t0 Vertex b at t=0 112 | /// @param[in] c_t0 Vertex c at t=0 113 | /// @param[in] d_t0 Vertex d at t=0 114 | /// @param[in] a_t1 Vertex a at t=1 115 | /// @param[in] b_t1 Vertex b at t=1 116 | /// @param[in] c_t1 Vertex c at t=1 117 | /// @param[in] d_t1 Vertex d at t=1 118 | /// @param[in] tol The tolerance of the interval. 119 | /// @param[in] co_domain_tolerance The tolerance of the co-domain. 120 | /// @param[in] err The maximum error on each axis when calculating the vertices, err, aka, filter. 121 | /// @param[in] ms The minimum separation. 122 | /// @param[in] max_time The maximum time to check. 123 | /// @param[in] max_itr The maximum number of iterations. 124 | /// @param[out] toi The time of impact. 125 | /// @param[out] output_tolerance The resulting tolerance. 126 | /// @tparam is_vertex_face Whether to check vertex-face or edge-edge collision. 127 | /// @return True if there is a root (collision), false otherwise. 128 | template 129 | bool interval_root_finder_BFS( 130 | const Vector3 &a_t0, 131 | const Vector3 &b_t0, 132 | const Vector3 &c_t0, 133 | const Vector3 &d_t0, 134 | const Vector3 &a_t1, 135 | const Vector3 &b_t1, 136 | const Vector3 &c_t1, 137 | const Vector3 &d_t1, 138 | const Array3 &tol, 139 | const Scalar co_domain_tolerance, 140 | const Array3 &err, 141 | const Scalar ms, 142 | const Scalar max_time, 143 | const long max_itr, 144 | Scalar &toi, 145 | Scalar &output_tolerance); 146 | 147 | /// @brief Perform interval root finding for edge-edge CCD using BFS. 148 | /// @param[in] ea0_t0 The start position of the first vertex of the first edge. 149 | /// @param[in] ea1_t0 The start position of the second vertex of the first edge. 150 | /// @param[in] eb0_t0 The start position of the first vertex of the second edge. 151 | /// @param[in] eb1_t0 The start position of the second vertex of the second edge. 152 | /// @param[in] ea0_t1 The end position of the first vertex of the first edge. 153 | /// @param[in] ea1_t1 The end position of the second vertex of the first edge. 154 | /// @param[in] eb0_t1 The end position of the first vertex of the second edge. 155 | /// @param[in] eb1_t1 The end position of the second vertex of the second edge. 156 | /// @param[in] tol The tolerance of the interval. 157 | /// @param[in] co_domain_tolerance The tolerance of the co-domain. 158 | /// @param[in] err The maximum error on each axis when calculating the vertices, err, aka, filter. 159 | /// @param[in] ms The minimum separation. 160 | /// @param[in] max_time The maximum time to check. 161 | /// @param[in] max_itr The maximum number of iterations. 162 | /// @param[out] toi The time of impact. 163 | /// @param[out] output_tolerance The resulting tolerance. 164 | /// @return True if there is a root (collision), false otherwise. 165 | bool edge_edge_interval_root_finder_BFS( 166 | const Vector3 &ea0_t0, 167 | const Vector3 &ea1_t0, 168 | const Vector3 &eb0_t0, 169 | const Vector3 &eb1_t0, 170 | const Vector3 &ea0_t1, 171 | const Vector3 &ea1_t1, 172 | const Vector3 &eb0_t1, 173 | const Vector3 &eb1_t1, 174 | const Array3 &tol, 175 | const Scalar co_domain_tolerance, 176 | const Array3 &err, 177 | const Scalar ms, 178 | const Scalar max_time, 179 | const long max_itr, 180 | Scalar &toi, 181 | Scalar &output_tolerance); 182 | 183 | /// @brief Perform interval root finding for vertex-face CCD using BFS. 184 | /// @param[in] v_t0 The start position of the vertex. 185 | /// @param[in] f0_t0 The start position of the first vertex of the face. 186 | /// @param[in] f1_t0 The start position of the second vertex of the face. 187 | /// @param[in] f2_t0 The start position of the third vertex of the face. 188 | /// @param[in] v_t1 The end position of the vertex. 189 | /// @param[in] f0_t1 The end position of the first vertex of the face. 190 | /// @param[in] f1_t1 The end position of the second vertex of the face. 191 | /// @param[in] f2_t1 The end position of the third vertex of the face. 192 | /// @param[in] tol The tolerance of the interval. 193 | /// @param[in] co_domain_tolerance The tolerance of the co-domain. 194 | /// @param[in] err The maximum error on each axis when calculating the vertices, err, aka, filter. 195 | /// @param[in] ms The minimum separation. 196 | /// @param[in] max_time The maximum time to check. 197 | /// @param[in] max_itr The maximum number of iterations. 198 | /// @param[out] toi The time of impact. 199 | /// @param[out] output_tolerance The resulting tolerance. 200 | /// @return True if there is a root (collision), false otherwise. 201 | bool vertex_face_interval_root_finder_BFS( 202 | const Vector3 &v_t0, 203 | const Vector3 &f0_t0, 204 | const Vector3 &f1_t0, 205 | const Vector3 &f2_t0, 206 | const Vector3 &v_t1, 207 | const Vector3 &f0_t1, 208 | const Vector3 &f1_t1, 209 | const Vector3 &f2_t1, 210 | const Array3 &tol, 211 | const Scalar co_domain_tolerance, 212 | // this is the maximum error on each axis when calculating the vertices, err, aka, filter 213 | const Array3 &err, 214 | const Scalar ms, 215 | const Scalar max_time, 216 | const long max_itr, 217 | Scalar &toi, 218 | Scalar &output_tolerance); 219 | 220 | // calculate the sign of f. dim is the dimension we are evaluating. 221 | template 222 | T function_f_ee( 223 | const NumCCD &tpara, 224 | const NumCCD &upara, 225 | const NumCCD &vpara, 226 | const int dim, 227 | const Vector3 &ea0_t0, 228 | const Vector3 &ea1_t0, 229 | const Vector3 &eb0_t0, 230 | const Vector3 &eb1_t0, 231 | const Vector3 &ea0_t1, 232 | const Vector3 &ea1_t1, 233 | const Vector3 &eb0_t1, 234 | const Vector3 &eb1_t1); 235 | 236 | template 237 | T function_f_vf( 238 | const NumCCD &tpara, 239 | const NumCCD &upara, 240 | const NumCCD &vpara, 241 | const int dim, 242 | const Vector3 &v_t0, 243 | const Vector3 &f0_t0, 244 | const Vector3 &f1_t0, 245 | const Vector3 &f2_t0, 246 | const Vector3 &v_t1, 247 | const Vector3 &f0_t1, 248 | const Vector3 &f1_t1, 249 | const Vector3 &f2_t1); 250 | 251 | void print_times(); 252 | 253 | // get the filter of ccd. the inputs are the vertices of the bounding box of the simulation scene 254 | Array3 get_numerical_error( 255 | const std::vector &vertices, 256 | const bool is_vertex_face, 257 | const bool using_minimum_separation); 258 | 259 | } // namespace ticcd 260 | -------------------------------------------------------------------------------- /src/tight_inclusion/logger.cpp: -------------------------------------------------------------------------------- 1 | #include "logger.hpp" 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace ticcd { 8 | 9 | namespace { 10 | 11 | // Custom logger instance defined by the user, if any 12 | std::shared_ptr &get_shared_logger() 13 | { 14 | static std::shared_ptr logger; 15 | return logger; 16 | } 17 | 18 | } // namespace 19 | 20 | // Retrieve current logger 21 | spdlog::logger &logger() 22 | { 23 | if (get_shared_logger()) { 24 | return *get_shared_logger(); 25 | } else { 26 | // When using factory methods provided by spdlog (_st and _mt 27 | // functions), names must be unique, since the logger is registered 28 | // globally. Otherwise, you will need to create the logger manually. See 29 | // https://github.com/gabime/spdlog/wiki/2.-Creating-loggers 30 | static auto default_logger = spdlog::stdout_color_mt("ticcd"); 31 | return *default_logger; 32 | } 33 | } 34 | 35 | // Use a custom logger 36 | void set_logger(std::shared_ptr x) 37 | { 38 | get_shared_logger() = std::move(x); 39 | } 40 | 41 | } // namespace ticcd 42 | -------------------------------------------------------------------------------- /src/tight_inclusion/logger.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // clang-format off 4 | #include 5 | #include 6 | #include 7 | // clang-format on 8 | 9 | namespace ticcd { 10 | 11 | /// Retrieves the current logger. 12 | /// @return A const reference to the logger object. 13 | spdlog::logger &logger(); 14 | 15 | /// Setup a logger object. Calling this function with other function is not 16 | /// thread-safe. 17 | /// @param logger New logger object to be used. 18 | void set_logger(std::shared_ptr logger); 19 | 20 | } // namespace ticcd 21 | -------------------------------------------------------------------------------- /src/tight_inclusion/rational/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(SOURCES 2 | ccd.cpp 3 | ccd.hpp 4 | interval_root_finder.cpp 5 | interval_root_finder.hpp 6 | ) 7 | 8 | source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "Source Files" FILES ${SOURCES}) 9 | target_sources(tight_inclusion PRIVATE ${SOURCES}) 10 | 11 | ################################################################################ 12 | # Subfolders 13 | ################################################################################ 14 | -------------------------------------------------------------------------------- /src/tight_inclusion/rational/ccd.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace ticcd::rational { 10 | 11 | bool edgeEdgeCCD( 12 | const Vector3 &ea0_t0, 13 | const Vector3 &ea1_t0, 14 | const Vector3 &eb0_t0, 15 | const Vector3 &eb1_t0, 16 | const Vector3 &ea0_t1, 17 | const Vector3 &ea1_t1, 18 | const Vector3 &eb0_t1, 19 | const Vector3 &eb1_t1, 20 | const Array3 &err, 21 | const Scalar ms, 22 | Scalar &toi) 23 | { 24 | 25 | Array3 tol = compute_edge_edge_tolerances( 26 | ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1, eb1_t1, 27 | DEFAULT_CCD_DISTANCE_TOL); 28 | 29 | ////////////////////////////////////////////////////////// 30 | // TODO this should be the error of the whole mesh 31 | std::vector vlist = { 32 | {ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1, eb1_t1}}; 33 | 34 | bool use_ms = ms > 0; 35 | Array3 auto_err = get_numerical_error(vlist, false, use_ms); 36 | ////////////////////////////////////////////////////////// 37 | 38 | std::array toi_interval; 39 | 40 | bool is_impacting = edge_edge_interval_root_finder( 41 | ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1, eb1_t1, tol, 42 | auto_err, ms, toi_interval); 43 | 44 | // Return a conservative time-of-impact 45 | if (is_impacting) { 46 | toi = toi_interval[0][0]; 47 | } 48 | // This time of impact is very dangerous for convergence 49 | // assert(!is_impacting || toi > 0); 50 | return is_impacting; 51 | } 52 | 53 | bool vertexFaceCCD( 54 | const Vector3 &v_t0, 55 | const Vector3 &f0_t0, 56 | const Vector3 &f1_t0, 57 | const Vector3 &f2_t0, 58 | const Vector3 &v_t1, 59 | const Vector3 &f0_t1, 60 | const Vector3 &f1_t1, 61 | const Vector3 &f2_t1, 62 | const Array3 &err, 63 | const Scalar ms, 64 | Scalar &toi) 65 | { 66 | Array3 tol = compute_vertex_face_tolerances( 67 | v_t0, f0_t0, f1_t0, f2_t0, v_t1, f0_t1, f1_t1, f2_t1, 68 | DEFAULT_CCD_DISTANCE_TOL); 69 | 70 | ////////////////////////////////////////////////////////// 71 | // TODO this should be the error of the whole mesh 72 | std::vector vlist = { 73 | {v_t0, f0_t0, f1_t0, f2_t0, v_t1, f0_t1, f1_t1, f2_t1}}; 74 | 75 | bool use_ms = ms > 0; 76 | Array3 auto_err = get_numerical_error(vlist, false, use_ms); 77 | ////////////////////////////////////////////////////////// 78 | 79 | std::array toi_interval; 80 | 81 | bool is_impacting = vertex_face_interval_root_finder( 82 | v_t0, f0_t0, f1_t0, f2_t0, v_t1, f0_t1, f1_t1, f2_t1, tol, auto_err, 83 | ms, toi_interval); 84 | 85 | // Return a conservative time-of-impact 86 | if (is_impacting) { 87 | toi = toi_interval[0][0]; 88 | } 89 | 90 | // This time of impact is very dangerous for convergence 91 | // assert(!is_impacting || toi > 0); 92 | return is_impacting; 93 | } 94 | 95 | } // namespace ticcd::rational 96 | -------------------------------------------------------------------------------- /src/tight_inclusion/rational/ccd.hpp: -------------------------------------------------------------------------------- 1 | // Time-of-impact computation for rigid bodies with angular trajectories. 2 | #pragma once 3 | 4 | #include 5 | 6 | namespace ticcd::rational { 7 | 8 | // this version is an naive implementation of Tight-Inclusion CCD without optimizations 9 | bool edgeEdgeCCD( 10 | const Vector3 &ea0_t0, 11 | const Vector3 &ea1_t0, 12 | const Vector3 &eb0_t0, 13 | const Vector3 &eb1_t0, 14 | const Vector3 &ea0_t1, 15 | const Vector3 &ea1_t1, 16 | const Vector3 &eb0_t1, 17 | const Vector3 &eb1_t1, 18 | const Array3 &err, 19 | const Scalar ms, 20 | Scalar &toi); 21 | 22 | // this version is an naive implementation of Tight-Inclusion CCD without optimizations 23 | bool vertexFaceCCD( 24 | const Vector3 &v_t0, 25 | const Vector3 &f0_t0, 26 | const Vector3 &f1_t0, 27 | const Vector3 &f2_t0, 28 | const Vector3 &v_t1, 29 | const Vector3 &f0_t1, 30 | const Vector3 &f1_t1, 31 | const Vector3 &f2_t1, 32 | const Array3 &err, 33 | const Scalar ms, 34 | Scalar &toi); 35 | 36 | } // namespace ticcd::rational 37 | -------------------------------------------------------------------------------- /src/tight_inclusion/rational/interval_root_finder.cpp: -------------------------------------------------------------------------------- 1 | // A root finder using interval arithmetic. 2 | #include 3 | 4 | #include 5 | 6 | // #define COMPARE_WITH_RATIONAL 7 | 8 | namespace ticcd::rational { 9 | 10 | Array3r width(const std::array, 3> &x) 11 | { 12 | Array3r w; 13 | for (int i = 0; i < 3; i++) { 14 | Rational sub = x[i][1] - x[i][0]; 15 | w[i] = sub >= 0 ? sub : -sub; 16 | assert(w[i] >= 0); 17 | } 18 | return w; 19 | } 20 | 21 | Vector3r function_f_ee_rational( 22 | const Rational &t, 23 | const Rational &u, 24 | const Rational &v, 25 | const Vector3 &ea0_t0d, 26 | const Vector3 &ea1_t0d, 27 | const Vector3 &eb0_t0d, 28 | const Vector3 &eb1_t0d, 29 | const Vector3 &ea0_t1d, 30 | const Vector3 &ea1_t1d, 31 | const Vector3 &eb0_t1d, 32 | const Vector3 &eb1_t1d) 33 | { 34 | const Vector3r ea0_t0 = ea0_t0d.cast(); 35 | const Vector3r ea1_t0 = ea1_t0d.cast(); 36 | const Vector3r eb0_t0 = eb0_t0d.cast(); 37 | const Vector3r eb1_t0 = eb1_t0d.cast(); 38 | const Vector3r ea0_t1 = ea0_t1d.cast(); 39 | const Vector3r ea1_t1 = ea1_t1d.cast(); 40 | const Vector3r eb0_t1 = eb0_t1d.cast(); 41 | const Vector3r eb1_t1 = eb1_t1d.cast(); 42 | 43 | const Vector3r ea0 = (ea0_t1 - ea0_t0) * t + ea0_t0; 44 | const Vector3r ea1 = (ea1_t1 - ea1_t0) * t + ea1_t0; 45 | const Vector3r va = (ea1 - ea0) * u + ea0; 46 | 47 | const Vector3r eb0 = (eb0_t1 - eb0_t0) * t + eb0_t0; 48 | const Vector3r eb1 = (eb1_t1 - eb1_t0) * t + eb1_t0; 49 | const Vector3r vb = (eb1 - eb0) * v + eb0; 50 | 51 | return vb - va; 52 | } 53 | 54 | Vector3r function_f_ee_rational( 55 | const NumCCD &tpara, 56 | const NumCCD &upara, 57 | const NumCCD &vpara, 58 | const Vector3 &ea0_t0d, 59 | const Vector3 &ea1_t0d, 60 | const Vector3 &eb0_t0d, 61 | const Vector3 &eb1_t0d, 62 | const Vector3 &ea0_t1d, 63 | const Vector3 &ea1_t1d, 64 | const Vector3 &eb0_t1d, 65 | const Vector3 &eb1_t1d) 66 | { 67 | return function_f_ee_rational( 68 | Rational(double(tpara.numerator)) 69 | / Rational(double(tpara.denominator())), 70 | Rational(double(upara.numerator)) 71 | / Rational(double(upara.denominator())), 72 | Rational(double(vpara.numerator)) 73 | / Rational(double(vpara.denominator())), 74 | ea0_t0d, ea1_t0d, eb0_t0d, eb1_t0d, ea0_t1d, ea1_t1d, eb0_t1d, 75 | eb1_t1d); 76 | } 77 | 78 | Vector3r function_f_vf_rational( 79 | const Rational &t, 80 | const Rational &u, 81 | const Rational &v, 82 | const Vector3 &v_t0d, 83 | const Vector3 &f0_t0d, 84 | const Vector3 &f1_t0d, 85 | const Vector3 &f2_t0d, 86 | const Vector3 &v_t1d, 87 | const Vector3 &f0_t1d, 88 | const Vector3 &f1_t1d, 89 | const Vector3 &f2_t1d) 90 | { 91 | const Vector3r v_t0 = v_t0d.cast(); 92 | const Vector3r f0_t0 = f0_t0d.cast(); 93 | const Vector3r f1_t0 = f1_t0d.cast(); 94 | const Vector3r f2_t0 = f2_t0d.cast(); 95 | const Vector3r v_t1 = v_t1d.cast(); 96 | const Vector3r f0_t1 = f0_t1d.cast(); 97 | const Vector3r f1_t1 = f1_t1d.cast(); 98 | const Vector3r f2_t1 = f2_t1d.cast(); 99 | 100 | const Vector3r va = (v_t1 - v_t0) * t + v_t0; 101 | 102 | const Vector3r f0 = (f0_t1 - f0_t0) * t + f0_t0; 103 | const Vector3r f1 = (f1_t1 - f1_t0) * t + f1_t0; 104 | const Vector3r f2 = (f2_t1 - f2_t0) * t + f2_t0; 105 | const Vector3r vb = (f1 - f0) * u + (f2 - f0) * v + f0; 106 | return va - vb; 107 | } 108 | 109 | Vector3r function_f_vf_rational( 110 | const NumCCD &tpara, 111 | const NumCCD &upara, 112 | const NumCCD &vpara, 113 | const Vector3 &v_t0, 114 | const Vector3 &f0_t0, 115 | const Vector3 &f1_t0, 116 | const Vector3 &f2_t0, 117 | const Vector3 &v_t1, 118 | const Vector3 &f0_t1, 119 | const Vector3 &f1_t1, 120 | const Vector3 &f2_t1) 121 | { 122 | return function_f_ee_rational( 123 | Rational(double(tpara.numerator)) 124 | / Rational(double(tpara.denominator())), 125 | Rational(double(upara.numerator)) 126 | / Rational(double(upara.denominator())), 127 | Rational(double(vpara.numerator)) 128 | / Rational(double(vpara.denominator())), 129 | v_t0, f0_t0, f1_t0, f2_t0, v_t1, f0_t1, f1_t1, f2_t1); 130 | } 131 | 132 | template 133 | bool origin_in_function_bounding_box_rational( 134 | const std::array, 3> ¶s, 135 | const Vector3 &a_t0, 136 | const Vector3 &b_t0, 137 | const Vector3 &c_t0, 138 | const Vector3 &d_t0, 139 | const Vector3 &a_t1, 140 | const Vector3 &b_t1, 141 | const Vector3 &c_t1, 142 | const Vector3 &d_t1, 143 | const Array3 &box, 144 | bool &box_in_eps, 145 | Array3 *tolerance = nullptr) 146 | { 147 | std::array t, u, v; 148 | t = paras[0]; 149 | u = paras[1]; 150 | v = paras[2]; 151 | 152 | Eigen::Matrix pts; 153 | int c = 0; 154 | for (int i = 0; i < 2; i++) { 155 | for (int j = 0; j < 2; j++) { 156 | for (int k = 0; k < 2; k++) { 157 | if constexpr (!is_vertex_face) { 158 | pts.row(c) = function_f_ee_rational( 159 | t[i], u[j], v[k], a_t0, b_t0, c_t0, d_t0, a_t1, 160 | b_t1, c_t1, d_t1); 161 | } else { 162 | pts.row(c) = function_f_vf_rational( 163 | t[i], u[j], v[k], a_t0, b_t0, c_t0, d_t0, a_t1, 164 | b_t1, c_t1, d_t1); 165 | } 166 | c++; 167 | } 168 | } 169 | } 170 | 171 | Eigen::Array minv = pts.colwise().minCoeff(); 172 | Eigen::Array maxv = pts.colwise().maxCoeff(); 173 | 174 | if (tolerance != nullptr) { 175 | *tolerance = (maxv - minv).template cast(); 176 | } 177 | auto boxR = box.cast(); 178 | box_in_eps = (minv >= -boxR).all() && (maxv <= boxR).all(); 179 | return (minv <= boxR).all() && (maxv >= -boxR).all(); 180 | } 181 | 182 | std::pair 183 | bisect(const RationalInterval &inter) 184 | { 185 | Rational mid = (inter[0] + inter[1]) * Rational(0.5); 186 | return std::make_pair( 187 | {{inter[0], mid}}, {{mid, inter[1]}}); 188 | } 189 | 190 | template 191 | bool interval_root_finder( 192 | const Vector3 &a_t0, 193 | const Vector3 &b_t0, 194 | const Vector3 &c_t0, 195 | const Vector3 &d_t0, 196 | const Vector3 &a_t1, 197 | const Vector3 &b_t1, 198 | const Vector3 &c_t1, 199 | const Vector3 &d_t1, 200 | const Array3 &tol, 201 | const Array3 &err, 202 | const Scalar ms, 203 | std::array &root) 204 | { 205 | RationalInterval interval01 = {{Rational(0), Rational(1)}}; 206 | std::array paracube; 207 | paracube[0] = interval01; 208 | paracube[1] = interval01; 209 | paracube[2] = interval01; 210 | 211 | // Stack of intervals and the last split dimension 212 | std::stack, int>> istack; 213 | istack.emplace(paracube, -1); 214 | 215 | // current intervals 216 | std::array current; 217 | Array3 err_and_ms; 218 | err_and_ms[0] = err[0] + ms; 219 | err_and_ms[1] = err[1] + ms; 220 | err_and_ms[2] = err[2] + ms; 221 | 222 | Rational max_rational = Rational(std::numeric_limits::max()); 223 | const Eigen::Array tolR( 224 | std::isfinite(tol[0]) ? Rational(tol[0]) : max_rational, 225 | std::isfinite(tol[1]) ? Rational(tol[1]) : max_rational, 226 | std::isfinite(tol[2]) ? Rational(tol[2]) : max_rational); 227 | 228 | while (!istack.empty()) { 229 | current = istack.top().first; 230 | int last_split = istack.top().second; 231 | istack.pop(); 232 | 233 | bool box_in_eps; 234 | bool zero_in = origin_in_function_bounding_box_rational< 235 | Rational, is_vertex_face>( 236 | current, a_t0, b_t0, c_t0, d_t0, a_t1, b_t1, c_t1, d_t1, 237 | /*box=*/Array3::Zero(), box_in_eps); 238 | 239 | if (!zero_in) 240 | continue; 241 | Array3r widths = width(current); 242 | if ((widths <= tolR.array()).all()) { 243 | root = current; 244 | return true; 245 | } 246 | 247 | // Bisect the next dimension that is greater than its tolerance 248 | int split_i; 249 | for (int i = 1; i <= 3; i++) { 250 | split_i = (last_split + i) % 3; 251 | if (widths[split_i] > tolR(split_i)) { 252 | break; 253 | } 254 | } 255 | std::pair halves = 256 | bisect(current[split_i]); 257 | 258 | if (is_vertex_face) { 259 | if (split_i == 1) { 260 | if (halves.first[0] + current[2][0] <= 1) { 261 | current[split_i] = halves.first; 262 | istack.emplace(current, split_i); 263 | } 264 | if (halves.second[0] + current[2][0] <= 1) { 265 | current[split_i] = halves.second; 266 | istack.emplace(current, split_i); 267 | } 268 | } 269 | 270 | if (split_i == 2) { 271 | 272 | if (halves.first[0] + current[1][0] <= 1) { 273 | current[split_i] = halves.first; 274 | istack.emplace(current, split_i); 275 | } 276 | if (halves.second[0] + current[1][0] <= 1) { 277 | current[split_i] = halves.second; 278 | istack.emplace(current, split_i); 279 | } 280 | } 281 | if (split_i == 0) { 282 | current[split_i] = halves.second; 283 | istack.emplace(current, split_i); 284 | current[split_i] = halves.first; 285 | istack.emplace(current, split_i); 286 | } 287 | } else { 288 | current[split_i] = halves.second; 289 | istack.emplace(current, split_i); 290 | current[split_i] = halves.first; 291 | istack.emplace(current, split_i); 292 | } 293 | } 294 | return false; 295 | } 296 | 297 | bool edge_edge_interval_root_finder( 298 | const Vector3 &ea0_t0, 299 | const Vector3 &ea1_t0, 300 | const Vector3 &eb0_t0, 301 | const Vector3 &eb1_t0, 302 | const Vector3 &ea0_t1, 303 | const Vector3 &ea1_t1, 304 | const Vector3 &eb0_t1, 305 | const Vector3 &eb1_t1, 306 | const Array3 &tol, 307 | const Array3 &err, 308 | const Scalar ms, 309 | std::array &root) 310 | { 311 | return interval_root_finder( 312 | ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1, eb1_t1, tol, 313 | err, ms, root); 314 | } 315 | 316 | bool vertex_face_interval_root_finder( 317 | const Vector3 &v_t0, 318 | const Vector3 &f0_t0, 319 | const Vector3 &f1_t0, 320 | const Vector3 &f2_t0, 321 | const Vector3 &v_t1, 322 | const Vector3 &f0_t1, 323 | const Vector3 &f1_t1, 324 | const Vector3 &f2_t1, 325 | const Array3 &tol, 326 | const Array3 &err, 327 | const Scalar ms, 328 | std::array &root) 329 | { 330 | return interval_root_finder( 331 | v_t0, f0_t0, f1_t0, f2_t0, v_t1, f0_t1, f1_t1, f2_t1, tol, err, ms, 332 | root); 333 | } 334 | } // namespace ticcd::rational 335 | -------------------------------------------------------------------------------- /src/tight_inclusion/rational/interval_root_finder.hpp: -------------------------------------------------------------------------------- 1 | // A root finder using interval arithmetic. 2 | #pragma once 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | namespace ticcd::rational { 13 | using namespace ::rational; 14 | 15 | typedef std::array RationalInterval; 16 | typedef Eigen::Matrix 17 | Vector3r; 18 | typedef Eigen::Array 19 | Array3r; 20 | 21 | bool edge_edge_interval_root_finder( 22 | const Vector3 &ea0_t0, 23 | const Vector3 &ea1_t0, 24 | const Vector3 &eb0_t0, 25 | const Vector3 &eb1_t0, 26 | const Vector3 &ea0_t1, 27 | const Vector3 &ea1_t1, 28 | const Vector3 &eb0_t1, 29 | const Vector3 &eb1_t1, 30 | const Array3 &tol, 31 | const Array3 &err, 32 | const Scalar ms, 33 | std::array &root); 34 | 35 | bool vertex_face_interval_root_finder( 36 | const Vector3 &v_t0, 37 | const Vector3 &f0_t0, 38 | const Vector3 &f1_t0, 39 | const Vector3 &f2_t0, 40 | const Vector3 &v_t1, 41 | const Vector3 &f0_t1, 42 | const Vector3 &f1_t1, 43 | const Vector3 &f2_t1, 44 | const Array3 &tol, 45 | const Array3 &err, 46 | const Scalar ms, 47 | std::array &root); 48 | 49 | Scalar print_time_rational(); 50 | 51 | } // namespace ticcd::rational 52 | -------------------------------------------------------------------------------- /src/tight_inclusion/timer.hpp: -------------------------------------------------------------------------------- 1 | // This file is part of libigl, a simple c++ geometry processing library. 2 | // 3 | // Copyright (C) 2013 Alec Jacobson 4 | // 5 | // This Source Code Form is subject to the terms of the Mozilla Public License 6 | // v. 2.0. If a copy of the MPL was not distributed with this file, You can 7 | // obtain one at http://mozilla.org/MPL/2.0/. 8 | // High Resolution Timer. 9 | // 10 | // Resolution on Mac (clock tick) 11 | // Resolution on Linux (1 us not tested) 12 | // Resolution on Windows (clock tick not tested) 13 | 14 | #pragma once 15 | 16 | #include 17 | 18 | #ifdef WIN32 // Windows system specific 19 | #include 20 | #elif __APPLE__ // Unix based system specific 21 | #include // for mach_absolute_time 22 | #else 23 | #include 24 | #endif 25 | #include 26 | 27 | //////////////////////////////////////////////////////////////////////////////// 28 | 29 | #ifdef TIGHT_INCLUSION_WITH_TIMER 30 | #define TIGHT_INCLUSION_SCOPED_TIMER(total_time) \ 31 | ticcd::ScopedTimer __tight_inclusion_timer(total_time) 32 | #else 33 | #define TIGHT_INCLUSION_SCOPED_TIMER(total_time) 34 | #endif 35 | 36 | namespace ticcd { 37 | class Timer { 38 | public: 39 | // default constructor 40 | Timer() 41 | : stopped(0), 42 | #ifdef WIN32 43 | frequency(), startCount(), endCount() 44 | #elif __APPLE__ 45 | startCount(0), endCount(0) 46 | #else 47 | startCount(), endCount() 48 | #endif 49 | { 50 | #ifdef WIN32 51 | QueryPerformanceFrequency(&frequency); 52 | startCount.QuadPart = 0; 53 | endCount.QuadPart = 0; 54 | #elif __APPLE__ 55 | startCount = 0; 56 | endCount = 0; 57 | #else 58 | startCount.tv_sec = startCount.tv_usec = 0; 59 | endCount.tv_sec = endCount.tv_usec = 0; 60 | #endif 61 | 62 | stopped = 0; 63 | } 64 | // default destructor 65 | ~Timer() {} 66 | 67 | #ifdef __APPLE__ 68 | //Raw mach_absolute_times going in, difference in seconds out 69 | double subtractTimes(uint64_t endTime, uint64_t startTime) 70 | { 71 | uint64_t difference = endTime - startTime; 72 | static double conversion = 0.0; 73 | 74 | if (conversion == 0.0) { 75 | mach_timebase_info_data_t info; 76 | kern_return_t err = mach_timebase_info(&info); 77 | 78 | //Convert the timebase into seconds 79 | if (err == 0) 80 | conversion = 1e-9 * (double)info.numer / (double)info.denom; 81 | } 82 | 83 | return conversion * (double)difference; 84 | } 85 | #endif 86 | 87 | // start timer 88 | void start() 89 | { 90 | stopped = 0; // reset stop flag 91 | #ifdef WIN32 92 | QueryPerformanceCounter(&startCount); 93 | #elif __APPLE__ 94 | startCount = mach_absolute_time(); 95 | #else 96 | gettimeofday(&startCount, NULL); 97 | #endif 98 | } 99 | 100 | // stop the timer 101 | void stop() 102 | { 103 | stopped = 1; // set timer stopped flag 104 | 105 | #ifdef WIN32 106 | QueryPerformanceCounter(&endCount); 107 | #elif __APPLE__ 108 | endCount = mach_absolute_time(); 109 | #else 110 | gettimeofday(&endCount, NULL); 111 | #endif 112 | } 113 | // get elapsed time in second 114 | double getElapsedTime() { return this->getElapsedTimeInSec(); } 115 | // get elapsed time in second (same as getElapsedTime) 116 | double getElapsedTimeInSec() 117 | { 118 | return this->getElapsedTimeInMicroSec() * 0.000001; 119 | } 120 | 121 | // get elapsed time in milli-second 122 | double getElapsedTimeInMilliSec() 123 | { 124 | return this->getElapsedTimeInMicroSec() * 0.001; 125 | } 126 | // get elapsed time in micro-second 127 | double getElapsedTimeInMicroSec() 128 | { 129 | double startTimeInMicroSec = 0; 130 | double endTimeInMicroSec = 0; 131 | 132 | #ifdef WIN32 133 | if (!stopped) 134 | QueryPerformanceCounter(&endCount); 135 | 136 | startTimeInMicroSec = 137 | startCount.QuadPart * (1000000.0 / frequency.QuadPart); 138 | endTimeInMicroSec = 139 | endCount.QuadPart * (1000000.0 / frequency.QuadPart); 140 | #elif __APPLE__ 141 | if (!stopped) 142 | endCount = mach_absolute_time(); 143 | 144 | return subtractTimes(endCount, startCount) / 1e-6; 145 | #else 146 | if (!stopped) 147 | gettimeofday(&endCount, NULL); 148 | 149 | startTimeInMicroSec = 150 | (startCount.tv_sec * 1000000.0) + startCount.tv_usec; 151 | endTimeInMicroSec = 152 | (endCount.tv_sec * 1000000.0) + endCount.tv_usec; 153 | #endif 154 | 155 | return endTimeInMicroSec - startTimeInMicroSec; 156 | } 157 | 158 | private: 159 | // stop flag 160 | int stopped; 161 | #ifdef WIN32 162 | // ticks per second 163 | LARGE_INTEGER frequency; 164 | LARGE_INTEGER startCount; 165 | LARGE_INTEGER endCount; 166 | #elif __APPLE__ 167 | uint64_t startCount; 168 | uint64_t endCount; 169 | #else 170 | timeval startCount; 171 | timeval endCount; 172 | #endif 173 | }; 174 | 175 | class ScopedTimer { 176 | public: 177 | ScopedTimer() : m_total_time(nullptr) { start(); } 178 | 179 | ScopedTimer(double &total_time) : m_total_time(&total_time) { start(); } 180 | 181 | virtual ~ScopedTimer() { stop(); } 182 | 183 | inline void start() { m_timer.start(); } 184 | 185 | inline void stop() 186 | { 187 | m_timer.stop(); 188 | if (m_total_time) { 189 | *m_total_time += getElapsedTimeInMicroSec(); 190 | } 191 | } 192 | 193 | inline double getElapsedTimeInMicroSec() 194 | { 195 | return m_timer.getElapsedTimeInMicroSec(); 196 | } 197 | 198 | inline const Timer &timer() { return m_timer; } 199 | 200 | protected: 201 | std::string m_msg; 202 | Timer m_timer; 203 | double *m_total_time; 204 | }; 205 | 206 | } // namespace ticcd 207 | -------------------------------------------------------------------------------- /src/tight_inclusion/types.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace ticcd { 8 | #ifdef TIGHT_INCLUSION_WITH_DOUBLE_PRECISION 9 | typedef double Scalar; 10 | #else 11 | typedef float Scalar; 12 | #endif 13 | typedef Eigen::Matrix Vector3; 14 | typedef Eigen::Array Array3; 15 | } // namespace ticcd 16 | --------------------------------------------------------------------------------